1 // steam.cxx - Steam Gauge Calculations
3 // Copyright (C) 2000 Alexander Perry - alex.perry@ieee.org
5 // This program is free software; you can redistribute it and/or
6 // modify it under the terms of the GNU General Public License as
7 // published by the Free Software Foundation; either version 2 of the
8 // License, or (at your option) any later version.
10 // This program is distributed in the hope that it will be useful, but
11 // WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Public License for more details.
15 // You should have received a copy of the GNU General Public License
16 // along with this program; if not, write to the Free Software
17 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
26 #include <simgear/compiler.h>
30 #include <simgear/constants.h>
31 #include <simgear/math/sg_types.hxx>
32 #include <simgear/misc/props.hxx>
33 #include <Aircraft/aircraft.hxx>
34 #include <Main/bfi.hxx>
35 #include <NetworkOLK/features.hxx>
37 SG_USING_NAMESPACE(std);
39 #include "radiostack.hxx"
42 static bool isTied = false;
46 ////////////////////////////////////////////////////////////////////////
47 // Declare the functions that read the variables
48 ////////////////////////////////////////////////////////////////////////
50 // Anything that reads the BFI directly is not implemented at all!
53 double FGSteam::the_STATIC_inhg = 29.92;
54 double FGSteam::the_ALT_ft = 0.0; // Indicated altitude
55 double FGSteam::get_ALT_ft() { _CatchUp(); return the_ALT_ft; }
57 double FGSteam::the_ALT_datum_mb = 1013.0;
58 double FGSteam::get_ALT_datum_mb() { return the_ALT_datum_mb; }
60 void FGSteam::set_ALT_datum_mb ( double datum_mb ) {
61 the_ALT_datum_mb = datum_mb;
64 double FGSteam::get_ASI_kias() { return FGBFI::getAirspeed(); }
66 double FGSteam::the_VSI_case = 29.92;
67 double FGSteam::the_VSI_fps = 0.0;
68 double FGSteam::get_VSI_fps() { _CatchUp(); return the_VSI_fps; }
70 double FGSteam::the_VACUUM_inhg = 0.0;
71 double FGSteam::get_VACUUM_inhg() { _CatchUp(); return the_VACUUM_inhg; }
73 double FGSteam::the_MH_err = 0.0;
74 double FGSteam::the_MH_deg = 0.0;
75 double FGSteam::the_MH_degps = 0.0;
76 double FGSteam::get_MH_deg () { _CatchUp(); return the_MH_deg; }
78 double FGSteam::the_DG_err = 0.0;
79 double FGSteam::the_DG_deg = 0.0;
80 double FGSteam::the_DG_degps = 0.0;
81 double FGSteam::the_DG_inhg = 0.0;
82 double FGSteam::get_DG_deg () { _CatchUp(); return the_DG_deg; }
83 double FGSteam::get_DG_err () { _CatchUp(); return the_DG_err; }
85 void FGSteam::set_DG_err ( double approx_magvar ) {
86 the_DG_err = approx_magvar;
89 double FGSteam::the_TC_rad = 0.0;
90 double FGSteam::the_TC_std = 0.0;
91 double FGSteam::get_TC_rad () { _CatchUp(); return the_TC_rad; }
92 double FGSteam::get_TC_std () { _CatchUp(); return the_TC_std; }
95 ////////////////////////////////////////////////////////////////////////
96 // Recording the current time
97 ////////////////////////////////////////////////////////////////////////
100 int FGSteam::_UpdatesPending = 1000000; /* Forces filter to reset */
103 // FIXME: no need to use static
104 // functions any longer.
106 void FGSteam::update ( int timesteps )
110 fgTie("/steam/airspeed", FGSteam::get_ASI_kias);
111 fgTie("/steam/altitude", FGSteam::get_ALT_ft);
112 fgTie("/steam/altimeter-datum-mb",
113 FGSteam::get_ALT_datum_mb, FGSteam::set_ALT_datum_mb,
114 false); /* don't modify the value */
115 fgTie("/steam/turn-rate", FGSteam::get_TC_std);
116 fgTie("/steam/slip-skid", FGSteam::get_TC_rad);
117 fgTie("/steam/vertical-speed", FGSteam::get_VSI_fps);
118 fgTie("/steam/gyro-compass", FGSteam::get_DG_deg);
119 fgTie("/steam/adf", FGSteam::get_HackADF_deg);
120 fgTie("/steam/gyro-compass-error",
121 FGSteam::get_DG_err, FGSteam::set_DG_err,
122 false); /* don't modify the value */
123 fgTie("/steam/mag-compass", FGSteam::get_MH_deg);
125 _UpdatesPending += timesteps;
129 /* tc should be (elapsed_time_between_updates / desired_smoothing_time) */
130 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
134 { /* time went backwards; kill the filter */
137 { /* ignore mildly negative time */
141 { /* Normal mode of operation; fast approximation to exp(-tc) */
142 (*outthe) = (*outthe) * ( 1.0 - tc )
146 { /* Huge time step; assume filter has settled */
149 { /* Moderate time step; non linear response */
150 double keep = exp ( -tc );
151 // printf ( "ARP: Keep is %f\n", keep );
152 (*outthe) = (*outthe) * keep
153 + inthe * ( 1.0 - keep );
158 #define INHG_TO_MB 33.86388 /* Inches_of_mercury * INHG_TO_MB == millibars. */
160 // Convert air pressure to altitude by ICAO Standard Atmosphere
161 double pressInHgToAltFt(double p_inhg)
163 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
164 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
165 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
166 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
168 if (p_inhg > p_Tr) // 0.0 to 11.0 km
169 return (1.0 - pow((p_inhg / P_0), 1.0 / 5.2558797)) / 6.8755856e-6;
170 return h_Tr + log10(p_inhg / p_Tr) / -4.806346e-5; // 11.0 to 20.0 km
171 // We could put more code for higher altitudes here.
175 // Convert altitude to air pressure by ICAO Standard Atmosphere
176 double altFtToPressInHg(double alt_ft)
178 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
179 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
180 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
181 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
183 if (alt_ft < h_Tr) // 0.0 to 11.0 km
184 return P_0 * pow(1.0 - 6.8755856e-6 * alt_ft, 5.2558797);
185 return p_Tr * exp(-4.806346e-5 * (alt_ft - h_Tr)); // 11.0 to 20.0 km
186 // We could put more code for higher altitudes here.
191 ////////////////////////////////////////////////////////////////////////
192 // Here the fun really begins
193 ////////////////////////////////////////////////////////////////////////
196 void FGSteam::_CatchUp()
197 { if ( _UpdatesPending != 0 )
198 { double dt = _UpdatesPending * 1.0 /
199 fgGetInt("/sim/model-hz"); // FIXME: inefficient
200 double AccN, AccE, AccU;
202 double d, the_ENGINE_rpm;
205 /**************************
206 There is the possibility that this is the first call.
207 If this is the case, we will emit the feature registrations
208 just to be on the safe side. Doing it more than once will
209 waste CPU time but doesn't hurt anything really.
211 if ( _UpdatesPending > 999999 )
212 { FGFeature::register_int ( "Avionics/NAV1/Localizer", &NAV1_LOC );
213 FGFeature::register_double ( "Avionics/NAV1/Latitude", &NAV1_Lat );
214 FGFeature::register_double ( "Avionics/NAV1/Longitude", &NAV1_Lon );
215 FGFeature::register_double ( "Avionics/NAV1/Radial", &NAV1_Rad );
216 FGFeature::register_double ( "Avionics/NAV1/Altitude", &NAV1_Alt );
217 FGFeature::register_int ( "Avionics/NAV2/Localizer", &NAV2_LOC );
218 FGFeature::register_double ( "Avionics/NAV2/Latitude", &NAV2_Lat );
219 FGFeature::register_double ( "Avionics/NAV2/Longitude", &NAV2_Lon );
220 FGFeature::register_double ( "Avionics/NAV2/Radial", &NAV2_Rad );
221 FGFeature::register_double ( "Avionics/NAV2/Altitude", &NAV2_Alt );
222 FGFeature::register_double ( "Avionics/ADF/Latitude", &ADF_Lat );
223 FGFeature::register_double ( "Avionics/ADF/Longitude", &ADF_Lon );
227 /**************************
228 Someone has called our update function and
229 it turns out that we are running somewhat behind.
230 Here, we recalculate everything for a 'dt' second step.
233 /**************************
234 The ball responds to the acceleration vector in the body
235 frame, only the components perpendicular to the longitudinal
236 axis of the aircraft. This is only related to actual
237 side slip for a symmetrical aircraft which is not touching
238 the ground and not changing its attitude. Math simplifies
239 by assuming (for small angles) arctan(x)=x in radians.
240 Obvious failure mode is the absence of liquid in the
241 tube, which is there to damp the motion, so that instead
242 the ball will bounce around, hitting the tube ends.
243 More subtle flaw is having it not move or a travel limit
244 occasionally due to some dirt in the tube or on the ball.
246 // the_TC_rad = - ( FGBFI::getSideSlip () ); /* incorrect */
247 d = - current_aircraft.fdm_state->get_A_Z_pilot();
249 set_lowpass ( & the_TC_rad,
250 current_aircraft.fdm_state->get_A_Y_pilot () / d,
253 /**************************
254 The rate of turn indication is from an electric gyro.
255 We should have it spin up with the master switch.
256 It is mounted at a funny angle so that it detects
257 both rate of bank (i.e. rolling into and out of turns)
258 and the rate of turn (i.e. how fast heading is changing).
260 set_lowpass ( & the_TC_std,
261 current_aircraft.fdm_state->get_Phi_dot ()
262 * SGD_RADIANS_TO_DEGREES / 20.0 +
263 current_aircraft.fdm_state->get_Psi_dot ()
264 * SGD_RADIANS_TO_DEGREES / 3.0 , dt );
266 /**************************
267 We want to know the pilot accelerations,
268 to compute the magnetic compass errors.
270 AccN = current_aircraft.fdm_state->get_V_dot_north();
271 AccE = current_aircraft.fdm_state->get_V_dot_east();
272 AccU = current_aircraft.fdm_state->get_V_dot_down()
274 if ( fabs(the_TC_rad) > 0.2 )
275 { /* Massive sideslip jams it; it stops turning */
277 the_MH_err = FGBFI::getHeading () - the_MH_deg;
279 { double MagDip, MagVar, CosDip;
280 double FrcN, FrcE, FrcU, AccTot;
281 double EdgN, EdgE, EdgU;
282 double TrqN, TrqE, TrqU, Torque;
283 /* Find a force vector towards exact magnetic north */
284 MagVar = FGBFI::getMagVar() / SGD_RADIANS_TO_DEGREES;
285 MagDip = FGBFI::getMagDip() / SGD_RADIANS_TO_DEGREES;
286 CosDip = cos ( MagDip );
287 FrcN = CosDip * cos ( MagVar );
288 FrcE = CosDip * sin ( MagVar );
289 FrcU = sin ( MagDip );
290 /* Rotation occurs around acceleration axis,
291 but axis magnitude is irrelevant. So compute it. */
292 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
293 if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot );
295 /* Force applies to north marking on compass card */
296 EdgN = cos ( the_MH_err / SGD_RADIANS_TO_DEGREES );
297 EdgE = sin ( the_MH_err / SGD_RADIANS_TO_DEGREES );
299 /* Apply the force to the edge to get torques */
300 TrqN = EdgE * FrcU - EdgU * FrcE;
301 TrqE = EdgU * FrcN - EdgN * FrcU;
302 TrqU = EdgN * FrcE - EdgE * FrcN;
303 /* Select the component parallel to the axis */
304 Torque = ( TrqN * AccN +
306 TrqU * AccU ) * 5.0 / AccTot;
307 /* The magnetic compass has angular momentum,
308 so we apply a torque to it and wait */
310 { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
311 the_MH_err += dt * the_MH_degps;
313 if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else
314 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
315 the_MH_deg = FGBFI::getHeading () - the_MH_err;
318 /**************************
319 This is not actually correct, but provides a
320 scaling capability for the vacuum pump later on.
321 When we have a real engine model, we can ask it.
323 the_ENGINE_rpm = controls.get_throttle(0) * 26.0;
325 /**************************
326 First, we need to know what the static line is reporting,
327 which is a whole simulation area in itself. For now, we cheat.
328 We filter the actual value by one second to
329 account for the line impedance of the plumbing.
331 double static_inhg = altFtToPressInHg(FGBFI::getAltitude());
332 set_lowpass ( & the_STATIC_inhg, static_inhg, dt );
335 NO alternate static source error (student feature),
336 NO possibility of blockage (instructor feature),
337 NO slip-induced error, important for C172 for example.
340 /**************************
342 ICAO standard atmosphere MSL pressure is 1013.25 mb, and pressure
343 gradient is about 28 ft per mb at MSL increasing to about 32 at
344 5000 and 38 at 10000 ft.
345 Standard altimeters apply the subscale offset to the output altitude,
346 not to the input pressure; I don't know exactly what pressure gradient
347 they assume for this. I choose to make it accurate at low altitudes.
348 Remember, we are trying to simulate a real altimeter, not an ideal one.
350 set_lowpass ( & the_ALT_ft,
351 pressInHgToAltFt(the_STATIC_inhg) +
352 (the_ALT_datum_mb - 1013.25) * 28.0, /* accurate at low alt. */
353 dt * 10 ); /* smoothing time 0.1 s */
355 /**************************
356 The VSI case is a low-pass filter of the static line pressure.
357 The instrument reports the difference, scaled to approx ft.
358 NO option for student to break glass when static source fails.
359 NO capability for a fixed non-zero reading when level.
360 NO capability to have a scaling error of maybe a factor of two.
362 the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
363 * 10000.0; /* manual scaling factor */
364 set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
366 /**************************
367 The engine driven vacuum pump is directly attached
368 to the engine shaft, so each engine rotation pumps
369 a fixed volume. The amount of air in that volume
370 is determined by the vacuum line's internal pressure.
371 The instruments are essentially leaking air like
372 a fixed source impedance from atmospheric pressure.
373 The regulator provides a digital limit setting,
374 which is open circuit unless the pressure drop is big.
375 Thus, we can compute the vacuum line pressure directly.
376 We assume that there is negligible reservoir space.
377 NO failure of the pump supported (yet)
379 the_VACUUM_inhg = the_STATIC_inhg *
380 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
381 if ( the_VACUUM_inhg > 5.0 )
382 the_VACUUM_inhg = 5.0;
385 > I was merely going to do the engine rpm driven vacuum pump for both
386 > the AI and DG, have the gyros spin down down in power off descents,
387 > have it tumble when you exceed the usual pitch or bank limits,
388 > put in those insidious turning errors ... for now anyway.
390 if ( _UpdatesPending > 999999 )
391 the_DG_err = FGBFI::getMagVar();
392 the_DG_degps = 0.01; /* HACK! */
393 if (dt<1.0) the_DG_err += dt * the_DG_degps;
394 the_DG_deg = FGBFI::getHeading () - the_DG_err;
396 /**************************
397 Finished updates, now clear the timer
401 // cout << "0 Updates pending" << endl;
406 ////////////////////////////////////////////////////////////////////////
407 // Everything below is a transient hack; expect it to disappear
408 ////////////////////////////////////////////////////////////////////////
413 double FGSteam::get_HackGS_deg () {
414 if ( current_radiostack->get_nav1_inrange() &&
415 current_radiostack->get_nav1_has_gs() )
417 double x = current_radiostack->get_nav1_gs_dist();
418 double y = (FGBFI::getAltitude() - current_radiostack->get_nav1_elev())
420 double angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
421 return (current_radiostack->get_nav1_target_gs() - angle) * 5.0;
428 double FGSteam::get_HackVOR1_deg () {
431 if ( current_radiostack->get_nav1_inrange() ) {
432 r = current_radiostack->get_nav1_heading()
433 - current_radiostack->get_nav1_radial();
434 // cout << "Radial = " << current_radiostack->get_nav1_radial()
435 // << " Bearing = " << current_radiostack->get_nav1_heading()
438 if (r> 180.0) r-=360.0; else
439 if (r<-180.0) r+=360.0;
440 if ( fabs(r) > 90.0 )
441 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
442 // According to Robin Peel, the ILS is 4x more sensitive than a vor
443 if ( current_radiostack->get_nav1_loc() ) r *= 4.0;
452 double FGSteam::get_HackVOR2_deg () {
455 if ( current_radiostack->get_nav2_inrange() ) {
456 r = current_radiostack->get_nav2_heading()
457 - current_radiostack->get_nav2_radial();
458 // cout << "Radial = " << current_radiostack->get_nav1_radial()
459 // << " Bearing = " << current_radiostack->get_nav1_heading() << endl;
461 if (r> 180.0) r-=360.0; else
462 if (r<-180.0) r+=360.0;
463 if ( fabs(r) > 90.0 )
464 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
474 double FGSteam::get_HackOBS1_deg () {
475 return current_radiostack->get_nav1_radial();
479 double FGSteam::get_HackOBS2_deg () {
480 return current_radiostack->get_nav2_radial();
484 double FGSteam::get_HackADF_deg () {
485 static double last_r = 0;
488 if ( current_radiostack->get_adf_inrange() ) {
489 double r = current_radiostack->get_adf_heading() - FGBFI::getHeading();
491 // cout << "Radial = " << current_radiostack->get_adf_heading()
492 // << " Heading = " << FGBFI::getHeading() << endl;