// we need to parse GLL as well because it can mark the start of a new quantum data
// followed by values with no data, ex. altitude, vario, etc.
BOOL NMEAParser::GLL(TCHAR *String, TCHAR **params, size_t nparams, NMEA_INFO *GPS_INFO)
{
gpsValid = !NAVWarn(params[5][0]);
if (!activeGPS) return TRUE;
if (ReplayLogger::IsEnabled()) {
// block actual GPS signal
InterfaceTimeoutReset();
return TRUE;
}
GPS_INFO->NAVWarning = !gpsValid;
// use valid time with invalid fix
double glltime = StrToDouble(params[4],NULL);
if (glltime>0) {
#ifdef NEWTRIGGERGPS
double ThisTime = TimeConvert(glltime, GPS_INFO); // 091208
#else
double ThisTime = TimeModify(glltime, GPS_INFO);
#endif
#ifndef NEWTRIGGERGPS
if (!TimeHasAdvanced(ThisTime, GPS_INFO)) return FALSE; // 091208
#endif
#ifdef NEWTRIGGERGPS
// is time advanced to a new quantum?
if (ThisTime >NmeaTime) {
// yes so lets trigger the gps event
TriggerGPSUpdate();
Sleep(50); // 091208
NmeaTime=ThisTime;
TimeSet(GPS_INFO); // 091208
TimeHasAdvanced(ThisTime,GPS_INFO); // 091208
//StartupStore(_T(".............. trigger from GLL\n"));
}
#endif
}
if (!gpsValid) return FALSE; // 091108 addon BUGFIX GLL time with no valid signal
double tmplat;
double tmplon;
tmplat = MixedFormatToDegrees(StrToDouble(params[0], NULL));
tmplat = NorthOrSouth(tmplat, params[1][0]);
tmplon = MixedFormatToDegrees(StrToDouble(params[2], NULL));
tmplon = EastOrWest(tmplon,params[3][0]);
if (!((tmplat == 0.0) && (tmplon == 0.0))) {
GPS_INFO->Latitude = tmplat;
GPS_INFO->Longitude = tmplon;
} else {
}
return TRUE;
}
// we need to parse GLL as well because it can mark the start of a new quantum data
// followed by values with no data, ex. altitude, vario, etc.
BOOL NMEAParser::GLL(TCHAR *String, TCHAR **params, size_t nparams, NMEA_INFO *pGPS)
{
gpsValid = !NAVWarn(params[5][0]);
GPSCONNECT=TRUE;
if (!activeGPS) return TRUE;
pGPS->NAVWarning = !gpsValid;
// use valid time with invalid fix
GLLtime = StrToDouble(params[4],NULL);
if (!RMCAvailable && !GGAAvailable && (GLLtime>0)) {
#ifndef OLD_TIME_MODIFY
double ThisTime = TimeModify(params[4], pGPS, StartDay);
#else
double ThisTime = TimeModify(GLLtime, pGPS);
#endif
if (!TimeHasAdvanced(ThisTime, pGPS)) return FALSE;
}
if (!gpsValid) return FALSE;
double tmplat;
double tmplon;
tmplat = MixedFormatToDegrees(StrToDouble(params[0], NULL));
tmplat = NorthOrSouth(tmplat, params[1][0]);
tmplon = MixedFormatToDegrees(StrToDouble(params[2], NULL));
tmplon = EastOrWest(tmplon,params[3][0]);
if (!((tmplat == 0.0) && (tmplon == 0.0))) {
pGPS->Latitude = tmplat;
pGPS->Longitude = tmplon;
} else {
}
return TRUE;
} // END GLL
static BOOL FLYSEN(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS)
{
TCHAR ctemp[80];
double vtas;
static int offset=-1;
d->nmeaParser.connected = true;
// firmware 3.31h no offset
// firmware 3.32 1 offset
// Determine firmware version, assuming it will not change in the session!
if (offset<0) {
NMEAParser::ExtractParameter(String,ctemp,8);
if ( (_tcscmp(ctemp,_T("A"))==0) || (_tcscmp(ctemp,_T("V"))==0))
offset=0;
else {
NMEAParser::ExtractParameter(String,ctemp,9);
if ( (_tcscmp(ctemp,_T("A"))==0) || (_tcscmp(ctemp,_T("V"))==0))
offset=1;
else
return TRUE;
}
}
// VOID GPS SIGNAL
NMEAParser::ExtractParameter(String,ctemp,8+offset);
if (_tcscmp(ctemp,_T("V"))==0) {
pGPS->NAVWarning=true;
// GPSCONNECT=false; // 121127 NO!!
goto label_nogps;
}
// ------------------------
double tmplat;
double tmplon;
NMEAParser::ExtractParameter(String,ctemp,1+offset);
tmplat = MixedFormatToDegrees(StrToDouble(ctemp, NULL));
NMEAParser::ExtractParameter(String,ctemp,2+offset);
tmplat = NorthOrSouth(tmplat, ctemp[0]);
NMEAParser::ExtractParameter(String,ctemp,3+offset);
tmplon = MixedFormatToDegrees(StrToDouble(ctemp, NULL));
NMEAParser::ExtractParameter(String,ctemp,4+offset);
tmplon = EastOrWest(tmplon,ctemp[0]);
if (!((tmplat == 0.0) && (tmplon == 0.0))) {
pGPS->Latitude = tmplat;
pGPS->Longitude = tmplon;
pGPS->NAVWarning=false;
}
// GPS SPEED
NMEAParser::ExtractParameter(String,ctemp,6+offset);
pGPS->Speed = StrToDouble(ctemp,NULL)/10;
// TRACK BEARING
if (pGPS->Speed>1.0) {
NMEAParser::ExtractParameter(String,ctemp,5+offset);
pGPS->TrackBearing = AngleLimit360(StrToDouble(ctemp, NULL));
}
// HGPS
NMEAParser::ExtractParameter(String,ctemp,7+offset);
pGPS->Altitude = StrToDouble(ctemp,NULL);
// ------------------------
label_nogps:
// SATS
NMEAParser::ExtractParameter(String,ctemp,9+offset);
pGPS->SatellitesUsed = (int) StrToDouble(ctemp,NULL);
// DATE
// Firmware 3.32 has got the date
if (offset>0) {
NMEAParser::ExtractParameter(String,ctemp,0);
long gy, gm, gd;
TCHAR *Stop;
gy = _tcstol(&ctemp[4], &Stop, 10) + 2000;
ctemp[4] = '\0';
gm = _tcstol(&ctemp[2], &Stop, 10);
ctemp[2] = '\0';
gd = _tcstol(&ctemp[0], &Stop, 10);
if ( ((gy > 1980) && (gy <2100) ) && (gm != 0) && (gd != 0) ) {
pGPS->Year = gy;
pGPS->Month = gm;
pGPS->Day = gd;
}
}
// TIME
// ignoring 00:00.00
// We need to manage UTC time
#ifndef OLD_TIME_MODIFY
static int StartDay=-1;
if (pGPS->SatellitesUsed>0) {
NMEAParser::ExtractParameter(String,ctemp,0+offset);
pGPS->Time = TimeModify(ctemp, pGPS, StartDay);
}
// TODO : check if TimeHasAdvanced check is needed (cf. Parser.cpp)
#else
NMEAParser::ExtractParameter(String,ctemp,0+offset);
double fixTime = StrToDouble(ctemp,NULL);
static int day_difference=0, previous_months_day_difference=0;
static int startday=-1;
if (fixTime>0 && pGPS->SatellitesUsed>0) {
double hours, mins,secs;
hours = fixTime / 10000;
pGPS->Hour = (int)hours;
mins = fixTime / 100;
mins = mins - (pGPS->Hour*100);
pGPS->Minute = (int)mins;
secs = fixTime - (pGPS->Hour*10000) - (pGPS->Minute*100);
pGPS->Second = (int)secs;
fixTime = secs + (pGPS->Minute*60) + (pGPS->Hour*3600);
if ((startday== -1) && (pGPS->Day != 0)) {
if (offset)
StartupStore(_T(". FLYSEN First GPS DATE: %d-%d-%d%s"), pGPS->Year, pGPS->Month, pGPS->Day,NEWLINE);
else
StartupStore(_T(". FLYSEN No Date, using PNA GPS DATE: %d-%d-%d%s"), pGPS->Year, pGPS->Month, pGPS->Day,NEWLINE);
startday = pGPS->Day;
day_difference=0;
previous_months_day_difference=0;
}
if (startday != -1) {
if (pGPS->Day < startday) {
// detect change of month (e.g. day=1, startday=26)
previous_months_day_difference=day_difference+1;
day_difference=0;
startday = pGPS->Day;
StartupStore(_T(". FLYSEN Change GPS DATE to NEW MONTH: %d-%d-%d (%d days running)%s"),
pGPS->Year, pGPS->Month, pGPS->Day,previous_months_day_difference,NEWLINE);
}
if ( (pGPS->Day-startday)!=day_difference) {
StartupStore(_T(". FLYSEN Change GPS DATE: %d-%d-%d%s"), pGPS->Year, pGPS->Month, pGPS->Day,NEWLINE);
}
day_difference = pGPS->Day-startday;
if ((day_difference+previous_months_day_difference)>0) {
fixTime += (day_difference+previous_months_day_difference) * 86400;
}
}
pGPS->Time = fixTime;
}
#endif
// HPA from the pressure sensor
// NMEAParser::ExtractParameter(String,ctemp,10+offset);
// double ps = StrToDouble(ctemp,NULL)/100;
// pGPS->BaroAltitude = (1 - pow(fabs(ps / QNH), 0.190284)) * 44307.69;
// HBAR 1013.25
NMEAParser::ExtractParameter(String,ctemp,11+offset);
double palt=StrToDouble(ctemp,NULL);
UpdateBaroSource( pGPS, 0,d, QNEAltitudeToQNHAltitude(palt));
// VARIO
NMEAParser::ExtractParameter(String,ctemp,12+offset);
pGPS->Vario = StrToDouble(ctemp,NULL)/100;
// TAS
NMEAParser::ExtractParameter(String,ctemp,13+offset);
vtas=StrToDouble(ctemp,NULL)/10;
pGPS->IndicatedAirspeed = vtas/AirDensityRatio(palt);
pGPS->TrueAirspeed = vtas;
if (pGPS->IndicatedAirspeed >0)
pGPS->AirspeedAvailable = TRUE;
else
pGPS->AirspeedAvailable = FALSE;
// ignore n.14 airspeed source
// OAT
NMEAParser::ExtractParameter(String,ctemp,15+offset);
pGPS->OutsideAirTemperature = StrToDouble(ctemp,NULL);
pGPS->TemperatureAvailable=TRUE;
// ignore n.16 baloon temperature
// BATTERY PERCENTAGES
NMEAParser::ExtractParameter(String,ctemp,17+offset);
pGPS->ExtBatt1_Voltage = StrToDouble(ctemp,NULL)+1000;
NMEAParser::ExtractParameter(String,ctemp,18+offset);
pGPS->ExtBatt2_Voltage = StrToDouble(ctemp,NULL)+1000;
pGPS->VarioAvailable = TRUE;
// currently unused in LK, but ready for next future
TriggerVarioUpdate();
TriggerGPSUpdate();
return TRUE;
}
BOOL NMEAParser::GGA(TCHAR *String, TCHAR **params, size_t nparams, NMEA_INFO *pGPS)
{
GGAAvailable = TRUE;
GPSCONNECT = TRUE; // 091208
// this will force gps invalid but will NOT assume gps valid!
nSatellites = (int)(min(16.0, StrToDouble(params[6], NULL)));
if (nSatellites==0) {
gpsValid = false;
}
double ggafix = StrToDouble(params[5],NULL);
if ( ggafix==0 || ggafix>5 ) {
#ifdef DEBUG_GPS
if (ggafix>5) StartupStore(_T("------ GGA DEAD RECKON fix skipped%s"),NEWLINE);
#endif
gpsValid=false;
} else {
gpsValid=true;
}
if (!activeGPS) return TRUE;
pGPS->SatellitesUsed = nSatellites; // 091208
pGPS->NAVWarning = !gpsValid; // 091208
GGAtime=StrToDouble(params[0],NULL);
// Even with invalid fix, we might still have valid time
// I assume that 0 is invalid, and I am very sorry for UTC time 00:00 ( missing a second a midnight).
// is better than risking using 0 as valid, since many gps do not respect any real nmea standard
//
// Update 121215: do not update time with GGA if RMC is found, because at 00UTC only RMC will set the date change!
// Remember that we trigger update of calculations when we get GGA.
// So what happens if the gps sequence is GGA and then RMC?
// 2359UTC:
// GGA , old date, trigger gps calc
// RMC, old date
// 0000UTC:
// GGA, old date even if new date will come for the same quantum,
// GGAtime>0, see (*)
// BANG! oldtime from RMC is 2359, new time from GGA is 0, time is in the past!
//
// If the gps is sending first RMC, this problem does not appear of course.
//
// (*) IMPORTANT> GGAtime at 00UTC will most likely be >0! Because time is in hhmmss.ss .ss is always >0!!
// We check GGAtime, RMCtime, GLLtime etc. for 0 because in case of error the gps will send 000000.000 !!
//
if ( (!RMCAvailable && (GGAtime>0)) || ((GGAtime>0) && (GGAtime == RMCtime)) ) { // RMC already came in same time slot
#if DEBUGSEQ
StartupStore(_T("... GGA update time = %f RMCtime=%f\n"),GGAtime,RMCtime); // 31C
#endif
double ThisTime = TimeModify(GGAtime, pGPS);
if (!TimeHasAdvanced(ThisTime, pGPS)) {
#if DEBUGSEQ
StartupStore(_T(".... GGA time not advanced, skip\n")); // 31C
#endif
return FALSE;
}
}
if (gpsValid) {
double tmplat;
double tmplon;
tmplat = MixedFormatToDegrees(StrToDouble(params[1], NULL));
tmplat = NorthOrSouth(tmplat, params[2][0]);
tmplon = MixedFormatToDegrees(StrToDouble(params[3], NULL));
tmplon = EastOrWest(tmplon,params[4][0]);
if (!((tmplat == 0.0) && (tmplon == 0.0))) {
pGPS->Latitude = tmplat;
pGPS->Longitude = tmplon;
}
else {
#ifdef DEBUG_GPS
StartupStore(_T("++++++ GGA gpsValid with invalid posfix!%s"),NEWLINE);
#endif
gpsValid=false;
}
}
// GGA is marking now triggering end of data, so OK to use baro
// Even with invalid fix we might have valid baro data of course
// any NMEA sentence with time can now trigger gps update: the first to detect new time will make trigger.
// we assume also that any sentence with no time belongs to current time.
// note that if no time from gps, no use of vario and baro data, but also no fix available.. so no problems
if(RMZAvailable)
{
UpdateBaroSource(pGPS, isFlarm? BARO__RMZ_FLARM:BARO__RMZ, NULL, RMZAltitude);
}
else if(RMAAvailable) {
UpdateBaroSource(pGPS, BARO__RMA,NULL, RMAAltitude);
}
// If no gps fix, at this point we trigger refresh and quit
if (!gpsValid) {
#if DEBUGSEQ
StartupStore(_T("........ GGA no gps valid, triggerGPS!\n")); // 31C
#endif
TriggerGPSUpdate();
return FALSE;
}
// "Altitude" should always be GPS Altitude.
pGPS->Altitude = ParseAltitude(params[8], params[9]);
pGPS->Altitude += (GPSAltitudeOffset/1000); // BUGFIX 100429
double GeoidSeparation;
if (_tcslen(params[10])>0) {
// No real need to parse this value,
// but we do assume that no correction is required in this case
GeoidSeparation = ParseAltitude(params[10], params[11]);
} else {
if (UseGeoidSeparation) {
GeoidSeparation = LookupGeoidSeparation(pGPS->Latitude, pGPS->Longitude);
pGPS->Altitude -= GeoidSeparation;
}
}
// if RMC would be Triggering update, we loose the relative altitude, which is coming AFTER rmc!
// This was causing old altitude recorded in new pos fix.
// 120428:
// GGA will trigger gps if there is no RMC,
// or if GGAtime is the same as RMCtime, which means that RMC already came and we are last in the sequence
if ( !RMCAvailable || (GGAtime == RMCtime) ) {
#if DEBUGSEQ
StartupStore(_T("... GGA trigger gps, GGAtime==RMCtime\n")); // 31C
#endif
TriggerGPSUpdate();
}
return TRUE;
} // END GGA
BOOL NMEAParser::RMC(TCHAR *String, TCHAR **params, size_t nparams, NMEA_INFO *pGPS)
{
TCHAR *Stop;
static bool logbaddate=true;
double speed=0;
gpsValid = !NAVWarn(params[1][0]);
GPSCONNECT = TRUE;
RMCAvailable=true; // 100409
#ifdef PNA
if (DeviceIsGM130) {
double ps = GM130BarPressure();
RMZAltitude = (1 - pow(fabs(ps / QNH), 0.190284)) * 44307.69;
// StartupStore(_T("....... Pressure=%.0f QNH=%.2f Altitude=%.1f\n"),ps,QNH,RMZAltitude);
RMZAvailable = TRUE;
UpdateBaroSource(pGPS, BARO__GM130, NULL, RMZAltitude);
}
if (DeviceIsRoyaltek3200) {
if (Royaltek3200_ReadBarData()) {
double ps = Royaltek3200_GetPressure();
RMZAltitude = (1 - pow(fabs(ps / QNH), 0.190284)) * 44307.69;
#if 0
pGPS->TemperatureAvailable=true;
pGPS->OutsideAirTemperature = Royaltek3200_GetTemperature();
#endif
}
RMZAvailable = TRUE;
UpdateBaroSource(pGPS, BARO__ROYALTEK3200, NULL, RMZAltitude);
}
#endif // PNA
if (!activeGPS) return TRUE;
// if no valid fix, we dont get speed either!
if (gpsValid)
{
// speed is in knots, 2 = 3.7kmh
speed = StrToDouble(params[6], NULL);
}
pGPS->NAVWarning = !gpsValid;
// say we are updated every time we get this,
// so infoboxes get refreshed if GPS connected
// the RMC sentence marks the start of a new fix, so we force the old data to be saved for calculations
// Even with no valid position, we let RMC set the time and date if valid
long gy, gm, gd;
gy = _tcstol(¶ms[8][4], &Stop, 10) + 2000;
params[8][4] = '\0';
gm = _tcstol(¶ms[8][2], &Stop, 10);
params[8][2] = '\0';
gd = _tcstol(¶ms[8][0], &Stop, 10);
// SeeYou PC is sending NMEA sentences with RMC date 2072-02-27
if ( ((gy > 1980) && (gy <2100) ) && (gm != 0) && (gd != 0) ) {
pGPS->Year = gy;
pGPS->Month = gm;
pGPS->Day = gd;
force_advance:
RMCtime = StrToDouble(params[0],NULL);
double ThisTime = TimeModify(RMCtime, pGPS);
// RMC time has priority on GGA and GLL etc. so if we have it we use it at once
if (!TimeHasAdvanced(ThisTime, pGPS)) {
#if DEBUGSEQ
StartupStore(_T("..... RMC time not advanced, skipping \n")); // 31C
#endif
return FALSE;
}
} else {
if (devIsCondor(devA())) {
#if DEBUGSEQ
StartupStore(_T(".. Condor not sending valid date, using 1.1.2012%s"),NEWLINE);
#endif
gy=2012; gm=1; gd=1;
goto force_advance;
}
if (gpsValid && logbaddate) { // 091115
StartupStore(_T("------ NMEAParser:RMC Receiving an invalid or null DATE from GPS%s"),NEWLINE);
StartupStore(_T("------ NMEAParser: Date received is y=%d m=%d d=%d%s"),gy,gm,gd,NEWLINE); // 100422
StartupStore(_T("------ This message will NOT be repeated.%s"),NEWLINE);
DoStatusMessage(MsgToken(875));
logbaddate=false;
}
gy=2012; gm=2; gd=30; // an impossible date!
goto force_advance;
}
if (gpsValid) {
double tmplat;
double tmplon;
tmplat = MixedFormatToDegrees(StrToDouble(params[2], NULL));
tmplat = NorthOrSouth(tmplat, params[3][0]);
tmplon = MixedFormatToDegrees(StrToDouble(params[4], NULL));
tmplon = EastOrWest(tmplon,params[5][0]);
if (!((tmplat == 0.0) && (tmplon == 0.0))) {
pGPS->Latitude = tmplat;
pGPS->Longitude = tmplon;
}
pGPS->Speed = KNOTSTOMETRESSECONDS * speed;
if (pGPS->Speed>trackbearingminspeed) {
pGPS->TrackBearing = AngleLimit360(StrToDouble(params[7], NULL));
}
} // gpsvalid 091108
// As soon as we get a fix for the first time, set the
// system clock to the GPS time.
static bool sysTimeInitialised = false;
if (!pGPS->NAVWarning && (gpsValid)) {
if (SetSystemTimeFromGPS) {
if (!sysTimeInitialised) {
if ( ( pGPS->Year > 1980 && pGPS->Year<2100) && ( pGPS->Month > 0) && ( pGPS->Hour > 0)) {
sysTimeInitialised =true; // Attempting only once
SYSTEMTIME sysTime;
// ::GetSystemTime(&sysTime);
int hours = (int)pGPS->Hour;
int mins = (int)pGPS->Minute;
int secs = (int)pGPS->Second;
sysTime.wYear = (unsigned short)pGPS->Year;
sysTime.wMonth = (unsigned short)pGPS->Month;
sysTime.wDay = (unsigned short)pGPS->Day;
sysTime.wHour = (unsigned short)hours;
sysTime.wMinute = (unsigned short)mins;
sysTime.wSecond = (unsigned short)secs;
sysTime.wMilliseconds = 0;
::SetSystemTime(&sysTime);
}
}
}
}
if(RMZAvailable) {
UpdateBaroSource(pGPS, BARO__RMZ, NULL, RMZAltitude);
}
else if(RMAAvailable) {
UpdateBaroSource(pGPS, BARO__RMA, NULL, RMAAltitude);
}
if (!GGAAvailable) {
// update SatInUse, some GPS receiver dont emmit GGA sentance
if (!gpsValid) {
pGPS->SatellitesUsed = 0;
} else {
pGPS->SatellitesUsed = -1;
}
}
if ( !GGAAvailable || (GGAtime == RMCtime) ) {
#if DEBUGSEQ
StartupStore(_T("... RMC trigger gps, GGAtime==RMCtime\n")); // 31C
#endif
TriggerGPSUpdate();
}
return TRUE;
} // END RMC
static BOOL FLYSEN(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *GPS_INFO)
{
TCHAR ctemp[80];
double vtas;
static int offset=-1;
// firmware 3.31h no offset
// firmware 3.32 1 offset
// Determine firmware version, assuming it will not change in the session!
if (offset<0) {
NMEAParser::ExtractParameter(String,ctemp,8);
if ( (_tcscmp(ctemp,_T("A"))==0) || (_tcscmp(ctemp,_T("V"))==0))
offset=0;
else {
NMEAParser::ExtractParameter(String,ctemp,9);
if ( (_tcscmp(ctemp,_T("A"))==0) || (_tcscmp(ctemp,_T("V"))==0))
offset=1;
else
return TRUE;
}
}
// VOID GPS SIGNAL
NMEAParser::ExtractParameter(String,ctemp,8+offset);
if (_tcscmp(ctemp,_T("V"))==0) {
GPS_INFO->NAVWarning=true;
GPSCONNECT=false;
goto label_nogps;
}
// ------------------------
double tmplat;
double tmplon;
NMEAParser::ExtractParameter(String,ctemp,1+offset);
tmplat = MixedFormatToDegrees(StrToDouble(ctemp, NULL));
NMEAParser::ExtractParameter(String,ctemp,2+offset);
tmplat = NorthOrSouth(tmplat, ctemp[0]);
NMEAParser::ExtractParameter(String,ctemp,3+offset);
tmplon = MixedFormatToDegrees(StrToDouble(ctemp, NULL));
NMEAParser::ExtractParameter(String,ctemp,4+offset);
tmplon = EastOrWest(tmplon,ctemp[0]);
if (!((tmplat == 0.0) && (tmplon == 0.0))) {
GPS_INFO->Latitude = tmplat;
GPS_INFO->Longitude = tmplon;
GPS_INFO->NAVWarning=false;
GPSCONNECT=true;
}
// GPS SPEED
NMEAParser::ExtractParameter(String,ctemp,6+offset);
GPS_INFO->Speed = StrToDouble(ctemp,NULL)/10;
// TRACK BEARING
if (GPS_INFO->Speed>1.0) {
NMEAParser::ExtractParameter(String,ctemp,5+offset);
GPS_INFO->TrackBearing = AngleLimit360(StrToDouble(ctemp, NULL));
}
// HGPS
NMEAParser::ExtractParameter(String,ctemp,7+offset);
GPS_INFO->Altitude = StrToDouble(ctemp,NULL);
// ------------------------
label_nogps:
// SATS
NMEAParser::ExtractParameter(String,ctemp,9+offset);
GPS_INFO->SatellitesUsed = (int) StrToDouble(ctemp,NULL);
// DATE
// Firmware 3.32 has got the date
if (offset>0) {
NMEAParser::ExtractParameter(String,ctemp,0);
long gy, gm, gd;
TCHAR *Stop;
gy = _tcstol(&ctemp[4], &Stop, 10) + 2000;
ctemp[4] = '\0';
gm = _tcstol(&ctemp[2], &Stop, 10);
ctemp[2] = '\0';
gd = _tcstol(&ctemp[0], &Stop, 10);
if ( ((gy > 1980) && (gy <2100) ) && (gm != 0) && (gd != 0) ) {
GPS_INFO->Year = gy;
GPS_INFO->Month = gm;
GPS_INFO->Day = gd;
}
}
// TIME
// ignoring 00:00.00
// And no UTC, since this is local time already.
NMEAParser::ExtractParameter(String,ctemp,0+offset);
double fixTime = StrToDouble(ctemp,NULL);
if (fixTime>0 && GPS_INFO->SatellitesUsed>0) {
double hours, mins,secs;
hours = fixTime / 10000;
GPS_INFO->Hour = (int)hours;
mins = fixTime / 100;
mins = mins - (GPS_INFO->Hour*100);
GPS_INFO->Minute = (int)mins;
secs = fixTime - (GPS_INFO->Hour*10000) - (GPS_INFO->Minute*100);
GPS_INFO->Second = (int)secs;
}
// HPA from the pressure sensor
// NMEAParser::ExtractParameter(String,ctemp,10+offset);
// double ps = StrToDouble(ctemp,NULL)/100;
// GPS_INFO->BaroAltitude = (1 - pow(fabs(ps / QNH), 0.190284)) * 44307.69;
// HBAR 1013.25
NMEAParser::ExtractParameter(String,ctemp,11+offset);
if (d == pDevPrimaryBaroSource) {
GPS_INFO->BaroAltitude = AltitudeToQNHAltitude(StrToDouble(ctemp,NULL));
GPS_INFO->BaroAltitudeAvailable = TRUE;
}
// VARIO
NMEAParser::ExtractParameter(String,ctemp,12+offset);
GPS_INFO->Vario = StrToDouble(ctemp,NULL)/100;
// TAS
NMEAParser::ExtractParameter(String,ctemp,13+offset);
vtas=StrToDouble(ctemp,NULL)/10;
GPS_INFO->IndicatedAirspeed = vtas/AirDensityRatio(GPS_INFO->BaroAltitude);
GPS_INFO->TrueAirspeed = vtas;
if (GPS_INFO->IndicatedAirspeed >0)
GPS_INFO->AirspeedAvailable = TRUE;
else
GPS_INFO->AirspeedAvailable = FALSE;
// ignore n.14 airspeed source
// OAT
NMEAParser::ExtractParameter(String,ctemp,15+offset);
GPS_INFO->OutsideAirTemperature = StrToDouble(ctemp,NULL);
GPS_INFO->TemperatureAvailable=TRUE;
// ignore n.16 baloon temperature
// BATTERY PERCENTAGES
NMEAParser::ExtractParameter(String,ctemp,17+offset);
GPS_INFO->ExtBatt1_Voltage = StrToDouble(ctemp,NULL)+1000;
NMEAParser::ExtractParameter(String,ctemp,18+offset);
GPS_INFO->ExtBatt2_Voltage = StrToDouble(ctemp,NULL)+1000;
GPS_INFO->VarioAvailable = TRUE;
// currently unused in LK, but ready for next future
TriggerVarioUpdate();
TriggerGPSUpdate();
return TRUE;
}
BOOL NMEAParser::GGA(TCHAR *String, TCHAR **params, size_t nparams, NMEA_INFO *GPS_INFO)
{
if (ReplayLogger::IsEnabled()) {
return TRUE;
}
GGAAvailable = TRUE;
GPSCONNECT = TRUE; // 091208
// this will force gps invalid but will NOT assume gps valid!
nSatellites = (int)(min(16.0, StrToDouble(params[6], NULL)));
if (nSatellites==0) {
gpsValid = false;
}
double ggafix = StrToDouble(params[5],NULL);
if ( ggafix==0 || ggafix>5 ) {
#ifdef DEBUG_GPS
if (ggafix>5) StartupStore(_T("------ GGA DEAD RECKON fix skipped%s"),NEWLINE);
#endif
gpsValid=false;
} else {
gpsValid=true;
}
// don't set any GPS_INFO if not activeGPS!!
if (!activeGPS) return TRUE;
GPS_INFO->SatellitesUsed = nSatellites; // 091208
GPS_INFO->NAVWarning = !gpsValid; // 091208
// // GPS_INFO->SatellitesUsed = (int)(min(16,StrToDouble(params[6], NULL))); 091205 091208 moved up
// GPS_INFO->SatellitesUsed = nSatellites; // 091205
double ggatime=StrToDouble(params[0],NULL);
// Even with invalid fix, we might still have valid time
// I assume that 0 is invalid, and I am very sorry for UTC time 00:00 ( missing a second a midnight).
// is better than risking using 0 as valid, since many gps do not respect any real nmea standard
if (ggatime>0) {
#ifdef NEWTRIGGERGPS
double ThisTime = TimeConvert(ggatime, GPS_INFO);
#else
double ThisTime = TimeModify(ggatime, GPS_INFO);
#endif
#ifndef NEWTRIGGERGPS
if (!TimeHasAdvanced(ThisTime, GPS_INFO))
return FALSE;
#endif
#ifdef NEWTRIGGERGPS
// is time advanced to a new quantum?
if (ThisTime >NmeaTime) {
// yes so lets trigger the gps event
TriggerGPSUpdate();
Sleep(50); // 091208
NmeaTime=ThisTime;
TimeSet(GPS_INFO); // 091208
TimeHasAdvanced(ThisTime, GPS_INFO); // 091208
StartupStore(_T(".............. trigger from GGA\n"));
}
#endif
}
if (gpsValid) {
double tmplat;
double tmplon;
tmplat = MixedFormatToDegrees(StrToDouble(params[1], NULL));
tmplat = NorthOrSouth(tmplat, params[2][0]);
tmplon = MixedFormatToDegrees(StrToDouble(params[3], NULL));
tmplon = EastOrWest(tmplon,params[4][0]);
if (!((tmplat == 0.0) && (tmplon == 0.0))) {
GPS_INFO->Latitude = tmplat;
GPS_INFO->Longitude = tmplon;
}
else {
#ifdef DEBUG_GPS
StartupStore(_T("++++++ GGA gpsValid with invalid posfix!%s"),NEWLINE);
#endif
gpsValid=false;
}
}
// GGA is marking now triggering end of data, so OK to use baro
// Even with invalid fix we might have valid baro data of course
// any NMEA sentence with time can now trigger gps update: the first to detect new time will make trigger.
// we assume also that any sentence with no time belongs to current time.
// note that if no time from gps, no use of vario and baro data, but also no fix available.. so no problems
if(RMZAvailable) {
GPS_INFO->BaroAltitudeAvailable = true;
GPS_INFO->BaroAltitude = RMZAltitude;
}
else if(RMAAvailable) {
GPS_INFO->BaroAltitudeAvailable = true;
GPS_INFO->BaroAltitude = RMAAltitude;
}
#ifndef NEWTRIGGERGPS
if (!gpsValid) { // 091108 addon BUGFIX GCA
// in old mode, GGA had priority over RMC for triggering, so this was needed in case of no signal
TriggerGPSUpdate(); // 091205 TESTFIX
return FALSE; // 091108 addon BUGFIX GCA
}
#endif
// "Altitude" should always be GPS Altitude.
GPS_INFO->Altitude = ParseAltitude(params[8], params[9]);
GPS_INFO->Altitude += (GPSAltitudeOffset/1000); // BUGFIX 100429
double GeoidSeparation;
if (_tcslen(params[10])>0) {
// No real need to parse this value,
// but we do assume that no correction is required in this case
GeoidSeparation = ParseAltitude(params[10], params[11]);
} else {
if (UseGeoidSeparation) {
GeoidSeparation = LookupGeoidSeparation(GPS_INFO->Latitude, GPS_INFO->Longitude);
GPS_INFO->Altitude -= GeoidSeparation;
}
}
#ifndef NEWTRIGGERGPS
// if RMC would be Triggering update, we loose the relative altitude, which is coming AFTER rmc!
// This was causing old altitude recorded in new pos fix.
TriggerGPSUpdate();
#endif
return TRUE;
}
BOOL NMEAParser::RMC(TCHAR *String, TCHAR **params, size_t nparams, NMEA_INFO *GPS_INFO)
{
TCHAR *Stop;
static bool logbaddate=true;
double speed=0;
gpsValid = !NAVWarn(params[1][0]);
GPSCONNECT = TRUE;
RMCAvailable=true; // 100409
if (!activeGPS) return TRUE; // 091205 BUGFIX true
// if no valid fix, we dont get speed either!
if (gpsValid)
{
speed = StrToDouble(params[6], NULL);
// speed is in knots, 2 = 3.7kmh
if (speed>2.0) {
GPS_INFO->MovementDetected = TRUE;
if (ReplayLogger::IsEnabled()) {
// stop logger replay if aircraft is actually moving.
ReplayLogger::Stop();
}
} else {
GPS_INFO->MovementDetected = FALSE;
if (ReplayLogger::IsEnabled()) {
// block actual GPS signal if not moving and a log is being replayed
return TRUE;
}
}
}
GPS_INFO->NAVWarning = !gpsValid;
// say we are updated every time we get this,
// so infoboxes get refreshed if GPS connected
// the RMC sentence marks the start of a new fix, so we force the old data to be saved for calculations
#ifndef NEWTRIGGERGPS
if (!GGAAvailable) {
TriggerGPSUpdate();
}
#endif
// Even with no valid position, we let RMC set the time and date if valid
long gy, gm, gd;
gy = _tcstol(¶ms[8][4], &Stop, 10) + 2000;
params[8][4] = '\0';
gm = _tcstol(¶ms[8][2], &Stop, 10);
params[8][2] = '\0';
gd = _tcstol(¶ms[8][0], &Stop, 10);
// SeeYou PC is sending NMEA sentences with RMC date 2072-02-27
if ( ((gy > 1980) && (gy <2100) ) && (gm != 0) && (gd != 0) ) {
GPS_INFO->Year = gy;
GPS_INFO->Month = gm;
GPS_INFO->Day = gd;
#ifdef NEWTRIGGERGPS
double ThisTime = TimeConvert(StrToDouble(params[0],NULL), GPS_INFO); // 091208
#else
double ThisTime = TimeModify(StrToDouble(params[0],NULL), GPS_INFO);
#endif
#ifndef NEWTRIGGERGPS
if (!TimeHasAdvanced(ThisTime, GPS_INFO))
return FALSE;
#endif
#ifdef NEWTRIGGERGPS
// is time advanced to a new quantum?
if (ThisTime >NmeaTime) {
// yes so lets trigger the gps event
TriggerGPSUpdate();
// and only then advance the time in the GPSINFO
Sleep(50); // 091208
NmeaTime=ThisTime;
TimeSet(GPS_INFO); // 091208
TimeHasAdvanced(ThisTime, GPS_INFO); // 091208
StartupStore(_T(".............. trigger from RMC\n"));
}
#endif
} else {
if (gpsValid && logbaddate) { // 091115
StartupStore(_T("------ NMEAParser:RMC Receiving an invalid or null DATE from GPS%s"),NEWLINE);
StartupStore(_T("------ NMEAParser: Date received is y=%d m=%d d=%d%s"),gy,gm,gd,NEWLINE); // 100422
StartupStore(_T("------ This message will NOT be repeated.%s"),NEWLINE);
// DoStatusMessage(_T("WARNING: GPS IS SENDING INVALID DATE, AND PROBABLY WRONG TIME")); // REMOVE FIXV2
// LKTOKEN 875 WARNING: GPS IS SENDING INVALID DATE, AND PROBABLY WRONG TIME"));
DoStatusMessage(gettext(TEXT("_@M875_")));
logbaddate=false;
}
}
// } // 091108
if (gpsValid) { // 091108 BUGFIX set latlon and speed ONLY if valid gpsdata, missing check!
double tmplat;
double tmplon;
tmplat = MixedFormatToDegrees(StrToDouble(params[2], NULL));
tmplat = NorthOrSouth(tmplat, params[3][0]);
tmplon = MixedFormatToDegrees(StrToDouble(params[4], NULL));
tmplon = EastOrWest(tmplon,params[5][0]);
if (!((tmplat == 0.0) && (tmplon == 0.0))) {
GPS_INFO->Latitude = tmplat;
GPS_INFO->Longitude = tmplon;
}
GPS_INFO->Speed = KNOTSTOMETRESSECONDS * speed;
if (GPS_INFO->Speed>1.0) {
// JMW don't update bearing unless we're moving
GPS_INFO->TrackBearing = AngleLimit360(StrToDouble(params[7], NULL));
}
} // gpsvalid 091108
// Altair doesn't have a battery-backed up realtime clock,
// so as soon as we get a fix for the first time, set the
// system clock to the GPS time.
static bool sysTimeInitialised = false;
if (!GPS_INFO->NAVWarning && (gpsValid)) {
if (SetSystemTimeFromGPS) {
if (!sysTimeInitialised) {
if ( ( GPS_INFO->Year > 1980 && GPS_INFO->Year<2100) && ( GPS_INFO->Month > 0) && ( GPS_INFO->Hour > 0)) {
sysTimeInitialised =true; // Attempting only once
SYSTEMTIME sysTime;
// ::GetSystemTime(&sysTime);
int hours = (int)GPS_INFO->Hour;
int mins = (int)GPS_INFO->Minute;
int secs = (int)GPS_INFO->Second;
sysTime.wYear = (unsigned short)GPS_INFO->Year;
sysTime.wMonth = (unsigned short)GPS_INFO->Month;
sysTime.wDay = (unsigned short)GPS_INFO->Day;
sysTime.wHour = (unsigned short)hours;
sysTime.wMinute = (unsigned short)mins;
sysTime.wSecond = (unsigned short)secs;
sysTime.wMilliseconds = 0;
::SetSystemTime(&sysTime);
}
}
}
}
if (!ReplayLogger::IsEnabled()) {
if(RMZAvailable) {
// JMW changed from Altitude to BaroAltitude
GPS_INFO->BaroAltitudeAvailable = true;
GPS_INFO->BaroAltitude = RMZAltitude;
}
else if(RMAAvailable) {
// JMW changed from Altitude to BaroAltitude
GPS_INFO->BaroAltitudeAvailable = true;
GPS_INFO->BaroAltitude = RMAAltitude;
}
}
if (!GGAAvailable) {
// update SatInUse, some GPS receiver dont emmit GGA sentance
if (!gpsValid) {
GPS_INFO->SatellitesUsed = 0;
} else {
GPS_INFO->SatellitesUsed = -1;
}
}
return TRUE;
}
static BOOL FLYSEN(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *GPS_INFO)
{
TCHAR ctemp[80];
double vtas;
// VOID GPS SIGNAL
NMEAParser::ExtractParameter(String,ctemp,8);
if (_tcscmp(ctemp,_T("V"))==0) {
GPS_INFO->NAVWarning=true;
GPSCONNECT=false;
goto label_nogps;
}
// ------------------------
double tmplat;
double tmplon;
NMEAParser::ExtractParameter(String,ctemp,1);
tmplat = MixedFormatToDegrees(StrToDouble(ctemp, NULL));
NMEAParser::ExtractParameter(String,ctemp,2);
tmplat = NorthOrSouth(tmplat, ctemp[0]);
NMEAParser::ExtractParameter(String,ctemp,3);
tmplon = MixedFormatToDegrees(StrToDouble(ctemp, NULL));
NMEAParser::ExtractParameter(String,ctemp,4);
tmplon = EastOrWest(tmplon,ctemp[0]);
if (!((tmplat == 0.0) && (tmplon == 0.0))) {
GPS_INFO->Latitude = tmplat;
GPS_INFO->Longitude = tmplon;
GPS_INFO->NAVWarning=false;
GPSCONNECT=true;
}
// GPS SPEED
NMEAParser::ExtractParameter(String,ctemp,6);
GPS_INFO->Speed = StrToDouble(ctemp,NULL)/10;
// TRACK BEARING
if (GPS_INFO->Speed>1.0) {
NMEAParser::ExtractParameter(String,ctemp,5);
GPS_INFO->TrackBearing = AngleLimit360(StrToDouble(ctemp, NULL));
}
// HGPS
NMEAParser::ExtractParameter(String,ctemp,7);
GPS_INFO->Altitude = StrToDouble(ctemp,NULL);
// ------------------------
label_nogps:
// SATS
NMEAParser::ExtractParameter(String,ctemp,9);
GPS_INFO->SatellitesUsed = (int) StrToDouble(ctemp,NULL);
GPS_INFO->SatellitesUsed = 4;
// TIME
// ignoring 00:00.00 , but that's minor problem. We don't have the date.
// And no UTC, since this is local time already.
NMEAParser::ExtractParameter(String,ctemp,0);
double fixTime = StrToDouble(ctemp,NULL);
if (fixTime>0 && GPS_INFO->SatellitesUsed>0) {
double hours, mins,secs;
hours = fixTime / 10000;
GPS_INFO->Hour = (int)hours;
mins = fixTime / 100;
mins = mins - (GPS_INFO->Hour*100);
GPS_INFO->Minute = (int)mins;
secs = fixTime - (GPS_INFO->Hour*10000) - (GPS_INFO->Minute*100);
GPS_INFO->Second = (int)secs;
}
// HPA from the pressure sensor
// NMEAParser::ExtractParameter(String,ctemp,10);
// double ps = StrToDouble(ctemp,NULL)/100;
// GPS_INFO->BaroAltitude = (1 - pow(fabs(ps / QNH), 0.190284)) * 44307.69;
// HBAR 1013.25
NMEAParser::ExtractParameter(String,ctemp,11);
GPS_INFO->BaroAltitude = AltitudeToQNHAltitude(StrToDouble(ctemp,NULL));
// VARIO
NMEAParser::ExtractParameter(String,ctemp,12);
GPS_INFO->Vario = StrToDouble(ctemp,NULL)/100;
// TAS
NMEAParser::ExtractParameter(String,ctemp,13);
vtas=StrToDouble(ctemp,NULL)/10;
GPS_INFO->IndicatedAirspeed = vtas/AirDensityRatio(GPS_INFO->BaroAltitude);
GPS_INFO->TrueAirspeed = vtas;
if (GPS_INFO->IndicatedAirspeed >0)
GPS_INFO->AirspeedAvailable = TRUE;
else
GPS_INFO->AirspeedAvailable = FALSE;
// ignore n.14 airspeed source
// OAT
NMEAParser::ExtractParameter(String,ctemp,15);
GPS_INFO->OutsideAirTemperature = StrToDouble(ctemp,NULL);
GPS_INFO->TemperatureAvailable=TRUE;
// ignore n.16 baloon temperature
// BATTERY PERCENTAGES
NMEAParser::ExtractParameter(String,ctemp,17);
GPS_INFO->ExtBatt1_Voltage = StrToDouble(ctemp,NULL)+1000;
NMEAParser::ExtractParameter(String,ctemp,18);
GPS_INFO->ExtBatt2_Voltage = StrToDouble(ctemp,NULL)+1000;
GPS_INFO->BaroAltitudeAvailable = TRUE;
GPS_INFO->VarioAvailable = TRUE;
// currently unused in LK, but ready for next future
TriggerVarioUpdate();
TriggerGPSUpdate();
return TRUE;
}
