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();
}
bool
BlueFlyDevice::ParsePRS(const char *content, NMEAInfo &info)
{
// e.g. PRS 17CBA
char *endptr;
long value = strtol(content, &endptr, 16);
if (endptr != content) {
AtmosphericPressure pressure = AtmosphericPressure::Pascal(fixed(value));
kalman_filter.Update(pressure.GetHectoPascal(), fixed(0.25), fixed(0.02));
info.ProvideNoncompVario(ComputeNoncompVario(kalman_filter.GetXAbs(),
kalman_filter.GetXVel()));
info.ProvideStaticPressure(AtmosphericPressure::HectoPascal(kalman_filter.GetXAbs()));
}
return true;
}
void
BMP085Device::onBMP085Values(fixed temperature,
AtmosphericPressure pressure)
{
ScopeLock protect(device_blackboard->mutex);
NMEAInfo &basic = device_blackboard->SetRealState(index);
basic.UpdateClock();
basic.alive.Update(basic.clock);
#ifdef USE_TEMPERATURE
basic.temperature = temperature;
basic.temperature_available = true;
#endif
kalman_filter.Update(pressure.GetHectoPascal(), fixed(0.05));
basic.ProvideNoncompVario(ComputeNoncompVario(kalman_filter.GetXAbs(),
kalman_filter.GetXVel()));
basic.ProvideStaticPressure(AtmosphericPressure::HectoPascal(kalman_filter.GetXAbs()));
device_blackboard->ScheduleMerge();
}
gcc_visibility_default
JNIEXPORT void JNICALL
Java_org_xcsoar_NonGPSSensors_setBarometricPressure(
JNIEnv* env, jobject obj, jfloat pressure, jfloat sensor_noise_variance) {
/* We use a Kalman filter to smooth Android device pressure sensor
noise. The filter requires two parameters: the first is the
variance of the distribution of second derivatives of pressure
values that we expect to see in flight, and the second is the
maximum time between pressure sensor updates in seconds before
the filter gives up on smoothing and uses the raw value.
The pressure acceleration variance used here is actually wider
than the maximum likelihood variance observed in the data: it
turns out that the distribution is more heavy-tailed than a
normal distribution, probably because glider pilots usually
experience fairly constant pressure change most of the time. */
static constexpr fixed KF_VAR_ACCEL(0.0075);
static constexpr fixed KF_MAX_DT(60);
// XXX this shouldn't be a global variable
static SelfTimingKalmanFilter1d kalman_filter(KF_MAX_DT, KF_VAR_ACCEL);
const unsigned int index = getDeviceIndex(env, obj);
ScopeLock protect(device_blackboard->mutex);
/* Kalman filter updates are also protected by the blackboard
mutex. These should not take long; we won't hog the mutex
unduly. */
kalman_filter.Update(fixed(pressure), fixed(sensor_noise_variance));
NMEAInfo &basic = device_blackboard->SetRealState(index);
basic.ProvideNoncompVario(ComputeNoncompVario(kalman_filter.GetXAbs(),
kalman_filter.GetXVel()));
basic.ProvideStaticPressure(
AtmosphericPressure::HectoPascal(kalman_filter.GetXAbs()));
device_blackboard->ScheduleMerge();
}
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();
}
