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;
}
