From 7e6bc192bad98454187ffe2b5521d28fe6b722a4 Mon Sep 17 00:00:00 2001 From: mfranz Date: Sat, 31 Mar 2007 09:36:19 +0000 Subject: [PATCH] John DENKER: "This altimetry method is valid to above 100,000 feet, and correctly handles Kollsman settings" --- src/Environment/Makefile.am | 3 +- src/Environment/atmosphere.cxx | 213 +++++++++++++++++++++++++ src/Environment/atmosphere.hxx | 124 ++++++++++++++ src/Instrumentation/Makefile.am | 1 - src/Instrumentation/altimeter.cxx | 98 +++++------- src/Instrumentation/altimeter.hxx | 20 +-- src/Instrumentation/instrument_mgr.cxx | 3 +- src/Systems/static.cxx | 8 +- src/Systems/static.hxx | 1 + 9 files changed, 396 insertions(+), 75 deletions(-) create mode 100644 src/Environment/atmosphere.cxx create mode 100644 src/Environment/atmosphere.hxx diff --git a/src/Environment/Makefile.am b/src/Environment/Makefile.am index 3661e5041..77ce29eea 100644 --- a/src/Environment/Makefile.am +++ b/src/Environment/Makefile.am @@ -8,6 +8,7 @@ libEnvironment_a_SOURCES = \ environment.cxx environment.hxx \ environment_mgr.cxx environment_mgr.hxx \ environment_ctrl.cxx environment_ctrl.hxx \ - fgmetar.cxx fgmetar.hxx fgclouds.cxx fgclouds.hxx + fgmetar.cxx fgmetar.hxx fgclouds.cxx fgclouds.hxx \ + atmosphere.cxx atmosphere.hxx INCLUDES = -I$(top_srcdir) -I$(top_srcdir)/src diff --git a/src/Environment/atmosphere.cxx b/src/Environment/atmosphere.cxx new file mode 100644 index 000000000..70180bdfb --- /dev/null +++ b/src/Environment/atmosphere.cxx @@ -0,0 +1,213 @@ +#include "atmosphere.hxx" + +using namespace std; +#include + +FGAtmoCache::FGAtmoCache() : + a_tvs_p(0) +{} + +FGAtmoCache::~FGAtmoCache() { + delete a_tvs_p; +} + +// Pressure as a function of height. +// Valid below 32000 meters, +// i.e. troposphere and first two layers of stratosphere. +// Does not depend on any caching; can be used to +// *construct* caches and interpolation tables. +// +// Height in meters, pressure in pascals. + +double FGAtmo::p_vs_a(const double height) { + using namespace atmodel; + if (height <= 11000.) { + return P_layer(height, 0.0, ISA::P0, ISA::T0, ISA::lam0); + } else if (height <= 20000.) { + return P_layer(height, 11000., 22632.06, 216.65, 0.0); + } else if (height <= 32000.) { + return P_layer(height, 20000., 5474.89, 216.65, -0.001); + } + return 0; +} + +// degrees C, height in feet +double FGAtmo::fake_t_vs_a_us(const double h_ft) { + using namespace atmodel; + return ISA::T0 - ISA::lam0 * h_ft * foot - freezing; +} + +// Dewpoint. degrees C or K, height in feet +double FGAtmo::fake_dp_vs_a_us(const double dpsl, const double h_ft) { + const double dp_lapse(0.002); // [K/m] approximate + // Reference: http://en.wikipedia.org/wiki/Lapse_rate + return dpsl - dp_lapse * h_ft * atmodel::foot; +} + +// Height as a function of pressure. +// Valid in the troposphere only. +double FGAtmo::a_vs_p(const double press, const double qnh) { + using namespace atmodel; + using namespace ISA; + double nn = lam0 * Rgas / g / mm; + return T0 * ( pow(qnh/P0,nn) - pow(press/P0,nn) ) / lam0; +} + +// force retabulation +void FGAtmoCache::tabulate() { + using namespace atmodel; + delete a_tvs_p; + a_tvs_p = new SGInterpTable; + + for (double hgt = -1000; hgt <= 32000;) { + double press = p_vs_a(hgt); + a_tvs_p->addEntry(press / inHg, hgt / foot); + +#ifdef DEBUG_EXPORT_P_H + char buf[100]; + char* fmt = " { %9.2f , %5.0f },"; + if (press < 10000) fmt = " { %9.3f , %5.0f },"; + snprintf(buf, 100, fmt, press, hgt); + cout << buf << endl; +#endif + if (hgt < 6000) { + hgt += 500; + } else { + hgt += 1000; + } + } +} + +// make sure cache is valid +void FGAtmoCache::cache() { + if (!a_tvs_p) + tabulate(); +} + +// Pressure within a layer, as a function of height. +// Physics model: standard or nonstandard atmosphere, +// depending on what parameters you pass in. +// Height in meters, pressures in pascals. +// As always, lapse is positive in the troposphere, +// and zero in the first part of the stratosphere. + +double FGAtmo::P_layer(const double height, const double href, + const double Pref, const double Tref, + const double lapse) { + using namespace atmodel; + if (lapse) { + double N = lapse * Rgas / mm / g; + return Pref * pow( (Tref - lapse*(height - href)) / Tref , (1/N)); + } else { + return Pref * exp(-g * mm / Rgas / Tref * (height - href)); + } +} + +// Check the basic function, +// then compare against the interpolator. +void FGAtmoCache::check_model() { + double hgts[] = { + -1000, + -250, + 0, + 250, + 1000, + 5250, + 11000, + 11000.00001, + 15500, + 20000, + 20000.00001, + 25500, + 32000, + 32000.00001, + -9e99 + }; + + for (int i = 0; ; i++) { + double height = hgts[i]; + if (height < -1e6) + break; + using namespace atmodel; + cache(); + double press = p_vs_a(height); + cout << "Height: " << height + << " \tpressure: " << press << endl; + cout << "Check: " + << a_tvs_p->interpolate(press / inHg)*foot << endl; + } +} + +////////////////////////////////////////////////////////////////////// + +FGAltimeter::FGAltimeter() : kset(atmodel::ISA::P0), kft(0) +{ + cache(); +} + +double FGAltimeter::reading_ft(const double p_inHg, const double set_inHg) { + using namespace atmodel; + double press_alt = a_tvs_p->interpolate(p_inHg); + double kollsman_shift = a_tvs_p->interpolate(set_inHg); + return (press_alt - kollsman_shift); +} + +// Altimeter setting. +// Field elevation in feet +// Field pressure in inHg +// field elevation in troposphere only +double FGAtmo::qnh(const double field_ft, const double press_inHg) { + using namespace atmodel; + + // Equation derived in altimetry.htm + // exponent in QNH equation: + double nn = ISA::lam0 * Rgas / g / mm; + // pressure ratio factor: + double prat = pow(ISA::P0/inHg / press_inHg, nn); + + return press_inHg + * pow(1 + ISA::lam0 * field_ft * foot / ISA::T0 * prat, 1/nn); +} + +void FGAltimeter::dump_stack1(const double Tref) { + using namespace atmodel; + const int bs(200); + char buf[bs]; + double Psl = P_layer(0, 0, ISA::P0, Tref, ISA::lam0); + snprintf(buf, bs, "Tref: %6.2f Psl: %5.0f = %7.4f", + Tref, Psl, Psl / inHg); + cout << buf << endl; + + snprintf(buf, bs, + " %6s %6s %6s %6s %6s %6s %6s", + "A", "Aind", "Apr", "Aprind", "P", "Psl", "Qnh"); + cout << buf << endl; + + double hgts[] = {0, 2500, 5000, 7500, 10000, -9e99}; + for (int ii = 0; ; ii++) { + double hgt_ft = hgts[ii]; + if (hgt_ft < -1e6) + break; + double press = P_layer(hgt_ft*foot, 0, ISA::P0, Tref, ISA::lam0); + double p_inHg = press / inHg; + double qnhx = qnh(hgt_ft, p_inHg); + double qnh2 = round(qnhx*100)/100; + + double Aprind = reading_ft(p_inHg); + double Apr = a_vs_p(p_inHg*inHg) / foot; + double hind = reading_ft(p_inHg, qnh2); + snprintf(buf, bs, + " %6.0f %6.0f %6.0f %6.0f %6.2f %6.2f %6.2f", + hgt_ft, hind, Apr, Aprind, p_inHg, Psl/inHg, qnh2); + cout << buf << endl; + } +} + + +void FGAltimeter::dump_stack() { + using namespace atmodel; + cout << "........." << endl; + cout << "Size: " << sizeof(FGAtmo) << endl; + dump_stack1(ISA::T0); + dump_stack1(ISA::T0 - 20); +} diff --git a/src/Environment/atmosphere.hxx b/src/Environment/atmosphere.hxx new file mode 100644 index 000000000..68e64202d --- /dev/null +++ b/src/Environment/atmosphere.hxx @@ -0,0 +1,124 @@ +// atmosphere.hxx -- routines to model the air column +// +// Written by David Megginson, started February 2002. +// Modified by John Denker to correct physics errors in 2007 +// +// Copyright (C) 2002 David Megginson - david@megginson.com +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +// +// $Id$ + + +#ifndef _ATMOSPHERE_HXX +#define _ATMOSPHERE_HXX + +#include +#include + +#ifdef SG_HAVE_STD_INCLUDES +# include +#else +# include +#endif + +using namespace std; + +/** + * Model the atmosphere in a way consistent with the laws + * of physics. + * + * Each instance of this class models a particular air mass. + * You may freely move up, down, or sideways in the air mass. + * In contrast, if you want to compare different air masses, + * you should use a separate instance for each one. + * + * See also ./environment.hxx + */ + +#define SCD(name,val) const double name(val) +namespace atmodel { + SCD(g, 9.80665); // [m/s/s] acceleration of gravity + SCD(mm, .0289644); // [kg/mole] molar mass of air (dry?) + SCD(Rgas, 8.31432); // [J/K/mole] gas constant + SCD(inch, 0.0254); // [m] definition of inch + SCD(foot, 12 * inch); // [m] + SCD(inHg, 101325.0 / 760 * 1000 * inch); // [Pa] definition of inHg + SCD(freezing, 273.15); // [K] centigrade - kelvin offset + SCD(nm, 1852); // [m] nautical mile (NIST) + SCD(sm, 5280*foot); // [m] nautical mile (NIST) + + namespace ISA { + SCD(P0, 101325.0); // [pascals] ISA sea-level pressure + SCD(T0, 15. + freezing); // [K] ISA sea-level temperature + SCD(lam0, .0065); // [K/m] ISA troposphere lapse rate + } +} +#undef SCD + + + +// The base class is little more than a namespace. +// It has no constructor, no destructor, and no variables. +class FGAtmo { +public: + double p_vs_a(const double height); + double a_vs_p(const double press, const double qnh = atmodel::ISA::P0); + double fake_t_vs_a_us(const double h_ft); + double fake_dp_vs_a_us(const double dpsl, const double h_ft); + double P_layer(const double height, const double href, + const double Pref, const double Tref, + const double lapse ); + void check_one(const double height); + double qnh(const double field_ft, const double press_in); +}; + + + +class FGAtmoCache : FGAtmo { + friend class FGAltimeter; + SGInterpTable * a_tvs_p; // _tvs_ means "tabulated versus" + +public: + FGAtmoCache(); + ~FGAtmoCache(); + void tabulate(); + void cache(); + void check_model(); // debug +}; + + + +class FGAltimeter : public FGAtmoCache { + double kset; + double kft; + +public: + FGAltimeter(); + double reading_ft(const double p_inHg, + const double set_inHg = atmodel::ISA::P0/atmodel::inHg); + inline double press_alt_ft(const double p_inHg) { + return a_tvs_p->interpolate(p_inHg); + } + inline double kollsman_ft(const double set_inHg) { + return a_tvs_p->interpolate(set_inHg); + } + + // debug + void dump_stack(); + void dump_stack1(const double Tref); +}; + +#endif // _ATMOSPHERE_HXX diff --git a/src/Instrumentation/Makefile.am b/src/Instrumentation/Makefile.am index cf16083d8..b88113aa7 100644 --- a/src/Instrumentation/Makefile.am +++ b/src/Instrumentation/Makefile.am @@ -10,7 +10,6 @@ libInstrumentation_a_SOURCES = \ attitude_indicator.cxx attitude_indicator.hxx \ clock.cxx clock.hxx \ dme.cxx dme.hxx \ - encoder.cxx encoder.hxx \ gps.cxx gps.hxx \ gsdi.cxx gsdi.hxx \ gyro.cxx gyro.hxx \ diff --git a/src/Instrumentation/altimeter.cxx b/src/Instrumentation/altimeter.cxx index ebd2f6d0d..56516da2c 100644 --- a/src/Instrumentation/altimeter.cxx +++ b/src/Instrumentation/altimeter.cxx @@ -1,65 +1,38 @@ // altimeter.cxx - an altimeter tied to the static port. // Written by David Megginson, started 2002. +// Modified by John Denker in 2007 to use a two layer atmosphere +// model in src/Environment/atmosphere.?xx // // This file is in the Public Domain and comes with no warranty. +// Example invocation, in the instrumentation.xml file: +// +// encoder +// 0 +// /systems/static/pressure-inhg +// 10 +// 0 +// +// Note non-default name, quantum, and tau values. + #include -#include "altimeter.hxx" #include
#include
+#include +#include "altimeter.hxx" -// A higher number means more responsive -#define RESPONSIVENESS 10.0 - - -// Altitude based on pressure difference from sea level. -// pressure difference inHG, altitude ft -static double altitude_data[][2] = { - { -8.41, -8858.27 }, - { 0.00, 0.00 }, - { 3.05, 2952.76 }, - { 5.86, 5905.51 }, - { 8.41, 8858.27 }, - { 10.74, 11811.02 }, - { 12.87, 14763.78 }, - { 14.78, 17716.54 }, - { 16.55, 20669.29 }, - { 18.13, 23622.05 }, - { 19.62, 26574.80 }, - { 20.82, 29527.56 }, - { 21.96, 32480.31 }, - { 23.01, 35433.07 }, - { 23.91, 38385.83 }, - { 24.71, 41338.58 }, - { 25.40, 44291.34 }, - { 26.00, 47244.09 }, - { 26.51, 50196.85 }, - { 26.96, 53149.61 }, - { 27.35, 56102.36 }, - { 27.68, 59055.12 }, - { 27.98, 62007.87 }, - { 29.62, 100000.00 }, // just to fill it in - { -1, -1 } -}; - - -Altimeter::Altimeter ( SGPropertyNode *node ) +Altimeter::Altimeter ( SGPropertyNode *node, double quantum ) : _name(node->getStringValue("name", "altimeter")), _num(node->getIntValue("number", 0)), _static_pressure(node->getStringValue("static-pressure", "/systems/static/pressure-inhg")), - _altitude_table(new SGInterpTable) -{ - int i; - for (i = 0; altitude_data[i][0] != -1; i++) - _altitude_table->addEntry(altitude_data[i][0], altitude_data[i][1]); -} + _tau(node->getDoubleValue("tau", 0.1)), + _quantum(node->getDoubleValue("quantum", quantum)) +{} Altimeter::~Altimeter () -{ - delete _altitude_table; -} +{} void Altimeter::init () @@ -68,27 +41,36 @@ Altimeter::init () branch = "/instrumentation/" + _name; SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true ); + raw_PA = 0.0; + _pressure_node = fgGetNode(_static_pressure.c_str(), true); + _serviceable_node = node->getChild("serviceable", 0, true); + _setting_node = node->getChild("setting-inhg", 0, true); + _press_alt_node = node->getChild("pressure-alt-ft", 0, true); + _mode_c_node = node->getChild("mode-c-alt-ft", 0, true); + _altitude_node = node->getChild("indicated-altitude-ft", 0, true); - _serviceable_node = node->getChild("serviceable", 0, true); - _setting_node = node->getChild("setting-inhg", 0, true); - _pressure_node = fgGetNode(_static_pressure.c_str(), true); - _altitude_node = node->getChild("indicated-altitude-ft", 0, true); + if (_setting_node->getDoubleValue() == 0) + _setting_node->setDoubleValue(29.921260); } void Altimeter::update (double dt) { if (_serviceable_node->getBoolValue()) { + double trat = _tau > 0 ? dt/_tau : 100; double pressure = _pressure_node->getDoubleValue(); double setting = _setting_node->getDoubleValue(); - - // Move towards the current setting - double last_altitude = _altitude_node->getDoubleValue(); - double current_altitude = - _altitude_table->interpolate(setting - pressure); - _altitude_node->setDoubleValue(fgGetLowPass(last_altitude, - current_altitude, - dt * RESPONSIVENESS)); + double press_alt = _press_alt_node->getDoubleValue(); + // The mechanism settles slowly toward new pressure altitude: + raw_PA = fgGetLowPass(raw_PA, _altimeter.press_alt_ft(pressure), trat); + _mode_c_node->setDoubleValue(100 * round(raw_PA/100)); + _kollsman = fgGetLowPass(_kollsman, _altimeter.kollsman_ft(setting), trat); + if (_quantum) + press_alt = _quantum*round(raw_PA/_quantum); + else + press_alt = raw_PA; + _press_alt_node->setDoubleValue(press_alt); + _altitude_node->setDoubleValue(press_alt - _kollsman); } } diff --git a/src/Instrumentation/altimeter.hxx b/src/Instrumentation/altimeter.hxx index 1e4aa1202..e5baae64c 100644 --- a/src/Instrumentation/altimeter.hxx +++ b/src/Instrumentation/altimeter.hxx @@ -1,5 +1,6 @@ // altimeter.hxx - an altimeter tied to the static port. // Written by David Megginson, started 2002. +// Updated by John Denker to match changes in altimeter.cxx in 2007 // // This file is in the Public Domain and comes with no warranty. @@ -7,15 +8,9 @@ #ifndef __INSTRUMENTS_ALTIMETER_HXX #define __INSTRUMENTS_ALTIMETER_HXX 1 -#ifndef __cplusplus -# error This library requires C++ -#endif - #include #include - - -class SGInterpTable; +#include /** @@ -36,7 +31,7 @@ class Altimeter : public SGSubsystem public: - Altimeter (SGPropertyNode *node); + Altimeter (SGPropertyNode *node, double quantum = 0); virtual ~Altimeter (); virtual void init (); @@ -47,14 +42,19 @@ private: string _name; int _num; string _static_pressure; + double _tau; + double _quantum; + double _kollsman; + double raw_PA; SGPropertyNode_ptr _serviceable_node; SGPropertyNode_ptr _setting_node; SGPropertyNode_ptr _pressure_node; + SGPropertyNode_ptr _press_alt_node; + SGPropertyNode_ptr _mode_c_node; SGPropertyNode_ptr _altitude_node; - SGInterpTable * _altitude_table; - + FGAltimeter _altimeter; }; #endif // __INSTRUMENTS_ALTIMETER_HXX diff --git a/src/Instrumentation/instrument_mgr.cxx b/src/Instrumentation/instrument_mgr.cxx index 777110bce..68a3d3037 100644 --- a/src/Instrumentation/instrument_mgr.cxx +++ b/src/Instrumentation/instrument_mgr.cxx @@ -27,7 +27,6 @@ #include "attitude_indicator.hxx" #include "clock.hxx" #include "dme.hxx" -#include "encoder.hxx" #include "gps.hxx" #include "gsdi.hxx" #include "heading_indicator.hxx" @@ -121,7 +120,7 @@ bool FGInstrumentMgr::build () new DME( node ), 1.0 ); } else if ( name == "encoder" ) { set_subsystem( "instrument" + temp.str(), - new Encoder( node ) ); + new Altimeter( node ) ); } else if ( name == "gps" ) { set_subsystem( "instrument" + temp.str(), new GPS( node ), 0.45 ); diff --git a/src/Systems/static.cxx b/src/Systems/static.cxx index b3c916d80..d3cbbbf36 100644 --- a/src/Systems/static.cxx +++ b/src/Systems/static.cxx @@ -11,7 +11,9 @@ StaticSystem::StaticSystem ( SGPropertyNode *node ) : _name(node->getStringValue("name", "static")), - _num(node->getIntValue("number", 0)) + _num(node->getIntValue("number", 0)), + _tau(node->getDoubleValue("tau", 1)) + { } @@ -45,11 +47,11 @@ void StaticSystem::update (double dt) { if (_serviceable_node->getBoolValue()) { - + double trat = _tau ? dt/_tau : 100; double target = _pressure_in_node->getDoubleValue(); double current = _pressure_out_node->getDoubleValue(); // double delta = target - current; - _pressure_out_node->setDoubleValue(fgGetLowPass(current, target, dt)); + _pressure_out_node->setDoubleValue(fgGetLowPass(current, target, trat)); } } diff --git a/src/Systems/static.hxx b/src/Systems/static.hxx index f777863bd..d62ff7f33 100644 --- a/src/Systems/static.hxx +++ b/src/Systems/static.hxx @@ -48,6 +48,7 @@ private: string _name; int _num; + double _tau; SGPropertyNode_ptr _serviceable_node; SGPropertyNode_ptr _pressure_in_node; SGPropertyNode_ptr _pressure_out_node;