]> git.mxchange.org Git - flightgear.git/blob - src/Instrumentation/inst_vertical_speed_indicator.cxx
Merge branch 'durk/ai-atc'
[flightgear.git] / 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 <limits>
24 #include <simgear/math/interpolater.hxx>
25
26 #include "inst_vertical_speed_indicator.hxx"
27 #include <Main/fg_props.hxx>
28 #include <Main/util.hxx>
29
30
31 // Altitude based on pressure difference from sea level.
32 //
33 // See http://www.pdas.com/programs/atmos.f90, the tables match exactly
34 // environment.cxx (Standard 1976).
35 // Example :
36 // - at 27900 m the level is 20 km :
37 //   geopotential altitude 27.9 x 6369 / ( 27.9 + 6369) = 27.778 km.
38 // - deltah = 27.778 - 20 = 7.778 km and tbase 216.65 degK.
39 // - delta = 5.403295E-2 x exp( -34.163195 x 7.778 / 216.65 ) = 0.016.
40 // - pressure = (1 - delta ) x 29.92 = 29.44 inhg.
41 // - to construct the tables, round delta to 3 digits.
42
43 // altitude ft, pressure step inHG
44 static double pressure_data[][2] = {
45  { 0.00,     3.33 },    // guess !
46  { 2952.76,  3.05 },
47  { 5905.51,  2.81 },
48  { 8858.27,  2.55 },
49  { 11811.02, 2.33 },
50  { 14763.78, 2.13 },
51  { 17716.54, 1.91 },
52  { 20669.29, 1.77 },
53  { 23622.05, 1.58 },
54  { 26574.80, 1.49 },
55  { 29527.56, 1.20 },
56  { 32480.31, 1.14 },
57  { 35433.07, 1.05 },
58  { 38385.83, 0.90 },
59  { 41338.58, 0.80 },
60  { 44291.34, 0.69 },
61  { 47244.09, 0.60 },
62  { 50196.85, 0.51 },
63  { 53149.61, 0.45 },
64  { 56102.36, 0.39 },
65  { 59055.12, 0.33 },
66  { 62007.87, 0.30 },
67  { 64960.63, 0.26 },
68  { 67913.39, 0.21 },
69  { 70866.14, 0.18 },
70  { 73818.90, 0.18 },
71  { 76771.65, 0.15 },
72  { 79724.41, 0.12 },
73  { 82677.17, 0.12 },
74  { 85629.92, 0.09 },
75  { 88582.68, 0.09 },
76  { 91535.43, 0.06 },
77  { 94488.19, 0.06 },
78  { 97440.94, 0.06 },
79  { 100393.70, 0.06 },
80  { -1, -1 }
81 };
82
83 // pressure difference inHG, altitude ft
84 static double altitude_data[][2] = {
85  {  0.00, 0.00 },
86  {  3.05, 2952.76 },
87  {  5.86, 5905.51 },
88  {  8.41, 8858.27 },
89  { 10.74, 11811.02 },
90  { 12.87, 14763.78 },
91  { 14.78, 17716.54 },
92  { 16.55, 20669.29 },
93  { 18.13, 23622.05 },
94  { 19.62, 26574.80 },
95  { 20.82, 29527.56 },
96  { 21.96, 32480.31 },
97  { 23.01, 35433.07 },
98  { 23.91, 38385.83 },
99  { 24.71, 41338.58 },
100  { 25.40, 44291.34 },
101  { 26.00, 47244.09 },
102  { 26.51, 50196.85 },
103  { 26.96, 53149.61 },
104  { 27.35, 56102.36 },
105  { 27.68, 59055.12 },
106  { 27.98, 62007.87 },
107  { 28.24, 64960.63 },
108  { 28.45, 67913.39 },
109  { 28.63, 70866.14 },
110  { 28.81, 73818.90 },
111  { 28.96, 76771.65 },
112  { 29.08, 79724.41 },
113  { 29.20, 82677.17 },
114  { 29.29, 85629.92 },
115  { 29.38, 88582.68 },
116  { 29.44, 91535.43 },
117  { 29.50, 94488.19 },
118  { 29.56, 97440.94 },
119  { 29.62, 100393.70 },
120  { -1, -1 }
121 };
122
123
124 // SI constants
125 #define SEA_LEVEL_INHG 29.92
126
127 // A higher number means more responsive.
128 #define RESPONSIVENESS 5.0
129
130 // External environment
131 #define MAX_INHG_PER_S 0.0002
132
133
134 InstVerticalSpeedIndicator::InstVerticalSpeedIndicator ( SGPropertyNode *node ) :
135                             _name(node->getStringValue("name", "inst-vertical-speed-indicator")),
136                             _num(node->getIntValue("number", 0)),
137                             _internal_pressure_inhg( SEA_LEVEL_INHG ),
138                             _internal_sea_inhg( SEA_LEVEL_INHG ),
139                             _speed_ft_per_s( 0 ),
140                             _pressure_table(new SGInterpTable),
141                             _altitude_table(new SGInterpTable)
142 {
143     int i;
144     for ( i = 0; pressure_data[i][0] != -1; i++)
145         _pressure_table->addEntry( pressure_data[i][0], pressure_data[i][1] );
146
147     for ( i = 0; altitude_data[i][0] != -1; i++)
148         _altitude_table->addEntry( altitude_data[i][0], altitude_data[i][1] );
149 }
150
151
152 InstVerticalSpeedIndicator::~InstVerticalSpeedIndicator ()
153 {
154     delete _pressure_table;
155     delete _altitude_table;
156 }
157
158
159 void InstVerticalSpeedIndicator::init ()
160 {
161     SGPropertyNode *node = fgGetNode("/instrumentation", true)->getChild(_name, _num, true);
162
163     _serviceable_node =
164         node->getNode("serviceable", true);
165     _freeze_node =
166         fgGetNode("/sim/freeze/master", true);
167
168     // A real IVSI is operated by static pressure changes.
169     // It operates like a conventional VSI, except that an internal sensor
170     // detects load factors, to momentarily alters the static pressure
171     // (with lag).
172     // It appears lag free at subsonic speed; at high altitude indication may
173     // be less than 1/3 of actual conditions.
174     _pressure_node =
175         fgGetNode("/environment/pressure-inhg", true);
176     _sea_node =
177         fgGetNode("/environment/pressure-sea-level-inhg", true);
178     _speed_up_node =
179         fgGetNode("/sim/speed-up", true);
180     _speed_node =
181         node->getNode("indicated-speed-fps", true);
182     _speed_min_node =
183         node->getNode("indicated-speed-fpm", true);
184
185     // Initialize at ambient pressure
186     _internal_pressure_inhg = _pressure_node->getDoubleValue();
187 }
188
189
190 void InstVerticalSpeedIndicator::update (double dt)
191 {
192     if (_serviceable_node->getBoolValue())
193     {
194         // avoids hang, when freeze
195         if( !_freeze_node->getBoolValue() && std::numeric_limits<double>::min() < fabs(dt))
196         {
197             double pressure_inhg = _pressure_node->getDoubleValue();
198             double sea_inhg = _sea_node->getDoubleValue();
199             double speed_up = _speed_up_node->getDoubleValue();
200             
201             // must work by speed up
202             if( speed_up > 1 )
203             {
204                 dt *= speed_up;
205             }
206               
207             // limit effect of external environment
208             double rate_sea_inhg_per_s = ( sea_inhg - _internal_sea_inhg ) / dt;
209             
210             if( rate_sea_inhg_per_s > - MAX_INHG_PER_S && rate_sea_inhg_per_s < MAX_INHG_PER_S )
211             {
212                double rate_inhg_per_s = ( pressure_inhg - _internal_pressure_inhg ) / dt;
213
214                // IVSI determines alone the current altitude, without altimeter setting.
215                // Altimeter setting is 29.92 above 10000 or 18000 ft.
216                // Below this level, the slope is slightly wrong.
217                double altitude_ft = _altitude_table->interpolate( SEA_LEVEL_INHG - pressure_inhg );
218                double slope_inhg = _pressure_table->interpolate( altitude_ft );
219
220
221                double last_speed_ft_per_s = _speed_ft_per_s;
222
223                // slope at 900 m
224                _speed_ft_per_s = - rate_inhg_per_s * 2952.75591 / slope_inhg;
225
226                // filter noise
227                _speed_ft_per_s = fgGetLowPass( last_speed_ft_per_s, _speed_ft_per_s,
228                                                dt * RESPONSIVENESS );
229             }
230             
231             _speed_node->setDoubleValue( _speed_ft_per_s );
232             _speed_min_node->setDoubleValue( _speed_ft_per_s * 60.0 );
233
234             // backup
235             _internal_pressure_inhg = pressure_inhg;
236             _internal_sea_inhg = sea_inhg;
237         }
238     }
239 }
240
241 // end of inst_vertical_speed_indicator.cxx