示例#1
0
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;
}
示例#2
0
文件: CProbe.cpp 项目: DRIZO/xcsoar
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;
}