예제 #1
0
void
I2CbaroDevice::onI2CbaroValues(unsigned sensor, AtmosphericPressure pressure)
{
  ScopeLock protect(device_blackboard->mutex);
  NMEAInfo &basic = device_blackboard->SetRealState(index);
  basic.UpdateClock();
  basic.alive.Update(basic.clock);

  if (pressure.IsPlausible()) {
    double param;

    // Set filter properties depending on sensor type
    if (sensor == 85 && press_use == DeviceConfig::PressureUse::STATIC_WITH_VARIO) {
       if (static_p == 0) kalman_filter.SetAccelerationVariance(0.0075);
       param = 0.05;
    } else {
       param = 0.5;
    }

    kalman_filter.Update(pressure.GetHectoPascal(), param);

    switch (press_use) {
      case DeviceConfig::PressureUse::NONE:
        break;

      case DeviceConfig::PressureUse::STATIC_ONLY:
        static_p = kalman_filter.GetXAbs();
        basic.ProvideStaticPressure(AtmosphericPressure::HectoPascal(static_p));
        break;

      case DeviceConfig::PressureUse::STATIC_WITH_VARIO:
        static_p = pressure.GetHectoPascal();
        basic.ProvideNoncompVario(ComputeNoncompVario(kalman_filter.GetXAbs(), kalman_filter.GetXVel()));
        basic.ProvideStaticPressure(AtmosphericPressure::HectoPascal(static_p));
        break;

      case DeviceConfig::PressureUse::TEK_PRESSURE:
        basic.ProvideTotalEnergyVario(ComputeNoncompVario(kalman_filter.GetXAbs(),
                                                    kalman_filter.GetXVel()));
        break;

      case DeviceConfig::PressureUse::PITOT:
        if (static_p != 0) {
          auto dyn = pressure.GetHectoPascal() - static_p - pitot_offset;
          if (dyn < 0.31)
            // suppress speeds below ~25 km/h
            dyn = 0;
          basic.ProvideDynamicPressure(AtmosphericPressure::HectoPascal(dyn));
        }
        break;

      case DeviceConfig::PressureUse::PITOT_ZERO:
        pitot_offset = kalman_filter.GetXAbs() - static_p;
        basic.ProvideSensorCalibration(1, pitot_offset);
        break;
    }
  }

  device_blackboard->ScheduleMerge();
}
예제 #2
0
static void
ComputePressure(NMEAInfo &basic, const AtmosphericPressure qnh)
{
  const bool qnh_available = qnh.IsPlausible();
  const bool static_pressure_available = basic.static_pressure_available;
  const bool pressure_altitude_available = basic.pressure_altitude_available;

  if (!static_pressure_available) {
    if (pressure_altitude_available) {
      basic.static_pressure =
        AtmosphericPressure::PressureAltitudeToStaticPressure(basic.pressure_altitude);
      basic.static_pressure_available = basic.pressure_altitude_available;
    } else if (basic.baro_altitude_available && qnh_available) {
      basic.static_pressure =
        qnh.QNHAltitudeToStaticPressure(basic.baro_altitude);
      basic.static_pressure_available = basic.baro_altitude_available;
    }
  }

  if (!pressure_altitude_available) {
    if (static_pressure_available) {
      basic.pressure_altitude =
        AtmosphericPressure::StaticPressureToPressureAltitude(basic.static_pressure);
      basic.pressure_altitude_available = basic.static_pressure_available;
    } else if (basic.baro_altitude_available && qnh_available) {
      basic.pressure_altitude =
        qnh.QNHAltitudeToPressureAltitude(basic.baro_altitude);
      basic.pressure_altitude_available = basic.baro_altitude_available;
    }
  }

  if (qnh_available) {
    /* if the current pressure and the QNH is known, then true baro
       altitude should be discarded, because the external device which
       provided it may have a different QNH setting */

    if (static_pressure_available) {
      basic.baro_altitude =
        qnh.StaticPressureToQNHAltitude(basic.static_pressure);
      basic.baro_altitude_available = basic.static_pressure_available;
    } else if (pressure_altitude_available) {
      basic.baro_altitude =
        qnh.PressureAltitudeToQNHAltitude(basic.pressure_altitude);
      basic.baro_altitude_available = basic.pressure_altitude_available;
    }
  } else if (!basic.baro_altitude_available && pressure_altitude_available)
    /* no QNH, but let's fill in the best fallback value we can get,
       without setting BaroAltitudeAvailable */
    basic.baro_altitude = basic.pressure_altitude;
}
예제 #3
0
void
I2CbaroDevice::onI2CbaroValues(unsigned sensor, AtmosphericPressure pressure)
{
  ScopeLock protect(device_blackboard->mutex);
  NMEAInfo &basic = device_blackboard->SetRealState(index);
  basic.UpdateClock();
  basic.alive.Update(basic.clock);

  if (pressure.IsPlausible()) {
    fixed param;

    // Set filter properties depending on sensor type
    if (sensor == 85 && press_use == DeviceConfig::PressureUse::STATIC_WITH_VARIO) {
       if (static_p == fixed(0)) kalman_filter.SetAccelerationVariance(fixed(0.0075));
       param = fixed(0.05);
    } else {
       param = fixed(0.5);
    }

    fixed p = pressure.GetHectoPascal();

#if 0
static FILE* fp;
static int c;
if (c == 0) {
  char path[MAX_PATH];
  LocalPath(path, _T("bmp085.trace"));
  fp = fopen(path, "w");
}
if (fp) {
  fprintf(fp, "%f,\n", p);
  if (c == 3000) {
    fclose(fp);
    fp = NULL;
  }
  c++;
}
#endif

    kalman_filter.Update(p, param);

    switch (press_use) {
      case DeviceConfig::PressureUse::NONE:
        break;

      case DeviceConfig::PressureUse::STATIC_ONLY:
        static_p = kalman_filter.GetXAbs();
        basic.ProvideStaticPressure(AtmosphericPressure::HectoPascal(static_p));
        break;

      case DeviceConfig::PressureUse::STATIC_WITH_VARIO:
        static_p = pressure.GetHectoPascal();
        basic.ProvideNoncompVario(ComputeNoncompVario(kalman_filter.GetXAbs(), kalman_filter.GetXVel()));
        basic.ProvideStaticPressure(AtmosphericPressure::HectoPascal(static_p));
        break;

      case DeviceConfig::PressureUse::TEK_PRESSURE:
        basic.ProvideTotalEnergyVario(ComputeNoncompVario(kalman_filter.GetXAbs(),
                                                    kalman_filter.GetXVel()));
        break;

      case DeviceConfig::PressureUse::PITOT:
        if (static_p != fixed(0))
        {
          fixed dyn = pressure.GetHectoPascal() - static_p - pitot_offset;
          if (dyn < fixed(0.31)) dyn = fixed(0);      // suppress speeds below ~25 km/h
          basic.ProvideDynamicPressure(AtmosphericPressure::HectoPascal(dyn));
        }
        break;

      case DeviceConfig::PressureUse::PITOT_ZERO:
        pitot_offset = kalman_filter.GetXAbs() - static_p;
        basic.ProvideSensorCalibration(fixed (1), pitot_offset);
        break;
    }
  }

  device_blackboard->ScheduleMerge();
}