bool OpenVarioDevice::POV(NMEAInputLine &line, NMEAInfo &info) { /* * Type definitions: * * E: TE vario in m/s * P: static pressure in hPa * Q: dynamic pressure in Pa * R: total pressure in hPa * S: true airspeed in km/h * T: temperature in deg C */ while (!line.IsEmpty()) { char type = line.ReadOneChar(); if (type == '\0') break; fixed value; if (!line.ReadChecked(value)) break; switch (type) { case 'E': { info.ProvideTotalEnergyVario(value); break; } case 'P': { AtmosphericPressure pressure = AtmosphericPressure::HectoPascal(value); info.ProvideStaticPressure(pressure); break; } case 'Q': { AtmosphericPressure pressure = AtmosphericPressure::Pascal(value); info.ProvideDynamicPressure(pressure); break; } case 'R': { AtmosphericPressure pressure = AtmosphericPressure::HectoPascal(value); info.ProvidePitotPressure(pressure); break; } case 'S': { value = Units::ToSysUnit(value, Unit::KILOMETER_PER_HOUR); info.ProvideTrueAirspeed(value); break; } case 'T': { info.temperature = CelsiusToKelvin(value); info.temperature_available = true; break; } } } return true; }
static bool ParseData(NMEAInputLine &line, NMEAInfo &info) { // $PCPROBE,T,Q0,Q1,Q2,Q3,ax,ay,az,temp,rh,batt,delta_press,abs_press,C, // see http://www.compassitaly.com/CMS/index.php/en/download/c-probe/231-c-probeusermanual/download unsigned _q[4]; bool q_available = line.ReadHexChecked(_q[0]); if (!line.ReadHexChecked(_q[1])) q_available = false; if (!line.ReadHexChecked(_q[2])) q_available = false; if (!line.ReadHexChecked(_q[3])) q_available = false; if (q_available) { fixed q[4]; for (unsigned i = 0; i < 4; ++i) // Cast to int16_t to interpret the 16th bit as the sign bit q[i] = fixed((int16_t)_q[i]) / 1000; fixed sin_pitch = -2 * (q[0] * q[2] - q[3] * q[1]); if (sin_pitch <= fixed(1) && sin_pitch >= fixed(-1)) { info.attitude.pitch_angle_available = true; info.attitude.pitch_angle = Angle::asin(sin_pitch); Angle heading = Angle::HalfCircle() + Angle::FromXY(sqr(q[3]) - sqr(q[0]) - sqr(q[1]) + sqr(q[2]), Double(q[1] * q[2] + q[3] * q[0])); info.attitude.heading_available.Update(info.clock); info.attitude.heading = heading; Angle roll = Angle::FromXY(sqr(q[3]) + sqr(q[0]) - sqr(q[1]) - sqr(q[2]), Double(q[0] * q[1] + q[3] * q[2])); info.attitude.bank_angle_available = true; info.attitude.bank_angle = roll; } } unsigned _a[3]; bool a_available = line.ReadHexChecked(_a[0]); if (!line.ReadHexChecked(_a[1])) a_available = false; if (!line.ReadHexChecked(_a[2])) a_available = false; if (a_available) { fixed a[3]; for (unsigned i = 0; i < 3; ++i) // Cast to int16_t to interpret the 16th bit as the sign bit a[i] = fixed((int16_t)_a[i]) / 1000; info.acceleration.ProvideGLoad(sqrt(sqr(a[0]) + sqr(a[1]) + sqr(a[2])), true); } unsigned temperature; if (line.ReadHexChecked(temperature)) { info.temperature_available = true; info.temperature = CelsiusToKelvin(fixed((int16_t)temperature) / 10); } unsigned humidity; if (line.ReadHexChecked(humidity)) { info.humidity_available = true; info.humidity = fixed((int16_t)humidity) / 10; } unsigned battery_level; if (line.ReadHexChecked(battery_level)) { info.battery_level_available.Update(info.clock); info.battery_level = fixed((int16_t)battery_level); } unsigned _delta_pressure; if (line.ReadHexChecked(_delta_pressure)) { fixed delta_pressure = fixed((int16_t)_delta_pressure) / 10; info.ProvideDynamicPressure(AtmosphericPressure::Pascal(delta_pressure)); } return true; }