/trunk/gpx.c |
---|
509,7 → 509,22 |
if(NMEA_In_Fix_Txt[0] >= '1') |
{ |
sprintf(string, "<trkpt lat=\"%s\" lon=\"%s\">\r\n",NMEA_In_Lat_Txt,NMEA_In_Lon_Txt); |
// sprintf(string, "<trkpt lat=\"%s\" lon=\"%s\">\r\n",NMEA_In_Lat_Txt,NMEA_In_Lon_Txt); |
if(NMEA_Position.Status != INVALID) |
{ |
if(NMEA_Position.Latitude < 0) u8_1 = '-'; |
else u8_1 = '+'; |
i32_1 = abs(NMEA_Position.Latitude)/10000000L; |
i32_2 = abs(NMEA_Position.Latitude)%10000000L; |
sprintf(string, "<trkpt lat=\"%c%ld.%07ld0\" ",u8_1, i32_1, i32_2); |
CheckSumAndWrite(&Check16Block,string, doc->file); |
if(NMEA_Position.Longitude < 0) u8_1 = '-'; |
else u8_1 = '+'; |
i32_1 = abs(NMEA_Position.Longitude)/10000000L; |
i32_2 = abs(NMEA_Position.Longitude)%10000000L; |
sprintf(string, "lon=\"%c%ld.%07ld0\">\r\n",u8_1, i32_1, i32_2); |
} |
else sprintf(string, "<trkpt lat=\"\" lon=\"\">"); |
} |
else |
if(GPSData.Position.Status != INVALID) |
/trunk/main.c |
---|
97,6 → 97,7 |
u8 NCFlags = 0; |
s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg |
u8 ErrorGpsFixLost = 0; |
u8 ErrorNMEAFixLost = 0; |
u8 FromFC_LoadWP_List = 0, FromFC_Load_SinglePoint = 0, FromFC_Save_SinglePoint = 0; |
u8 ToFC_MaxWpListIndex = 4; |
u8 ClearFCStatusFlags = 0; |
361,6 → 362,12 |
sprintf(ErrorMSG,"GPS Fix lost "); |
newErrorCode = 21; |
} |
else if(ErrorNMEAFixLost) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"Ext.GPS Fix lost"); |
newErrorCode = 47; |
} |
else if(NC_To_FC_Flags & NC_TO_FC_FLYING_RANGE) |
{ |
LED_RED_ON; |
673,11 → 680,11 |
if(MenuBlinkBit) MenuBlinkBit = 0; else MenuBlinkBit = 1; |
if(NMEA_cnt) if(--NMEA_cnt == 0) |
{ |
TrigLogging.NMEA_Lat[0] = 0; |
TrigLogging.NMEA_Lon[0] = 0; |
if(NMEA_Position.Status != INVALID) ErrorNMEAFixLost = 1; |
TrigLogging.NMEA_Alt[0] = 0; |
NMEA_In_Fix_Txt[0] = 0; |
NMEA_cnt_gga = 0; |
NMEA_Position.Status = INVALID; |
} |
if(FC.StatusFlags & FC_STATUS_FLY) |
{ |
760,7 → 767,7 |
CntSpiErrorPerSecond = 0; |
} |
running = 0; |
// if(!PollingTimeout) DebugOut.Analog[17]++; |
// if(!PollingTimeout) DebugOut.Analog[]++; |
return(0); |
} |
/trunk/main.h |
---|
18,7 → 18,7 |
#define VERSION_MAJOR 2 |
#define VERSION_MINOR 21 |
#define VERSION_PATCH 1 |
#define VERSION_PATCH 2 |
// 0 = A |
// 1 = B |
// 2 = C |
41,7 → 41,7 |
#define CAN_SLAVE_COMPATIBLE 2 |
#ifndef FOLLOW_ME |
#define FC_SPI_COMPATIBLE 92 // <------------------ |
#define FC_SPI_COMPATIBLE 93 // <------------------ |
#else |
#define FC_SPI_COMPATIBLE 0xFF |
#endif |
364,7 → 364,7 |
extern s8 PartnerErrorMSG[25]; |
extern u8 ErrorCode,PartnerErrorCode; |
extern u8 StopNavigation; |
extern u8 ErrorGpsFixLost; |
extern u8 ErrorGpsFixLost,ErrorNMEAFixLost; |
extern u8 FromFC_LoadWP_List, ToFC_MaxWpListIndex, FromFC_Load_SinglePoint, FromFC_Save_SinglePoint; |
extern volatile u32 SPIWatchDog; // stop Navigation if this goes to zero |
extern volatile u32 SD_WatchDog; // stop Logging if this goes to zero |
/trunk/menu.c |
---|
851,10 → 851,15 |
if(!NMEA_cnt) LCD_printfxy(0,3,"No Data ") |
else |
{ |
LCD_printfxy(0,1,"Lon:%s ",NMEA_In_Lon_Txt); |
LCD_printfxy(0,2,"Lat:%s ",NMEA_In_Lat_Txt); |
if(NMEA_In_Fix_Txt[0] < '1') LCD_printfxy(1,3,"No Fix! %s",NMEA_In_Fix_Txt) |
else LCD_printfxy(0,3,"Fix:%s %s ",NMEA_In_Fix_Txt,NMEA_In_Alt_Txt); |
else |
{ |
LCD_printfxy(0,1,"Lon:%s ",NMEA_In_Lon_Txt); |
LCD_printfxy(0,2,"Lat: %s ",NMEA_In_Lat_Txt); |
LCD_printfxy(11,1,"'%s ",&NMEA_In_Lon_Txt[7]); |
LCD_printfxy(11,2,"'%s ",&NMEA_In_Lat_Txt[6]); |
LCD_printfxy(0,3,"Fix:%s %s ",NMEA_In_Fix_Txt,NMEA_In_Alt_Txt); |
} |
LCD_printfxy(11,0,"%3u",NMEA_cnt_gga); |
if(!NMEA_cnt_gga) LCD_printfxy(0,3,"No $GPGGA Data! "); |
/trunk/triggerlog.c |
---|
82,15 → 82,23 |
void TriggerInput(void) // triggerinput on IO1 |
{ |
TrigLogging.Longitude = GPSData.Position.Longitude; |
TrigLogging.Latitude = GPSData.Position.Latitude; |
if(NMEA_Position.Status != INVALID) |
{ |
TrigLogging.Longitude = NMEA_Position.Longitude; |
TrigLogging.Latitude = NMEA_Position.Latitude; |
} |
else |
{ |
TrigLogging.Longitude = GPSData.Position.Longitude; |
TrigLogging.Latitude = GPSData.Position.Latitude; |
} |
TrigLogging.Count++; |
if(BlitzSchuhConnected) TrigLogging.CountExternal++; |
else TrigLogging.CountExternal = 0; |
TrigLogging.AltiBaro = NaviData.Altimeter_5cm * 5; // in cm |
TrigLogging.AltiGPS = GPSData.Position.Altitude; |
TrigLogging.ShutterCounter = NaviData_Volatile.ShutterCounter; |
TrigLogging.AltiGPS = GPSData.Position.Altitude; |
TrigLogging.Hour = SystemTime.Hour; |
TrigLogging.Min = SystemTime.Min; |
TrigLogging.Sec = SystemTime.Sec; |
100,20 → 108,17 |
if(SimulationFlags) TrigLogging.Yaw = CompassSetpointCorrected; |
else TrigLogging.Yaw = GyroCompassCorrected; |
TrigLogging.NewData++; |
if(NMEA_cnt) |
{ |
u8 i; |
for(i=0; i<16;i++) |
for(i=0; i<10;i++) |
{ |
TrigLogging.NMEA_Lon[i] = NMEA_In_Lon_Txt[i]; |
TrigLogging.NMEA_Lat[i] = NMEA_In_Lat_Txt[i]; |
if(i<10) TrigLogging.NMEA_Alt[i] = NMEA_In_Alt_Txt[i]; |
TrigLogging.NMEA_Alt[i] = NMEA_In_Alt_Txt[i]; |
} |
} |
else |
{ |
TrigLogging.NMEA_Lon[0] = 0; |
TrigLogging.NMEA_Lat[0] = 0; |
TrigLogging.NMEA_Alt[0] = 0; |
} |
} |
374,13 → 379,6 |
sprintf(string, ";%04d-%02d-%02dT%02d:%02d:%02d.%dZ",SystemTime.Year, SystemTime.Month, SystemTime.Day, SystemTime.Hour, SystemTime.Min, SystemTime.Sec,SystemTime.mSec/100); |
fputs_(string, doc->file); |
// Position |
if(NMEA_In_Fix_Txt[0] >= '1') |
{ |
sprintf(string,";%s;%s",TrigLogging.NMEA_Lat,TrigLogging.NMEA_Lon); |
fputs_(string, doc->file); |
} |
else |
{ |
if(TrigLogging.Latitude < 0) sign = '-'; |
else sign = '+'; |
i1 = abs(TrigLogging.Latitude)/10000000L; |
394,7 → 392,6 |
i2 = abs(TrigLogging.Longitude)%10000000L; |
sprintf(string,";%c%ld.%07ld",sign, i1, i2); |
fputs_(string, doc->file); |
} |
// calculate relative altitude with respect to the altitude of the home position |
// Altitude GPS |
/trunk/triggerlog.h |
---|
43,8 → 43,6 |
u16 ServoControlNick; |
u16 ServoControlPoi; |
u8 NewData; |
u8 NMEA_Lon[16]; |
u8 NMEA_Lat[16]; |
u8 NMEA_Alt[10]; |
} TrigLog_t; |
extern TrigLog_t TrigLogging; |
/trunk/uart2.c |
---|
58,6 → 58,8 |
#include "config.h" |
#include "uart1.h" |
#include "mkprotocol.h" |
#include "gpspos.h" |
#include "main.h" |
// the tx buffer |
#define UART2_TX_BUFFER_LEN 150 |
70,6 → 72,7 |
unsigned char NMEA_In_Fix_Txt[10]; |
u16 NMEA_cnt,NMEA_cnt_gga; |
GPS_Pos_t NMEA_Position = {0,0,0, INVALID}; |
/********************************************************/ |
/* Initialize UART2 */ |
177,11 → 180,108 |
} |
u8 CheckAscii(u8 c) |
{ |
if((c >= '0' && c <= '9') || c == '.') return(1); |
return(0); |
} |
u8 CheckNmea(void) |
{ |
u8 i; |
if(NMEA_In_Fix_Txt[0] <= '0') return(0); |
if(NMEA_In_Lat_Txt[0] == 0) return(0); |
if(NMEA_In_Lon_Txt[0] == 0) return(0); |
if(NMEA_In_Lat_Txt[3] != '.') return(0); |
if(NMEA_In_Lon_Txt[4] != '.') return(0); |
for(i = 1; i < 13; i++) |
{ |
if(!CheckAscii(NMEA_In_Lat_Txt[i])) return(0); |
if(!CheckAscii(NMEA_In_Lon_Txt[i])) return(0); |
} |
return(1); |
} |
u8 AsciiToNum(u8 c) |
{ |
if(c == '.') return(0); |
return(c - '0'); |
} |
void ConvertNmeaToPos(GPS_Pos_t *pos) |
{ |
s32 tmp,tmp2,i; |
if(!CheckNmea()) |
{ |
if(NMEA_Position.Status != INVALID) ErrorNMEAFixLost = 1; |
pos->Status = INVALID; |
return; |
} |
tmp = AsciiToNum(NMEA_In_Lat_Txt[1]) * 100000000L; |
tmp += AsciiToNum(NMEA_In_Lat_Txt[2]) * 10000000L; |
// '.' |
tmp2 = AsciiToNum(NMEA_In_Lat_Txt[4]) * 10000000L; |
tmp2 += AsciiToNum(NMEA_In_Lat_Txt[5]) * 1000000L; |
tmp2 += AsciiToNum(NMEA_In_Lat_Txt[6]) * 100000L; |
tmp2 += AsciiToNum(NMEA_In_Lat_Txt[7]) * 10000L; |
tmp2 += AsciiToNum(NMEA_In_Lat_Txt[8]) * 1000L; |
tmp2 += AsciiToNum(NMEA_In_Lat_Txt[9]) * 100L; |
tmp2 += AsciiToNum(NMEA_In_Lat_Txt[10]) * 10L; |
tmp2 += AsciiToNum(NMEA_In_Lat_Txt[11]) * 1L; |
tmp = tmp + tmp2/6; // to convert from Minutes to Decimal degree |
if(NMEA_In_Lat_Txt[0] == '-') pos->Latitude = -tmp; else pos->Latitude = tmp; |
tmp = AsciiToNum(NMEA_In_Lon_Txt[1]) * 1000000000L; |
tmp += AsciiToNum(NMEA_In_Lon_Txt[2]) * 100000000L; |
tmp += AsciiToNum(NMEA_In_Lon_Txt[3]) * 10000000L; |
// '.' |
tmp2 = AsciiToNum(NMEA_In_Lon_Txt[5]) * 10000000L; |
tmp2 += AsciiToNum(NMEA_In_Lon_Txt[6]) * 1000000L; |
tmp2 += AsciiToNum(NMEA_In_Lon_Txt[7]) * 100000L; |
tmp2 += AsciiToNum(NMEA_In_Lon_Txt[8]) * 10000L; |
tmp2 += AsciiToNum(NMEA_In_Lon_Txt[9]) * 1000L; |
tmp2 += AsciiToNum(NMEA_In_Lon_Txt[10]) * 100L; |
tmp2 += AsciiToNum(NMEA_In_Lon_Txt[11]) * 10L; |
tmp2 += AsciiToNum(NMEA_In_Lon_Txt[12]) * 1L; |
tmp = tmp + tmp2/6; // to convert from Minutes to Decimal degree |
if(NMEA_In_Lon_Txt[0] == '-') pos->Longitude = -tmp; else pos->Longitude = tmp; |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// + Alitutude |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
i = 0; |
tmp2 = 1000; // Millimeter |
tmp = 0; |
if(!CheckAscii(NMEA_In_Alt_Txt[0])) i++; // '-' |
while(CheckAscii(NMEA_In_Alt_Txt[i]) && NMEA_In_Alt_Txt[i] != '.') |
{ |
tmp += tmp2 * AsciiToNum(NMEA_In_Alt_Txt[i++]); |
tmp2 *= 10; |
} |
if(NMEA_In_Alt_Txt[i++] == '.') // Zentimeter |
{ |
if(CheckAscii(NMEA_In_Alt_Txt[i])) |
{ |
tmp +=100 * AsciiToNum(NMEA_In_Alt_Txt[i++]); |
if(CheckAscii(NMEA_In_Alt_Txt[i])) |
{ |
tmp += 10 * AsciiToNum(NMEA_In_Alt_Txt[i++]); |
if(CheckAscii(NMEA_In_Alt_Txt[i])) tmp += 1 * AsciiToNum(NMEA_In_Alt_Txt[i++]); |
} |
} |
} |
if(NMEA_In_Alt_Txt[0] == '-') tmp = -tmp; |
pos->Altitude = tmp; |
pos->Status = NEWDATA; |
} |
void NEMA_Parser(unsigned char data) |
{ |
static u16 state = 0, fx,la,lo,i,a; |
if(data == '$') state = 0; |
static u32 latitude; |
static unsigned char lat_txt[16], lon_txt[16],fix_txt[10],alt_txt[10]; |
//$GPGGA,HHMMSS.ss,BBBB.BBBB,b,LLLLL.LLLL,l,Q,NN,D.D,H.H,h,G.G,g,A.A,RRRR*PP |
374,11 → 474,14 |
NMEA_In_Lon_Txt[i] = lon_txt[i]; |
} |
} |
if(NMEA_In_Fix_Txt[i] > '0' && fix_txt[i] == '0') ErrorNMEAFixLost = 1; |
for(i=0; i<10; i++) |
{ |
NMEA_In_Alt_Txt[i] = alt_txt[i]; |
NMEA_In_Fix_Txt[i] = fix_txt[i]; |
} |
if(NMEA_In_Fix_Txt[0] > '0') ErrorNMEAFixLost = 0; |
ConvertNmeaToPos(&NMEA_Position); |
break; |
default: state = 0; |
} |
396,8 → 499,8 |
UART_ClearITPendingBit(UART2, UART_IT_Receive); // clear receive interrupt flag |
UART_ClearITPendingBit(UART2, UART_IT_ReceiveTimeOut); // clear receive timeout interrupt flag |
// if debug UART is UART2 |
if (DebugUART == UART2) |
// if debug UART is UART2 -> old FC connects to FC through the Uart |
if((DebugUART == UART2) && (UART_VersionInfo.HWMajor < 30)) |
{ // forward received data to the UART1 tx buffer |
while(UART_GetFlagStatus(UART2, UART_FLAG_RxFIFOEmpty) != SET) |
{ |
404,8 → 507,7 |
// wait for space in the tx buffer of the UART1 |
while(UART_GetFlagStatus(UART1, UART_FLAG_TxFIFOFull) == SET) {}; |
// move the byte from the rx buffer of UART2 to the tx buffer of UART1 |
if(UART_VersionInfo.HWMajor < 30) UART_SendData(UART1, UART_ReceiveData(UART2)); |
else UART_ReceiveData(UART2); |
UART_SendData(UART1, UART_ReceiveData(UART2)); |
} |
} |
else |
/trunk/uart2.h |
---|
9,6 → 9,11 |
void UART2_Deinit (void); |
void UART2_TransmitTxData(void); |
void UART2_ProcessRxData(void); |
u8 CheckNmea(void); |
void ConvertNmeaToPos(GPS_Pos_t *pos); |
u8 CheckAscii(u8 c); |
u8 CheckNmea(void); |
u8 AsciiToNum(u8 c); |
extern unsigned char NMEA_In_Lat_Txt[16]; |
extern unsigned char NMEA_In_Lon_Txt[16]; |
15,4 → 20,7 |
extern unsigned char NMEA_In_Fix_Txt[10]; |
extern unsigned char NMEA_In_Alt_Txt[10]; |
extern u16 NMEA_cnt,NMEA_cnt_gga; |
extern GPS_Pos_t NMEA_Position; |
#endif //__UART2_H |
/trunk/ubx.c |
---|
57,6 → 57,7 |
#include "91x_lib.h" |
#include "uart0.h" |
#include "uart1.h" |
#include "uart2.h" |
#include "ubx.h" |
#include "main.h" |
#include "led.h" |
325,9 → 326,24 |
GPSData.Position.Status = INVALID; |
if(!(SimulationFlags & SIMULATION_ACTIVE)) |
{ |
if((Parameter.User3 > 100) && (NMEA_Position.Status != INVALID)) |
{ |
GPSData.Position.Longitude = NMEA_Position.Longitude; |
GPSData.Position.Latitude = NMEA_Position.Latitude; |
GPSData.Position.Altitude = NMEA_Position.Altitude; |
if(CheckAscii(NMEA_In_Fix_Txt[3])) |
GPSData.NumOfSats = 10 * AsciiToNum(NMEA_In_Fix_Txt[2]) + AsciiToNum(NMEA_In_Fix_Txt[3]); |
else |
if(CheckAscii(NMEA_In_Fix_Txt[2])) GPSData.NumOfSats = AsciiToNum(NMEA_In_Fix_Txt[2]); // nur eine Ziffer? (weiss nicht, ob das vorkommt..) |
else GPSData.NumOfSats = 0; |
} |
else |
{ |
GPSData.Position.Longitude = UbxPosLlh.LON; // in steps of 1E-7 deg |
GPSData.Position.Latitude = UbxPosLlh.LAT; // in steps of 1E-7 deg |
GPSData.Position.Altitude = UbxPosLlh.HMSL; // in steps of 1 mm |
} |
} |
else // simulation active |
{ |