src/Instrumentation/inst_vertical_speed_indicator.cxx

   1 // inst_vertical_speed_indicator.cxx
   2 // -- Instantaneous VSI (emulation calibrated to standard atmosphere).
   3 //
   4 // Started September 2004.
   5 //
   6 // Copyright (C) 2004
   7 //
   8 // This program is free software; you can redistribute it and/or
   9 // modify it under the terms of the GNU General Public License as
  10 // published by the Free Software Foundation; either version 2 of the
  11 // License, or (at your option) any later version.
  12 //
  13 // This program is distributed in the hope that it will be useful, but
  14 // WITHOUT ANY WARRANTY; without even the implied warranty of
  15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16 // General Public License for more details.
  17 //
  18 // You should have received a copy of the GNU General Public License
  19 // along with this program; if not, write to the Free Software
  20 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  21 //
  22
  23 #include <simgear/math/interpolater.hxx>
  24
  25 #include "inst_vertical_speed_indicator.hxx"
  26 #include <Main/fg_props.hxx>
  27 #include <Main/util.hxx>
  28
  29
  30 // Altitude based on pressure difference from sea level.
  31 //
  32 // See http://www.pdas.com/programs/atmos.f90, the tables match exactly
  33 // environment.cxx (Standard 1976).
  34 // Example :
  35 // - at 27900 m the level is 20 km :
  36 //   geopotential altitude 27.9 x 6369 / ( 27.9 + 6369) = 27.778 km.
  37 // - deltah = 27.778 - 20 = 7.778 km and tbase 216.65 degK.
  38 // - delta = 5.403295E-2 x exp( -34.163195 x 7.778 / 216.65 ) = 0.016.
  39 // - pressure = (1 - delta ) x 29.92 = 29.44 inhg.
  40 // - to construct the tables, round delta to 3 digits.
  41
  42 // altitude ft, pressure step inHG
  43 static double pressure_data[][2] = {
  44  { 0.00,     3.33 },    // guess !
  45  { 2952.76,  3.05 },
  46  { 5905.51,  2.81 },
  47  { 8858.27,  2.55 },
  48  { 11811.02, 2.33 },
  49  { 14763.78, 2.13 },
  50  { 17716.54, 1.91 },
  51  { 20669.29, 1.77 },
  52  { 23622.05, 1.58 },
  53  { 26574.80, 1.49 },
  54  { 29527.56, 1.20 },
  55  { 32480.31, 1.14 },
  56  { 35433.07, 1.05 },
  57  { 38385.83, 0.90 },
  58  { 41338.58, 0.80 },
  59  { 44291.34, 0.69 },
  60  { 47244.09, 0.60 },
  61  { 50196.85, 0.51 },
  62  { 53149.61, 0.45 },
  63  { 56102.36, 0.39 },
  64  { 59055.12, 0.33 },
  65  { 62007.87, 0.30 },
  66  { 64960.63, 0.26 },
  67  { 67913.39, 0.21 },
  68  { 70866.14, 0.18 },
  69  { 73818.90, 0.18 },
  70  { 76771.65, 0.15 },
  71  { 79724.41, 0.12 },
  72  { 82677.17, 0.12 },
  73  { 85629.92, 0.09 },
  74  { 88582.68, 0.09 },
  75  { 91535.43, 0.06 },
  76  { 94488.19, 0.06 },
  77  { 97440.94, 0.06 },
  78  { 100393.70, 0.06 },
  79  { -1, -1 }
  80 };
  81
  82 // pressure difference inHG, altitude ft
  83 static double altitude_data[][2] = {
  84  {  0.00, 0.00 },
  85  {  3.05, 2952.76 },
  86  {  5.86, 5905.51 },
  87  {  8.41, 8858.27 },
  88  { 10.74, 11811.02 },
  89  { 12.87, 14763.78 },
  90  { 14.78, 17716.54 },
  91  { 16.55, 20669.29 },
  92  { 18.13, 23622.05 },
  93  { 19.62, 26574.80 },
  94  { 20.82, 29527.56 },
  95  { 21.96, 32480.31 },
  96  { 23.01, 35433.07 },
  97  { 23.91, 38385.83 },
  98  { 24.71, 41338.58 },
  99  { 25.40, 44291.34 },
 100  { 26.00, 47244.09 },
 101  { 26.51, 50196.85 },
 102  { 26.96, 53149.61 },
 103  { 27.35, 56102.36 },
 104  { 27.68, 59055.12 },
 105  { 27.98, 62007.87 },
 106  { 28.24, 64960.63 },
 107  { 28.45, 67913.39 },
 108  { 28.63, 70866.14 },
 109  { 28.81, 73818.90 },
 110  { 28.96, 76771.65 },
 111  { 29.08, 79724.41 },
 112  { 29.20, 82677.17 },
 113  { 29.29, 85629.92 },
 114  { 29.38, 88582.68 },
 115  { 29.44, 91535.43 },
 116  { 29.50, 94488.19 },
 117  { 29.56, 97440.94 },
 118  { 29.62, 100393.70 },
 119  { -1, -1 }
 120 };
 121
 122
 123 // SI constants
 124 #define SEA_LEVEL_INHG 29.92
 125
 126 // A higher number means more responsive.
 127 #define RESPONSIVENESS 5.0
 128
 129 // External environment
 130 #define MAX_INHG_PER_S 0.0002
 131
 132
 133 InstVerticalSpeedIndicator::InstVerticalSpeedIndicator ( SGPropertyNode *node ) :
 134                             _internal_pressure_inhg( SEA_LEVEL_INHG ),
 135                             _internal_sea_inhg( SEA_LEVEL_INHG ),
 136                             _speed_ft_per_s( 0 ),
 137                             _pressure_table(new SGInterpTable),
 138                             _altitude_table(new SGInterpTable)
 139 {
 140     int i;
 141     for ( i = 0; pressure_data[i][0] != -1; i++)
 142         _pressure_table->addEntry( pressure_data[i][0], pressure_data[i][1] );
 143
 144     for ( i = 0; altitude_data[i][0] != -1; i++)
 145         _altitude_table->addEntry( altitude_data[i][0], altitude_data[i][1] );
 146
 147     for ( i = 0; i < node->nChildren(); ++i ) {
 148         SGPropertyNode *child = node->getChild(i);
 149         string cname = child->getName();
 150         string cval = child->getStringValue();
 151         if ( cname == "name" ) {
 152             name = cval;
 153         } else if ( cname == "number" ) {
 154             num = child->getIntValue();
 155         } else {
 156             SG_LOG( SG_INSTR, SG_WARN, "Error in inst-vertical-speed-indicator config logic" );
 157             if ( name.length() ) {
 158                 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
 159             }
 160         }
 161     }
 162 }
 163
 164
 165 InstVerticalSpeedIndicator::~InstVerticalSpeedIndicator ()
 166 {
 167     delete _pressure_table;
 168     delete _altitude_table;
 169 }
 170
 171
 172 void InstVerticalSpeedIndicator::init ()
 173 {
 174     _serviceable_node =
 175         fgGetNode("/instrumentation/inst-vertical-speed-indicator/serviceable", true);
 176     _freeze_node =
 177         fgGetNode("/sim/freeze/master", true);
 178
 179     // A real IVSI is operated by static pressure changes.
 180     // It operates like a conventional VSI, except that an internal sensor
 181     // detects load factors, to momentarily alters the static pressure
 182     // (with lag).
 183     // It appears lag free at subsonic speed; at high altitude indication may
 184     // be less than 1/3 of actual conditions.
 185     _pressure_node =
 186         fgGetNode("/environment/pressure-inhg", true);
 187     _sea_node =
 188         fgGetNode("/environment/pressure-sea-level-inhg", true);
 189     _speed_up_node =
 190         fgGetNode("/sim/speed-up", true);
 191     _speed_node =
 192         fgGetNode("/instrumentation/inst-vertical-speed-indicator/indicated-speed-fps",
 193                   true);
 194     _speed_min_node =
 195         fgGetNode("/instrumentation/inst-vertical-speed-indicator/indicated-speed-fpm",
 196                   true);
 197
 198     // Initialize at ambient pressure
 199     _internal_pressure_inhg = _pressure_node->getDoubleValue();
 200 }
 201
 202
 203 void InstVerticalSpeedIndicator::update (double dt)
 204 {
 205     if (_serviceable_node->getBoolValue())
 206     {
 207         // avoids hang, when freeze
 208         if( !_freeze_node->getBoolValue() )
 209         {
 210             double pressure_inhg = _pressure_node->getDoubleValue();
 211             double sea_inhg = _sea_node->getDoubleValue();
 212             double speed_up = _speed_up_node->getDoubleValue();
 213
 214             // must work by speed up
 215             if( speed_up > 1 )
 216             {
 217                 dt *= speed_up;
 218             }
 219
 220             // limit effect of external environment
 221             double rate_sea_inhg_per_s = ( sea_inhg - _internal_sea_inhg ) / dt;
 222
 223             if( rate_sea_inhg_per_s > - MAX_INHG_PER_S && rate_sea_inhg_per_s < MAX_INHG_PER_S )
 224             {
 225                double rate_inhg_per_s = ( pressure_inhg - _internal_pressure_inhg ) / dt;
 226
 227                // IVSI determines alone the current altitude, without altimeter setting.
 228                // Altimeter setting is 29.92 above 10000 or 18000 ft.
 229                // Below this level, the slope is slightly wrong.
 230                double altitude_ft = _altitude_table->interpolate( SEA_LEVEL_INHG - pressure_inhg );
 231                double slope_inhg = _pressure_table->interpolate( altitude_ft );
 232
 233
 234                double last_speed_ft_per_s = _speed_ft_per_s;
 235
 236                // slope at 900 m
 237                _speed_ft_per_s = - rate_inhg_per_s * 2952.75591 / slope_inhg;
 238
 239                // filter noise
 240                _speed_ft_per_s = fgGetLowPass( last_speed_ft_per_s, _speed_ft_per_s,
 241                                                dt * RESPONSIVENESS );
 242             }
 243
 244             _speed_node->setDoubleValue( _speed_ft_per_s );
 245             _speed_min_node->setDoubleValue( _speed_ft_per_s * 60.0 );
 246
 247             // backup
 248             _internal_pressure_inhg = pressure_inhg;
 249             _internal_sea_inhg = sea_inhg;
 250         }
 251     }
 252 }
 253
 254 // end of inst_vertical_speed_indicator.cxx