Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 880 → Rev 881

/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
{