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();
}
void
Airspaces::set_flight_levels(const AtmosphericPressure &press)
{
if (press.GetHectoPascal() != m_QNH) {
m_QNH = press.GetHectoPascal();
for (auto v = airspace_tree.begin(); v != airspace_tree.end(); ++v)
v->set_flight_level(press);
}
}
static bool
test_find_qnh2()
{
AtmosphericPressure sp = AtmosphericPressure::Standard().QNHAltitudeToStaticPressure(fixed(100));
AtmosphericPressure pres =
AtmosphericPressure::FindQNHFromPressure(sp, fixed(120));
if (verbose) {
printf("%g\n",FIXED_DOUBLE(pres.GetHectoPascal()));
}
return fabs(pres.GetHectoPascal() - fixed(1015.6)) < fixed(0.1);
// example, QNH=1014, ps=100203
// alt= 100
// alt_known = 120
// qnh= 1016
}
static bool
test_find_qnh()
{
AtmosphericPressure sp = AtmosphericPressure::Standard().QNHAltitudeToStaticPressure(fixed(100));
AtmosphericPressure pres =
AtmosphericPressure::FindQNHFromPressure(sp, fixed(100));
return fabs(pres.GetHectoPascal() - fixed(1013.25)) < fixed(0.01);
}
void
Airspaces::SetFlightLevels(const AtmosphericPressure &press)
{
if ((int)press.GetHectoPascal() != (int)qnh.GetHectoPascal()) {
qnh = press;
for (auto &v : airspace_tree)
v.SetFlightLevel(press);
}
}
void
FormatPressure(TCHAR *buffer, AtmosphericPressure pressure,
Unit unit, bool include_unit)
{
fixed _pressure = Units::ToUserUnit(pressure.GetHectoPascal(), unit);
if (include_unit)
_stprintf(buffer, GetPressureFormat(unit, include_unit), (double)_pressure,
Units::GetUnitName(unit));
else
_stprintf(buffer, GetPressureFormat(unit, include_unit), (double)_pressure);
}
void
FormatPressure(TCHAR *buffer, AtmosphericPressure pressure,
Unit unit, bool include_unit)
{
auto _pressure = Units::ToUserUnit(pressure.GetHectoPascal(), unit);
if (include_unit)
StringFormatUnsafe(buffer, GetPressureFormat(unit, include_unit),
(double)_pressure,
Units::GetUnitName(unit));
else
StringFormatUnsafe(buffer, GetPressureFormat(unit, include_unit),
(double)_pressure);
}
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();
}
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();
}
bool
VegaDevice::PutQNH(const AtmosphericPressure& pres, OperationEnvironment &env)
{
volatile_data.qnh = uround(pres.GetHectoPascal() * 10);
return volatile_data.SendTo(port, env);
}
fixed
AtmosphericPressure::StaticPressureToQNHAltitude(const AtmosphericPressure ps) const
{
return (pow(GetHectoPascal(), k1) - pow(ps.GetHectoPascal(), k1)) * inv_k2;
}
/**
* Compare the QNH setting with the specified value.
*
* @return true if the current setting is the same, false if the
* value is different or if there is no value
*/
bool CompareQNH(AtmosphericPressure value) const {
return qnh_available &&
fabs(qnh.GetHectoPascal() - value.GetHectoPascal()) <= fixed(0.5);
}
fixed
Units::ToUserPressure(AtmosphericPressure value)
{
return ToUserPressure(value.GetHectoPascal());
}
