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