static BOOL LK8EX1(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *GPS_INFO)
{
TCHAR ctemp[80];
bool havebaro=false;
// HPA from the pressure sensor
NMEAParser::ExtractParameter(String,ctemp,0);
double ps = StrToDouble(ctemp,NULL);
if (ps!=999999) {
ps/=100;
if (d == pDevPrimaryBaroSource) {
UpdateBaroSource( GPS_INFO, LK_EXT1, (1 - pow(fabs(ps / QNH), 0.190284)) * 44307.69);
}
}
// QNE
if (!havebaro) {
NMEAParser::ExtractParameter(String,ctemp,1);
double ba = StrToDouble(ctemp,NULL);
if (ba!=99999) {
if (d == pDevPrimaryBaroSource) {
UpdateBaroSource( GPS_INFO, LK_EXT1, AltitudeToQNHAltitude(ba));
}
}
}
// VARIO
NMEAParser::ExtractParameter(String,ctemp,2);
double va = StrToDouble(ctemp,NULL);
if (va != 9999) {
GPS_INFO->Vario = va/100;
GPS_INFO->VarioAvailable = TRUE;
} else
GPS_INFO->VarioAvailable = FALSE;
// OAT
NMEAParser::ExtractParameter(String,ctemp,3);
double ta = StrToDouble(ctemp,NULL);
if (ta != 99) {
GPS_INFO->OutsideAirTemperature = ta;
GPS_INFO->TemperatureAvailable=TRUE;
}
// BATTERY PERCENTAGES
NMEAParser::ExtractParameter(String,ctemp,4);
double voa = StrToDouble(ctemp,NULL);
if (voa!=999) {
GPS_INFO->ExtBatt1_Voltage = voa;
}
// currently unused in LK, but ready for next future
TriggerVarioUpdate();
return TRUE;
}
static BOOL LK8EX1(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS)
{
TCHAR ctemp[80];
bool havebaro=false;
// HPA from the pressure sensor
NMEAParser::ExtractParameter(String,ctemp,0);
double ps = StrToDouble(ctemp,NULL);
if (ps!=999999) {
UpdateBaroSource( pGPS, 0,d, StaticPressureToAltitude(ps));
havebaro = true;
}
// QNE
if (!havebaro) {
NMEAParser::ExtractParameter(String,ctemp,1);
double ba = StrToDouble(ctemp,NULL);
if (ba!=99999) {
UpdateBaroSource( pGPS, 0,d, AltitudeToQNHAltitude(ba));
}
}
// VARIO
NMEAParser::ExtractParameter(String,ctemp,2);
double va = StrToDouble(ctemp,NULL);
if (va != 9999) {
pGPS->Vario = va/100;
pGPS->VarioAvailable = TRUE;
} else
pGPS->VarioAvailable = FALSE;
// OAT
NMEAParser::ExtractParameter(String,ctemp,3);
double ta = StrToDouble(ctemp,NULL);
if (ta != 99) {
pGPS->OutsideAirTemperature = ta;
pGPS->TemperatureAvailable=TRUE;
}
// BATTERY PERCENTAGES
NMEAParser::ExtractParameter(String,ctemp,4);
double voa = StrToDouble(ctemp,NULL);
if (voa!=999) {
pGPS->ExtBatt1_Voltage = voa;
}
// currently unused in LK, but ready for next future
TriggerVarioUpdate();
return TRUE;
}
// RMN: Volkslogger
// Source data:
// $PGCS,1,0EC0,FFF9,0C6E,02*61
// $PGCS,1,0EC0,FFFA,0C6E,03*18
BOOL vl_PGCS1(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS)
{
TCHAR ctemp[80];
double InternalAltitude;
NMEAParser::ExtractParameter(String,ctemp,2);
// four characers, hex, barometric altitude
InternalAltitude = HexStrToDouble(ctemp,NULL);
double fBaroAltitude =0;
if(InternalAltitude > 60000)
fBaroAltitude =
AltitudeToQNHAltitude(InternalAltitude - 65535);
// Assuming that altitude has wrapped around. 60 000 m occurs at
// QNH ~2000 hPa
else
fBaroAltitude =
AltitudeToQNHAltitude(InternalAltitude);
// typo corrected 21.04.07
// Else the altitude is good enough.
UpdateBaroSource( pGPS, 0,d, fBaroAltitude);
// ExtractParameter(String,ctemp,3);
// four characters, hex, constant. Value 1371 (dec)
// nSatellites = (int)(min(12,HexStrToDouble(ctemp, NULL)));
if (pGPS->SatellitesUsed <= 0) {
pGPS->SatellitesUsed = 4;
}
return FALSE;
}
static BOOL VARIO(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS)
{
// $VARIO,fPressure,fVario,Bat1Volts,Bat2Volts,BatBank,TempSensor1,TempSensor2*CS
TCHAR ctemp[80];
NMEAParser::ExtractParameter(String,ctemp,0);
double ps = StrToDouble(ctemp,NULL);
UpdateBaroSource( pGPS, 0,d, (1 - pow(fabs(ps / QNH), 0.190284)) * 44307.69);
NMEAParser::ExtractParameter(String,ctemp,1);
pGPS->Vario = StrToDouble(ctemp,NULL)/10.0;
// JMW vario is in dm/s
NMEAParser::ExtractParameter(String,ctemp,2);
pGPS->ExtBatt1_Voltage = StrToDouble(ctemp,NULL);
NMEAParser::ExtractParameter(String,ctemp,3);
pGPS->ExtBatt2_Voltage = StrToDouble(ctemp,NULL);
NMEAParser::ExtractParameter(String,ctemp,4);
pGPS->ExtBatt_Bank = (int)StrToDouble(ctemp,NULL);
/*
StartupStore(_T("++++++ BATTBANK: "));
StartupStore(ctemp);
StartupStore(_T("\n"));
*/
pGPS->VarioAvailable = TRUE;
TriggerVarioUpdate();
return TRUE;
}
BOOL NMEAParser::RMZ(TCHAR *String, TCHAR **params, size_t nparams, NMEA_INFO *pGPS)
{
(void)pGPS;
// We want to wait for a couple of run so we are sure we are receiving RMC GGA etc.
if (RMZDelayed--) {
#if DEBUGBARO
StartupStore(_T("...RMZ delayed, not processed (%d)\n"),RMZDelayed);
#endif
return FALSE;
}
RMZDelayed=0;
RMZAltitude = ParseAltitude(params[0], params[1]);
RMZAltitude = AltitudeToQNHAltitude(RMZAltitude);
RMZAvailable = TRUE;
LastRMZHB=LKHearthBeats; // this is common to both ports!
// If we have a single RMZ with no gps fix data, we still manage the baro altitude.
if (!RMCAvailable && !GGAAvailable) {
UpdateBaroSource(pGPS, isFlarm? BARO__RMZ_FLARM:BARO__RMZ, NULL, RMZAltitude);
}
return FALSE;
}
static BOOL _PRS(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *_INFO){
(void)d;
if(UpdateBaroSource(_INFO, 0, d, StaticPressureToAltitude((HexStrToInt(String)*1.0)))){
TriggerVarioUpdate();
}
return TRUE;
}
static BOOL PILC(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *GPS_INFO)
{
TCHAR ctemp[80];
NMEAParser::ExtractParameter(String,ctemp,0);
if (_tcscmp(ctemp,_T("PDA1"))==0) {
NMEAParser::ExtractParameter(String,ctemp,1);
if (d == pDevPrimaryBaroSource)
{
UpdateBaroSource( GPS_INFO, ILEC, AltitudeToQNHAltitude(StrToDouble(ctemp, NULL)));
}
NMEAParser::ExtractParameter(String,ctemp,2);
GPS_INFO->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) {
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;
}
// else StartupStore(_T(".. NO WIND\n"));
GPS_INFO->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;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/// Parses LXWP0 sentence.
///
/// @param d device descriptor
/// @param sentence received NMEA sentence
/// @param info GPS info to be updated
///
/// @retval true if the sentence has been parsed
///
//static
bool DevLXMiniMap::LXWP0(PDeviceDescriptor_t d, const TCHAR* sentence, NMEA_INFO* info)
{
// $LXWP0,logger_stored, airspeed, airaltitude,
// v1[0],v1[1],v1[2],v1[3],v1[4],v1[5], hdg, windspeed*CS<CR><LF>
//
// 0 loger_stored : [Y|N] (not used in LX1600)
// 1 IAS [km/h] ----> Condor uses TAS!
// 2 baroaltitude [m]
// 3-8 vario values [m/s] (last 6 measurements in last second)
// 9 heading of plane (not used in LX1600)
// 10 windcourse [deg] (not used in LX1600)
// 11 windspeed [km/h] (not used in LX1600)
//
// e.g.:
// $LXWP0,Y,222.3,1665.5,1.71,,,,,,239,174,10.1
TICKER = GetTickCount();
double alt, airspeed;
if (ParToDouble(sentence, 1, &airspeed))
{
airspeed /= TOKPH;
info->TrueAirspeed = airspeed;
info->AirspeedAvailable = TRUE;
}
if (ParToDouble(sentence, 2, &alt))
{
info->IndicatedAirspeed = airspeed / AirDensityRatio(alt);
if (d == pDevPrimaryBaroSource)
{
UpdateQNH(CalculateQNH(alt, alt + AltOffset));
UpdateBaroSource(info, 0, d, alt + AltOffset);
/* if(FirstCheckBaroAlt)
{
FirstCheckBaroAlt = false;
InputEvents::eventSetup(_T("Basic"));
}*/
}
}
if (ParToDouble(sentence, 3, &info->Vario))
info->VarioAvailable = TRUE;
if (ParToDouble(sentence, 10, &info->ExternalWindDirection) &&
ParToDouble(sentence, 11, &info->ExternalWindSpeed))
info->ExternalWindAvailable = TRUE;
TriggerVarioUpdate();
return(true);
} // LXWP0()
bool DevLXV7_EXP::PLXVF(PDeviceDescriptor_t d, const TCHAR* sentence, NMEA_INFO* info)
{
double alt, airspeed;
if (ParToDouble(sentence, 1, &info->AccelX))
if (ParToDouble(sentence, 2, &info->AccelY))
if (ParToDouble(sentence, 3, &info->AccelZ))
info->AccelerationAvailable = true;
if (ParToDouble(sentence, 5, &airspeed))
{
// airspeed = 135.0/TOKPH;
info->IndicatedAirspeed = airspeed;
info->AirspeedAvailable = TRUE;
}
if (ParToDouble(sentence, 6, &alt))
{
UpdateBaroSource( info, 0, d, AltitudeToQNHAltitude(alt));
info->TrueAirspeed = airspeed * AirDensityRatio(alt);
}
if (ParToDouble(sentence, 4, &info->Vario))
{
info->VarioAvailable = TRUE;
TriggerVarioUpdate();
}
// Get STF switch
double fTmp;
if (ParToDouble(sentence, 7, &fTmp))
{
int iTmp = (int)(fTmp+0.1);
EnableExternalTriggerCruise = true;
static int iOldVarioSwitch=0;
if(iTmp != iOldVarioSwitch)
{
iOldVarioSwitch = iTmp;
if(iTmp==1)
{
ExternalTriggerCruise = true;
ExternalTriggerCircling = false;
}
else
{
ExternalTriggerCruise = false;
ExternalTriggerCircling = true;
}
}
}
return(true);
} // PLXVF()
static BOOL PZAN1(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *apGPS)
{
double palt=0;
TCHAR ctemp[80];
NMEAParser::ExtractParameter(String,ctemp,0);
palt=StrToDouble(ctemp,NULL);
UpdateBaroSource( apGPS, 0,d, AltitudeToQNHAltitude(palt));
return TRUE;
}
static BOOL GPWIN(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS)
{
TCHAR ctemp[80];
(void)pGPS;
(void)d;
NMEAParser::ExtractParameter(String, ctemp, 2);
UpdateBaroSource( pGPS, 0, d, AltitudeToQNHAltitude( iround(StrToDouble(ctemp, NULL) / 10)));
return FALSE;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/// Parses LXWP0 sentence.
///
/// @param d device descriptor
/// @param sentence received NMEA sentence
/// @param info GPS info to be updated
///
/// @retval true if the sentence has been parsed
///
//static
bool DevLX::LXWP0(PDeviceDescriptor_t d, const TCHAR* sentence, NMEA_INFO* info)
{
// $LXWP0,logger_stored, airspeed, airaltitude,
// v1[0],v1[1],v1[2],v1[3],v1[4],v1[5], hdg, windspeed*CS<CR><LF>
//
// 0 loger_stored : [Y|N] (not used in LX1600)
// 1 IAS [km/h] ----> Condor uses TAS!
// 2 baroaltitude [m]
// 3-8 vario values [m/s] (last 6 measurements in last second)
// 9 heading of plane (not used in LX1600)
// 10 windcourse [deg] (not used in LX1600)
// 11 windspeed [km/h] (not used in LX1600)
//
// e.g.:
// $LXWP0,Y,222.3,1665.5,1.71,,,,,,239,174,10.1
double alt=0, airspeed=0;
if (ParToDouble(sentence, 1, &airspeed))
{
airspeed /= TOKPH;
info->TrueAirspeed = airspeed;
info->AirspeedAvailable = TRUE;
}
if (ParToDouble(sentence, 2, &alt))
{
if (airspeed>0) {
LKASSERT(AirDensityRatio(alt)!=0);
info->IndicatedAirspeed = airspeed / AirDensityRatio(alt);
}
UpdateBaroSource( info, 0,d, AltitudeToQNHAltitude(alt));
}
if (ParToDouble(sentence, 8, &info->Vario)) /* take the last value to be more recent */
info->VarioAvailable = TRUE;
if (ParToDouble(sentence, 9, &info->MagneticHeading))
info->MagneticHeadingAvailable=TRUE;
if (ParToDouble(sentence, 10, &info->ExternalWindDirection) &&
ParToDouble(sentence, 11, &info->ExternalWindSpeed))
{
info->ExternalWindSpeed /= TOKPH; /* convert to m/s */
info->ExternalWindAvailable = TRUE;
}
TriggerVarioUpdate();
return(true);
} // LXWP0()
BOOL EWMicroRecorderParseNMEA(PDeviceDescriptor_t d,
TCHAR *String, NMEA_INFO *pGPS){
TCHAR ctemp[80], *params[5];
int nparams = NMEAParser::ValidateAndExtract(String, ctemp, 80, params, 5);
if (nparams < 1)
return FALSE;
if (!_tcscmp(params[0], TEXT("$PGRMZ")) && nparams >= 3) {
double altitude = NMEAParser::ParseAltitude(params[1], params[2]);
UpdateBaroSource( pGPS, 0,d, AltitudeToQNHAltitude(altitude));
return TRUE;
}
return FALSE;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/// Parses LXWP0 sentence.
///
/// @param d device descriptor
/// @param sentence received NMEA sentence
/// @param info GPS info to be updated
///
/// @retval true if the sentence has been parsed
///
//static
bool DevLX16xx::LXWP0(PDeviceDescriptor_t d, const TCHAR* sentence, NMEA_INFO* info)
{
// $LXWP0,logger_stored, airspeed, airaltitude,
// v1[0],v1[1],v1[2],v1[3],v1[4],v1[5], hdg, windspeed*CS<CR><LF>
//
// 0 loger_stored : [Y|N] (not used in LX1600)
// 1 IAS [km/h] ----> Condor uses TAS!
// 2 baroaltitude [m]
// 3-8 vario values [m/s] (last 6 measurements in last second)
// 9 heading of plane (not used in LX1600)
// 10 windcourse [deg] (not used in LX1600)
// 11 windspeed [km/h] (not used in LX1600)
//
// e.g.:
// $LXWP0,Y,222.3,1665.5,1.71,,,,,,239,174,10.1
double alt, airspeed;
if (ParToDouble(sentence, 1, &airspeed))
{
airspeed /= TOKPH;
info->TrueAirspeed = airspeed;
info->AirspeedAvailable = TRUE;
}
if(LX166AltitudeUpdateTimeout >0)
LX166AltitudeUpdateTimeout--;
else
if (ParToDouble(sentence, 2, &alt))
{
LX16xxAlt = (int) alt;
info->IndicatedAirspeed = airspeed / AirDensityRatio(alt);
UpdateBaroSource( info, 0,d, AltitudeToQNHAltitude(alt));
}
if (ParToDouble(sentence, 3, &info->Vario))
info->VarioAvailable = TRUE;
/*
if (ParToDouble(sentence, 10, &info->ExternalWindDirection) &&
ParToDouble(sentence, 11, &info->ExternalWindSpeed))
info->ExternalWindAvailable = TRUE;
*/
TriggerVarioUpdate();
return(true);
} // LXWP0()
// TASMAN instruments support for Tasman Flight Pack model Fp10
BOOL NMEAParser::PTAS1(TCHAR *String, TCHAR **params, size_t nparams, NMEA_INFO *pGPS)
{
double wnet,baralt,vtas;
wnet = (StrToDouble(params[0],NULL)-200)/(10*TOKNOTS);
baralt = max(0.0, (StrToDouble(params[2],NULL)-2000)/TOFEET);
vtas = StrToDouble(params[3],NULL)/TOKNOTS;
pGPS->AirspeedAvailable = TRUE;
pGPS->TrueAirspeed = vtas;
pGPS->VarioAvailable = TRUE;
pGPS->Vario = wnet;
UpdateBaroSource(pGPS, BARO__TASMAN, NULL, AltitudeToQNHAltitude(baralt));
pGPS->IndicatedAirspeed = vtas/AirDensityRatio(baralt);
TASAvailable = true; // 100411
TriggerVarioUpdate();
return FALSE;
}
bool PLXVF(PDeviceDescriptor_t d, const TCHAR* sentence, NMEA_INFO* info)
{
double alt, airspeed;
if (ParToDouble(sentence, 5, &airspeed)) {
info->IndicatedAirspeed = airspeed;
info->AirspeedAvailable = TRUE;
}
if (ParToDouble(sentence, 6, &alt)) {
UpdateBaroSource( info, 0, d, AltitudeToQNHAltitude(alt));
info->TrueAirspeed = airspeed * AirDensityRatio(alt);
}
if (ParToDouble(sentence, 4, &info->Vario)) {
info->VarioAvailable = TRUE;
TriggerVarioUpdate();
}
return(true);
}
static BOOL PWES0(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS)
{
/*
Sent by Westerboer VW1150 combining data stream from Flarm and VW1020.
RMZ is being sent too, which is a problem.
$PWES0,22,10,8,18,17,6,1767,1804,1073,1073,116,106
A B C D E F G H I J K L
A Device 21 = VW1000, 21 = VW 1010, 22 = VW1020, 23 = VW1030
B vario *10 = 2.2 m/s
C average vario *10 = 1.0 m/s
D netto vario *10 = 1.8 m/s
E average netto vario *10 = 1.7 m/s
F stf = -999.. 999 (neg faster.. slower)
G baro altitude = 1767 m
H baro altitude calibrated by user?
I IAS kmh *10 = 107.3 kmh
J TAS kmh *10 ?
K battery V *10 = 11.6 V
L OAT * 10 = 10.6 C
*/
TCHAR ctemp[180];
double vtas, vias;
double altqne, altqnh;
static bool initqnh=true;
#ifdef DEVICE_SERIAL
static int NoMsg =0;
// static int HardwareId = 0;
if(_tcslen(String) < 180)
if(((pGPS->SerialNumber == 0) || (oldSerial != SerialNumber)) && (NoMsg < 5))
{
NoMsg++ ;
NMEAParser::ExtractParameter(String,ctemp,0);
pGPS->HardwareId= (int)StrToDouble(ctemp,NULL);
switch (pGPS->HardwareId)
{
case 21: _tcscpy(d->Name, TEXT("VW1010")); break;
case 22: _tcscpy(d->Name, TEXT("VW1020")); break;
case 23: _tcscpy(d->Name, TEXT("VW1030")); break;
default: _tcscpy(d->Name, TEXT("Westerboer")); break;
}
StartupStore(_T(". %s\n"),ctemp);
_stprintf(ctemp, _T("%s DETECTED"), d->Name);
oldSerial = pGPS->SerialNumber;
DoStatusMessage(ctemp);
StartupStore(_T(". %s\n"),ctemp);
}
#endif
// instant vario
NMEAParser::ExtractParameter(String,ctemp,1);
pGPS->Vario = StrToDouble(ctemp,NULL)/10;
pGPS->VarioAvailable = TRUE;
// netto vario
NMEAParser::ExtractParameter(String,ctemp,3);
if (ctemp[0] != '\0') {
pGPS->NettoVario = StrToDouble(ctemp,NULL)/10;
pGPS->NettoVarioAvailable = TRUE;
} else
pGPS->NettoVarioAvailable = FALSE;
// Baro altitudes. To be verified, because I have no docs from Westerboer of any kind.
NMEAParser::ExtractParameter(String,ctemp,6);
altqne = StrToDouble(ctemp,NULL);
NMEAParser::ExtractParameter(String,ctemp,7);
altqnh = StrToDouble(ctemp,NULL);
// AutoQNH will take care of setting an average QNH if nobody does it for a while
if (initqnh) {
// if wester 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 WESTERBOER QNH %f%s"),QNH,NEWLINE);
initqnh=false;
} else {
// if locally QNH was set, either by user of by AutoQNH, stop processing QNH from Wester
if ( (QNH <= 1012) || (QNH>=1014)) initqnh=false;
// else continue entering initqnh until somebody changes qnh in either Wester or lk8000
}
}
UpdateBaroSource( pGPS, 0,d, AltitudeToQNHAltitude(altqne));
// IAS and TAS
NMEAParser::ExtractParameter(String,ctemp,8);
vias = StrToDouble(ctemp,NULL)/36;
NMEAParser::ExtractParameter(String,ctemp,9);
vtas = StrToDouble(ctemp,NULL)/36;
if (vias >1) {
pGPS->TrueAirspeed = vtas;
pGPS->IndicatedAirspeed = vias;
pGPS->AirspeedAvailable = TRUE;
} else
pGPS->AirspeedAvailable = FALSE;
// external battery voltage
NMEAParser::ExtractParameter(String,ctemp,10);
pGPS->ExtBatt1_Voltage = StrToDouble(ctemp,NULL)/10;
// OAT
NMEAParser::ExtractParameter(String,ctemp,11);
pGPS->OutsideAirTemperature = StrToDouble(ctemp,NULL)/10;
pGPS->TemperatureAvailable=TRUE;
TriggerVarioUpdate();
return TRUE;
}
static BOOL D(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS) {
/*
* 00 : vario ist in dm/sec
* 01 : pressure in cents of mB
* 02 : nettovario in dm/sec
* 03 : anemometer in km/h
* 04 : temperature in °C
* 05 : trk compass (dis) in °
* 06 : speed (dis) in km/h
* 07 : mcr equ in cm/sec
* 08 : wind speed in km/h
* 09 : goto goal in tenth of mt
* 10 : effic to ground in tenth
* 11 : effic to goal in tenth
*
* $D,+0,100554,+25,18,+31,,0,-356,+25,+11,115,96*6A
*/
TCHAR ctemp[80];
// Vario
NMEAParser::ExtractParameter(String,ctemp,0);
if (ctemp[0] != '\0') {
pGPS->Vario = StrToDouble(ctemp,NULL)/100;
pGPS->VarioAvailable = TRUE;
} else {
pGPS->VarioAvailable = FALSE;
}
// Pressure
NMEAParser::ExtractParameter(String,ctemp,1);
if (ctemp[0] != '\0') {
double abs_press = StrToDouble(ctemp,NULL);
UpdateBaroSource(pGPS, 0, d, StaticPressureToAltitude(abs_press));
}
// Netto Vario
NMEAParser::ExtractParameter(String,ctemp,2);
if (ctemp[0] != '\0') {
pGPS->NettoVario = StrToDouble(ctemp,NULL)/10;
pGPS->NettoVarioAvailable = TRUE;
} else {
pGPS->NettoVarioAvailable = FALSE;
}
// airspeed
NMEAParser::ExtractParameter(String,ctemp,3);
if (ctemp[0] != '\0') {
pGPS->TrueAirspeed = StrToDouble(ctemp,NULL) / 3600 * 1000;
pGPS->IndicatedAirspeed = pGPS->TrueAirspeed / AirDensityRatio(pGPS->Altitude);
} else {
pGPS->TrueAirspeed = 0;
pGPS->IndicatedAirspeed = 0;
}
// temperature
NMEAParser::ExtractParameter(String,ctemp,4);
if (ctemp[0] != '\0') {
pGPS->OutsideAirTemperature = StrToDouble(ctemp,NULL);
pGPS->TemperatureAvailable = TRUE;
} else {
pGPS->TemperatureAvailable = FALSE;
}
return TRUE;
}
static BOOL PDGFTL1(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS)
{
/*
$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) {
UpdateQNH(FindQNH(altqne,altqnh));
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
}
}
UpdateBaroSource( pGPS,0, d, AltitudeToQNHAltitude(altqne));
NMEAParser::ExtractParameter(String,ctemp,2);
#if 1
pGPS->Vario = StrToDouble(ctemp,NULL)/100;
#else
double newVario = StrToDouble(ctemp,NULL)/100;
pGPS->Vario = LowPassFilter(pGPS->Vario,newVario,0.1);
#endif
pGPS->VarioAvailable = TRUE;
NMEAParser::ExtractParameter(String,ctemp,3);
if (ctemp[0] != '\0') {
pGPS->NettoVario = StrToDouble(ctemp,NULL)/10; // dm/s
pGPS->NettoVarioAvailable = TRUE;
} else
pGPS->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(altqne);
pGPS->TrueAirspeed = vtas;
pGPS->IndicatedAirspeed = vias;
pGPS->AirspeedAvailable = TRUE;
}
} else {
pGPS->AirspeedAvailable = FALSE;
}
NMEAParser::ExtractParameter(String,ctemp,8);
pGPS->ExtBatt1_Voltage = StrToDouble(ctemp,NULL)/100;
NMEAParser::ExtractParameter(String,ctemp,9);
pGPS->ExtBatt2_Voltage = StrToDouble(ctemp,NULL)/100;
TriggerVarioUpdate();
return TRUE;
}
//
// Support for new 2011 Digifly TOURTELL protocol
// (Subset of TL1, pending upgrade in june 2011 for all devices)
//
static BOOL PDGFTTL(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS)
{
/*
$PDGFTTL field example
QNE 1013.25 altitude 0 2025 meters
QNH altitude 1 2000 meters
vario cm/s 2 250 2.5m/s
IAS 4 45 kmh
netto vario 3 -14 dm/s
*/
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
}
}
UpdateBaroSource( pGPS, 0,d, AltitudeToQNHAltitude(altqne));
NMEAParser::ExtractParameter(String,ctemp,2);
#if 1
pGPS->Vario = StrToDouble(ctemp,NULL)/100;
#else
double newVario = StrToDouble(ctemp,NULL)/100;
pGPS->Vario = LowPassFilter(pGPS->Vario,newVario,0.1);
#endif
pGPS->VarioAvailable = TRUE;
NMEAParser::ExtractParameter(String,ctemp,4);
if (ctemp[0] != '\0') {
pGPS->NettoVario = StrToDouble(ctemp,NULL)/10; // dm/s
pGPS->NettoVarioAvailable = TRUE;
} else
pGPS->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(pGPS->BaroAltitude);
pGPS->TrueAirspeed = vtas;
pGPS->IndicatedAirspeed = vias;
pGPS->AirspeedAvailable = TRUE;
}
} else {
pGPS->AirspeedAvailable = FALSE;
}
TriggerVarioUpdate();
return TRUE;
}
static BOOL cLXWP0(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS) {
TCHAR ctemp[80];
/*
$LXWP0,Y,222.3,1665.5,1.71,,,,,,239,174,10.1
0 loger_stored (Y/N)
1 IAS (kph) ----> Condor uses TAS!
2 baroaltitude (m)
3 vario (m/s)
4-8 unknown
9 heading of plane
10 windcourse (deg)
11 windspeed (kph)
*/
double alt, airspeed, wspeed, wfrom;
NMEAParser::ExtractParameter(String,ctemp,1);
airspeed = StrToDouble(ctemp,NULL)/TOKPH;
NMEAParser::ExtractParameter(String,ctemp,2);
alt = StrToDouble(ctemp,NULL);
pGPS->IndicatedAirspeed = airspeed/AirDensityRatio(alt);
pGPS->TrueAirspeed = airspeed;
UpdateBaroSource( pGPS, 0,d, AltitudeToQNHAltitude(alt));
NMEAParser::ExtractParameter(String,ctemp,3);
pGPS->Vario = StrToDouble(ctemp,NULL);
pGPS->AirspeedAvailable = TRUE;
pGPS->VarioAvailable = TRUE;
// we don't use heading for wind calculation since... wind is already calculated in condor!!
NMEAParser::ExtractParameter(String,ctemp,11);
wspeed=StrToDouble(ctemp,NULL);
NMEAParser::ExtractParameter(String,ctemp,10);
wfrom=StrToDouble(ctemp,NULL);
#if 1 // 120424 fix correct wind setting
wfrom+=180;
if (wfrom==360) wfrom=0;
if (wfrom>360) wfrom-=360;
wspeed/=3.6;
pGPS->ExternalWindAvailable = TRUE;
pGPS->ExternalWindSpeed = wspeed;
pGPS->ExternalWindDirection = wfrom;
#else
if (wspeed>0) {
wfrom+=180;
if (wfrom==360) wfrom=0;
if (wfrom>360) wfrom-=360;
wspeed/=3.6;
// do not update if it has not changed
if ( (wspeed!=CALCULATED_INFO.WindSpeed) || (wfrom != CALCULATED_INFO.WindBearing) ) {
SetWindEstimate(wspeed, wfrom,9);
CALCULATED_INFO.WindSpeed=wspeed;
CALCULATED_INFO.WindBearing=wfrom;
}
}
#endif
return TRUE;
}
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
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
////////////////////////////////////////////////////////////////////////////////////////////////////////
// $PCPROBE,T,Q0,Q1,Q2,Q3,ax,ay,az,temp,rh,batt,delta_press,abs_press,C,
// - "T" after "$PCPROBE" indicates that the string contains data. Data are represented as signed,
// 16-bit hexadecimal integers. The only exception is abs_press which is in signed 24-bits hex
// format.
// - Q0, Q1, Q2, Q3: 3D orientation of the C-Probe in quaternion format. Heading, pitch, and roll can
// be calculated as follows:
// q0 = Q0 * 0.001;
// q1 = Q1 * 0.001;
// q2 = Q2 * 0.001;
// q3 = Q3 * 0.001;
// sin_pitch = -2 * (q0 * q2 - q3 * q1); // if sin_pitch > 1 or sin_pitch < -1, discard the data
// pitch = asin(sin_pitch);
// heading = M_PI + atan2(2*(q1 * q2 + q3 * q0), q3 * q3 - q0 * q0 - q1 * q1 + q2 * q2);
// roll = atan2( 2 * (q0 * q1 + q3 * q2), q3 * q3 + q0 * q0 - q1 * q1 - q2 * q2);
// - ax, ay, az: x, y, z components of the acceleration in units of 0.001 g.
// - temp: temperature in units of 0.1°C.
// - rh: relative humidity in units of 0.1%.
// - batt: battery level from 0 to 100%.
// - delta_press: differential pressure (dynamic - static) in units of 0.1 Pa.
// - abs_press: absolute pressure in units of 1/400 Pa
// - C: is transmitted only if the C-Probe is being charged. In this case, heat produced by the charging
// process is likely to affect the readings of the temperature and humidity sensors.
////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOL CDevCProbe::ParseData( tnmeastring& wiss, NMEA_INFO *pINFO ) {
double q0 = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
double q1 = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
double q2 = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
double q3 = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
double sin_pitch = -2 * (q0 * q2 - q3 * q1); // if sin_pitch > 1 or sin_pitch < -1, discard the data
if(sin_pitch < 1.0 && sin_pitch > -1.0){
pINFO->MagneticHeadingAvailable=TRUE;
pINFO->MagneticHeading = (PI + atan2(2*(q1 * q2 + q3 * q0), q3 * q3 - q0 * q0 - q1 * q1 + q2 * q2))*RAD_TO_DEG;
pINFO->GyroscopeAvailable=TRUE;
pINFO->Pitch = asin(sin_pitch)*RAD_TO_DEG;
pINFO->Roll = atan2( 2 * (q0 * q1 + q3 * q2), q3 * q3 + q0 * q0 - q1 * q1 - q2 * q2)*RAD_TO_DEG;
}else{
pINFO->MagneticHeadingAvailable=FALSE;
pINFO->GyroscopeAvailable=FALSE;
}
pINFO->AccelerationAvailable=TRUE;
pINFO->AccelX = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
pINFO->AccelY = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
pINFO->AccelZ = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
pINFO->TemperatureAvailable=TRUE;
pINFO->OutsideAirTemperature = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.1;
pINFO->HumidityAvailable=TRUE;
pINFO->RelativeHumidity = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.1;
pINFO->ExtBatt1_Voltage = int16toDouble(HexStrToInt(wiss.GetNextString()))+1000;
double delta_press = int16toDouble(HexStrToInt(wiss.GetNextString())) * 1.0/10.0 ;
double abs_press = int24toDouble(HexStrToInt(wiss.GetNextString())) * (1.0/4.0);
LockDeviceData();
m_delta_press = delta_press;
m_abs_press = abs_press;
UnlockDeviceData();
// the highest sea level air pressure ever recorded was 1084 mb (32.01 in.)
// at Siberia associated with an extremely cold air mass.
if(abs_press > 0.0 && abs_press < 115000.0) {
UpdateBaroSource(pINFO, BARO__CPROBE, NULL, StaticPressureToAltitude(abs_press));
}
else {
if(pINFO->BaroAltitudeAvailable) {
abs_press = QNHAltitudeToStaticPressure(pINFO->BaroAltitude);
}
else {
abs_press = QNHAltitudeToStaticPressure(pINFO->Altitude);
}
}
if(delta_press>0.0){
pINFO->AirspeedAvailable = TRUE;
pINFO->IndicatedAirspeed = sqrt(2 * delta_press / 1.225);
pINFO->TrueAirspeed = TrueAirSpeed(delta_press, pINFO->RelativeHumidity, pINFO->OutsideAirTemperature, abs_press>0.0?abs_press:101325.0);
}
if(*(wiss.GetNextString()) == L'C'){
pINFO->ExtBatt1_Voltage *= -1;
}
TriggerVarioUpdate();
return TRUE;
}
////////////////////////////////////////////////////////////////////////////////////////////////////////
// $PCPROBE,T,Q0,Q1,Q2,Q3,ax,ay,az,temp,rh,batt,delta_press,abs_press,C,
// - "T" after "$PCPROBE" indicates that the string contains data. Data are represented as signed,
// 16-bit hexadecimal integers. The only exception is abs_press which is in signed 24-bits hex
// format.
// - Q0, Q1, Q2, Q3: 3D orientation of the C-Probe in quaternion format. Heading, pitch, and roll can
// be calculated as follows:
// q0 = Q0 * 0.001;
// q1 = Q1 * 0.001;
// q2 = Q2 * 0.001;
// q3 = Q3 * 0.001;
// sin_pitch = -2 * (q0 * q2 - q3 * q1); // if sin_pitch > 1 or sin_pitch < -1, discard the data
// pitch = asin(sin_pitch);
// heading = M_PI + atan2(2*(q1 * q2 + q3 * q0), q3 * q3 - q0 * q0 - q1 * q1 + q2 * q2);
// roll = atan2( 2 * (q0 * q1 + q3 * q2), q3 * q3 + q0 * q0 - q1 * q1 - q2 * q2);
// - ax, ay, az: x, y, z components of the acceleration in units of 0.001 g.
// - temp: temperature in units of 0.1°C.
// - rh: relative humidity in units of 0.1%.
// - batt: battery level from 0 to 100%.
// - delta_press: differential pressure (dynamic - static) in units of 0.1 Pa.
// - abs_press: absolute pressure in units of 1/400 Pa
// - C: is transmitted only if the C-Probe is being charged. In this case, heat produced by the charging
// process is likely to affect the readings of the temperature and humidity sensors.
////////////////////////////////////////////////////////////////////////////////////////////////////////
BOOL CDevCProbe::ParseData( wnmeastring& wiss, NMEA_INFO *pINFO ) {
double q0 = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
double q1 = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
double q2 = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
double q3 = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
double sin_pitch = -2 * (q0 * q2 - q3 * q1); // if sin_pitch > 1 or sin_pitch < -1, discard the data
if(sin_pitch < 1.0 && sin_pitch > -1.0){
pINFO->MagneticCompassAvailable=TRUE;
pINFO->Heading = (PI + atan2(2*(q1 * q2 + q3 * q0), q3 * q3 - q0 * q0 - q1 * q1 + q2 * q2))*RAD_TO_DEG;
pINFO->GyroscopeAvailable=TRUE;
pINFO->Pitch = asin(sin_pitch)*RAD_TO_DEG;
pINFO->Roll = atan2( 2 * (q0 * q1 + q3 * q2), q3 * q3 + q0 * q0 - q1 * q1 - q2 * q2)*RAD_TO_DEG;
}else{
pINFO->MagneticCompassAvailable=FALSE;
pINFO->GyroscopeAvailable=FALSE;
}
pINFO->AccelerationAvailable=TRUE;
pINFO->AccelX = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
pINFO->AccelY = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
pINFO->AccelZ = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.001;
pINFO->Gload = sqrt(pow(pINFO->AccelX,2.0)+pow(pINFO->AccelY,2.0)+pow(pINFO->AccelZ,2.0));
pINFO->TemperatureAvailable=TRUE;
pINFO->OutsideAirTemperature = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.1;
pINFO->HumidityAvailable=TRUE;
pINFO->RelativeHumidity = int16toDouble(HexStrToInt(wiss.GetNextString())) * 0.1;
pINFO->ExtBatt1_Voltage = int16toDouble(HexStrToInt(wiss.GetNextString()))+1000;
double delta_press = int16toDouble(HexStrToInt(wiss.GetNextString())) / 10.0 ;
double abs_press = int24toDouble(HexStrToInt(wiss.GetNextString())) / 400.0;
if(abs_press > 0.0) {
UpdateBaroSource(pINFO, BARO__CPROBE, NULL, StaticPressureToAltitude(abs_press*100));
}
else {
if(pINFO->BaroAltitudeAvailable) {
abs_press = QNHAltitudeToStaticPressure(pINFO->BaroAltitude);
}
else {
abs_press = QNHAltitudeToStaticPressure(pINFO->Altitude);
}
}
if(delta_press>=0){
pINFO->AirspeedAvailable = TRUE;
pINFO->IndicatedAirspeed = sqrt(2*delta_press);
pINFO->TrueAirspeed = TrueAirSpeed(delta_press, pINFO->RelativeHumidity, pINFO->OutsideAirTemperature, abs_press>0.0?abs_press*100:101325.0);
}
if(*(wiss.GetNextString()) == L'C'){
pINFO->ExtBatt1_Voltage *= -1;
}
LockDeviceData();
m_delta_press = delta_press;
m_abs_press = abs_press;
UnlockDeviceData();
TriggerVarioUpdate();
return TRUE;
}
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;
}
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)
{
UpdateBaroSource( GPS_INFO, FLYTEC, AltitudeToQNHAltitude( StrToDouble(ctemp, NULL)));
}
// 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;
}
