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