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;
}
// Converts altitude with QNH=1013.25 reference to QNH adjusted altitude
double AltitudeToQNHAltitude(double alt) {
// avoid overflow of double
if (alt>44330) {
alt=44330;
}
const double k1=0.190263;
double ps = pow((44330.8-alt)/4946.54,1.0/k1);
return StaticPressureToAltitude(ps);
}
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;
}
// Converts altitude with QNH=1013.25 reference to QNH adjusted altitude
double AltitudeToQNHAltitude(double alt) {
const double k1=0.190263;
double ps = pow((44330.8-alt)/4946.54,1.0/k1);
return StaticPressureToAltitude(ps);
}
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;
}
////////////////////////////////////////////////////////////////////////////////////////////////////////
// $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;
}
