static bool
test_find_qnh()
{
AtmosphericPressure pres;
pres.FindQNH(fixed(100), fixed(100));
return fabs(pres.get_QNH()-fixed(1013.25))<fixed(0.01);
}
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 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::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_qnh_to_static()
{
AtmosphericPressure pres = AtmosphericPressure::Standard();
fixed p0 = pres.QNHAltitudeToStaticPressure(fixed(0)).GetPascal();
if (verbose) {
printf("%g\n", double(p0));
}
return fabs(p0-fixed(101325))<fixed(0.1);
}
static bool
test_qnh_to_static()
{
AtmosphericPressure pres;
fixed p0 = pres.QNHAltitudeToStaticPressure(fixed_zero);
if (verbose) {
printf("%g\n",FIXED_DOUBLE(p0));
}
return fabs(p0-fixed(101325))<fixed(0.1);
}
static bool
test_isa_pressure(const fixed alt, const fixed prat)
{
const AtmosphericPressure pres = AtmosphericPressure::Standard();
fixed p0 = pres.QNHAltitudeToStaticPressure(alt).GetPascal();
if (verbose) {
printf("%g\n", double(p0));
}
return fabs(p0/fixed(101325)-prat)<fixed(0.001);
}
static bool
test_isa_pressure(const fixed alt, const fixed prat)
{
AtmosphericPressure pres;
fixed p0 = pres.QNHAltitudeToStaticPressure(alt);
if (verbose) {
printf("%g\n",FIXED_DOUBLE(p0));
}
return fabs(p0/fixed(101325)-prat)<fixed(0.001);
}
static bool
test_qnh_round()
{
AtmosphericPressure pres;
pres.FindQNH(fixed(100), fixed(120));
fixed h0 = pres.AltitudeToQNHAltitude(fixed(100));
if (verbose) {
printf("%g\n",FIXED_DOUBLE(h0));
}
return fabs(h0-fixed(120))<fixed(1);
}
static bool
test_qnh_round()
{
AtmosphericPressure sp = AtmosphericPressure::Standard().QNHAltitudeToStaticPressure(fixed(100));
AtmosphericPressure pres =
AtmosphericPressure::FindQNHFromPressure(sp, fixed(120));
fixed h0 = pres.PressureAltitudeToQNHAltitude(fixed(100));
if (verbose) {
printf("%g\n", double(h0));
}
return fabs(h0-fixed(120))<fixed(1);
}
static bool
test_qnh_round2()
{
AtmosphericPressure sp = AtmosphericPressure::Standard().QNHAltitudeToStaticPressure(fixed(100));
AtmosphericPressure pres =
AtmosphericPressure::FindQNHFromPressure(sp, fixed(120));
fixed h0 = pres.StaticPressureToQNHAltitude(pres);
if (verbose) {
printf("%g %g\n", FIXED_DOUBLE(pres.GetPascal()), FIXED_DOUBLE(h0));
}
return fabs(h0)<fixed(1);
}
void
Airspaces::set_flight_levels(const AtmosphericPressure &press)
{
if (press.get_QNH() != m_QNH) {
m_QNH = press.get_QNH();
for (AirspaceTree::iterator v = airspace_tree.begin();
v != airspace_tree.end(); ++v) {
v->set_flight_level(press);
}
}
}
static bool
test_qnh_round2()
{
AtmosphericPressure pres;
pres.FindQNH(fixed(100), fixed(120));
fixed p0 = pres.get_QNH()*100;
fixed h0 = pres.StaticPressureToQNHAltitude(p0);
if (verbose) {
printf("%g %g\n",FIXED_DOUBLE(p0),FIXED_DOUBLE(h0));
}
return fabs(h0)<fixed(1);
}
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;
}
static bool
test_find_qnh2()
{
AtmosphericPressure pres;
pres.FindQNH(fixed(100), fixed(120));
if (verbose) {
printf("%g\n",FIXED_DOUBLE(pres.get_QNH()));
}
return fabs(pres.get_QNH()-fixed(1015.6))<fixed(0.1);
// example, QNH=1014, ps=100203
// alt= 100
// alt_known = 120
// qnh= 1016
}
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
}
void
AirspaceAltitude::SetFlightLevel(const AtmosphericPressure &press)
{
static constexpr fixed fl_feet_to_m(30.48);
if (reference == AltitudeReference::STD)
altitude = press.PressureAltitudeToQNHAltitude(flight_level * fl_feet_to_m);
}
void
AIRSPACE_ALT::set_flight_level(const AtmosphericPressure &press)
{
static const fixed fl_feet_to_m(30.48);
if (Base == abFL)
Altitude = press.AltitudeToQNHAltitude(FL * fl_feet_to_m);
}
/**
* Set the QNH setting of the V7 vario
*/
static inline bool
SetQNH(Port &port, OperationEnvironment &env, const AtmosphericPressure &qnh)
{
char buffer[100];
unsigned QNHinPascal = uround(qnh.GetPascal());
sprintf(buffer, "PLXV0,QNH,W,%u", QNHinPascal);
return PortWriteNMEA(port, buffer, env);
}
bool
LXDevice::PutQNH(const AtmosphericPressure &pres)
{
fixed altitude_offset =
pres.StaticPressureToQNHAltitude(AtmosphericPressure::Standard());
char buffer[100];
sprintf(buffer, "PFLX3,%.2f,,,,,,,,,,,,", (double)altitude_offset / 0.3048);
PortWriteNMEA(port, buffer);
return true;
}
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
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
LXDevice::PutQNH(const AtmosphericPressure &pres, OperationEnvironment &env)
{
if (!EnableNMEA(env))
return false;
double altitude_offset =
(double)pres.StaticPressureToQNHAltitude(AtmosphericPressure::Standard()) / 0.3048;
char buffer[100];
if (IsV7())
sprintf(buffer, "PLXV0,QNH,W,%.2f", altitude_offset);
else
sprintf(buffer, "PFLX3,%.2f,,,,,,,,,,,,", altitude_offset);
return PortWriteNMEA(port, buffer, env);
}
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();
}
fixed
AtmosphericPressure::StaticPressureToQNHAltitude(const AtmosphericPressure ps) const
{
return (pow(GetHectoPascal(), k1) - pow(ps.GetHectoPascal(), k1)) * inv_k2;
}
bool
VegaDevice::PutQNH(const AtmosphericPressure& pres, OperationEnvironment &env)
{
volatile_data.qnh = uround(pres.GetHectoPascal() * 10);
return volatile_data.SendTo(port, env);
}
AtmosphericPressure
AtmosphericPressure::FindQNHFromPressure(const AtmosphericPressure pressure,
const fixed alt_known)
{
return pressure.QNHAltitudeToStaticPressure(-alt_known);
}
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();
}
