static BOOL PDGFTL1(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *GPS_INFO) { /* $PDGFTL1 field example QNE 1013.25 altitude 0 2025 meters QNH altitude 1 2000 meters vario cm/s 2 250 2.5m/s netto vario 3 -14 dm/s IAS 4 45 kmh ground efficiency 5 134 13:4 GR Wind speed 6 28 kmh Wind direction 7 65 degrees Main lithium battery 3,82 v 8 382 0.01v Backup AA battery 1,53 v 9 153 0.01v 100209 Paolo Ventafridda netto and ias optionals */ TCHAR ctemp[80]; double vtas, vias; double altqne, altqnh; static bool initqnh=true; NMEAParser::ExtractParameter(String,ctemp,0); altqne = StrToDouble(ctemp,NULL); NMEAParser::ExtractParameter(String,ctemp,1); altqnh = StrToDouble(ctemp,NULL); // AutoQNH will take care of setting an average QNH if nobody does it for a while if (initqnh) { // if digifly has qnh set by user qne and qnh are of course different if (altqne != altqnh) { QNH=FindQNH(altqne,altqnh); CAirspaceManager::Instance().QnhChangeNotify(QNH); StartupStore(_T(". Using Digifly QNH %f%s"),QNH,NEWLINE); initqnh=false; } else { // if locally QNH was set, either by user of by AutoQNH, stop processing QNH from digifly if ( (QNH <= 1012) || (QNH>=1014)) initqnh=false; // else continue entering initqnh until somebody changes qnh in either digifly or lk8000 } } if (d == pDevPrimaryBaroSource) { UpdateBaroSource( GPS_INFO, DIGIFLY, AltitudeToQNHAltitude(altqne)); } NMEAParser::ExtractParameter(String,ctemp,2); #if 1 GPS_INFO->Vario = StrToDouble(ctemp,NULL)/100; #else double newVario = StrToDouble(ctemp,NULL)/100; GPS_INFO->Vario = LowPassFilter(GPS_INFO->Vario,newVario,0.1); #endif GPS_INFO->VarioAvailable = TRUE; NMEAParser::ExtractParameter(String,ctemp,3); if (ctemp[0] != '\0') { GPS_INFO->NettoVario = StrToDouble(ctemp,NULL)/10; // dm/s GPS_INFO->NettoVarioAvailable = TRUE; } else GPS_INFO->NettoVarioAvailable = FALSE; NMEAParser::ExtractParameter(String,ctemp,4); if (ctemp[0] != '\0') { // we store m/s , so we convert it from kmh vias = StrToDouble(ctemp,NULL)/3.6; if (vias >1) { vtas = vias*AirDensityRatio(GPS_INFO->BaroAltitude); GPS_INFO->TrueAirspeed = vtas; GPS_INFO->IndicatedAirspeed = vias; GPS_INFO->AirspeedAvailable = TRUE; } } else { GPS_INFO->AirspeedAvailable = FALSE; } NMEAParser::ExtractParameter(String,ctemp,8); GPS_INFO->ExtBatt1_Voltage = StrToDouble(ctemp,NULL)/100; NMEAParser::ExtractParameter(String,ctemp,9); GPS_INFO->ExtBatt2_Voltage = StrToDouble(ctemp,NULL)/100; TriggerVarioUpdate(); return TRUE; }
bool ReplayLogger::UpdateInternal(void) { static bool init=true; if (!Enabled) { init = true; ReadLine(NULL); // close file Enabled = true; } static CatmullRomInterpolator cli; SYSTEMTIME st; GetLocalTime(&st); static double time_lstart = 0; if (init) { time_lstart = 0; } static double time=0; double deltatimereal; bool finished = false; double timelast = time; time = (st.wHour*3600+st.wMinute*60+st.wSecond-time_lstart); deltatimereal = time-timelast; if (init) { time_lstart = time; time = 0; deltatimereal = 0; ReplayTime = 0; cli.Reset(); } ReplayTime += TimeScale*deltatimereal; #if DEBUG_REPLAY TCHAR tutc[20]; Units::TimeToText(tutc, (int)ReplayTime); StartupStore(_T("........ REPLAY: UTC h%s\n"),tutc); #endif double mintime = cli.GetMinTime(); // li_lat.GetMinTime(); if (ReplayTime<mintime) { ReplayTime = mintime; } // if need a new point while (cli.NeedData(ReplayTime)&&(!finished)) { double t1, Lat1, Lon1, Alt1; finished = !ReadPoint(&t1,&Lat1,&Lon1,&Alt1); if (!finished && (t1>0)) { if (!cli.Update(t1,Lon1,Lat1,Alt1)) { break; } } } if (!finished) { double LatX, LonX, AltX, SpeedX, BearingX; double LatX1, LonX1, AltX1; cli.Interpolate(ReplayTime, &LonX, &LatX, &AltX); cli.Interpolate(ReplayTime+0.1, &LonX1, &LatX1, &AltX1); SpeedX = cli.GetSpeed(ReplayTime); DistanceBearing(LatX, LonX, LatX1, LonX1, NULL, &BearingX); if ((SpeedX>0) && (LatX != LatX1) && (LonX != LonX1)) { LockFlightData(); if (init) { flightstats.Reset(); } GPS_INFO.Latitude = LatX; GPS_INFO.Longitude = LonX; GPS_INFO.Speed = SpeedX; GPS_INFO.TrackBearing = BearingX; GPS_INFO.Altitude = AltX; GPS_INFO.BaroAltitude = AltitudeToQNHAltitude(AltX); GPS_INFO.Time = ReplayTime; UnlockFlightData(); } else { // This is required in case the integrator fails, // which can occur due to parsing faults ReplayTime = cli.GetMaxTime(); } } // quit if finished. if (finished) { #if DEBUG_REPLAY StartupStore(_T("......... REPLAY: FINISHED\n")); #endif Stop(); } init = false; return !finished; }
static BOOL PILC(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS) { TCHAR ctemp[80]; NMEAParser::ExtractParameter(String,ctemp,0); if (_tcscmp(ctemp,_T("PDA1"))==0) { NMEAParser::ExtractParameter(String,ctemp,1); UpdateBaroSource( pGPS, 0,d, AltitudeToQNHAltitude(StrToDouble(ctemp, NULL))); NMEAParser::ExtractParameter(String,ctemp,2); pGPS->Vario = StrToDouble(ctemp,NULL); NMEAParser::ExtractParameter(String,ctemp,3); // wind direction, integer double wfrom; wfrom = StrToDouble(ctemp,NULL); //@ could also be the NMEA checksum! NMEAParser::ExtractParameter(String,ctemp,4); // wind speed kph integer if (_tcslen(ctemp)!=0) { #if 1 // 120424 fix correct wind setting double wspeed, wconfidence; wspeed = StrToDouble(ctemp,NULL); NMEAParser::ExtractParameter(String,ctemp,5); // confidence 0-100 percentage wconfidence = StrToDouble(ctemp,NULL); pGPS->ExternalWindAvailable = TRUE; if (wconfidence>=1) { pGPS->ExternalWindSpeed = wspeed/TOKPH; pGPS->ExternalWindDirection = wfrom; } #else double wspeed, wconfidence; wspeed = StrToDouble(ctemp,NULL); NMEAParser::ExtractParameter(String,ctemp,5); // confidence 0-100 percentage wconfidence = StrToDouble(ctemp,NULL); // StartupStore(_T(".... WIND from %.0f speed=%.0f kph confidence=%.0f\n"),wfrom,wspeed,wconfidence); SetWindEstimate(wspeed/TOKPH, wfrom,(int)(wconfidence/100)*10); CALCULATED_INFO.WindSpeed=wspeed/TOKPH; CALCULATED_INFO.WindBearing=wfrom; #endif } else pGPS->ExternalWindAvailable = FALSE; pGPS->VarioAvailable = TRUE; TriggerVarioUpdate(); return TRUE; } if (_tcscmp(ctemp,_T("SET"))==0) { NMEAParser::ExtractParameter(String,ctemp,1); QNH = StrToDouble(ctemp,NULL); // StartupStore(_T("... SET QNH= %.1f\n"),QNH); CAirspaceManager::Instance().QnhChangeNotify(QNH); return TRUE; } // Using the polar requires recalculating polar coefficients internally each time.. no thanks. // if (_tcscmp(ctemp,_T("POLAR"))==0) { } 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; }