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>
35 SG_USING_NAMESPACE(std);
37 #include "radiostack.hxx"
40 static bool isTied = false;
44 ////////////////////////////////////////////////////////////////////////
45 // Declare the functions that read the variables
46 ////////////////////////////////////////////////////////////////////////
48 double FGSteam::the_STATIC_inhg = 29.92;
49 double FGSteam::the_ALT_ft = 0.0; // Indicated altitude
50 double FGSteam::get_ALT_ft() { _CatchUp(); return the_ALT_ft; }
52 double FGSteam::the_ALT_datum_mb = 1013.0;
53 double FGSteam::get_ALT_datum_mb() { return the_ALT_datum_mb; }
55 void FGSteam::set_ALT_datum_mb ( double datum_mb ) {
56 the_ALT_datum_mb = datum_mb;
59 double FGSteam::get_ASI_kias() { return fgGetDouble("/velocities/airspeed-kt"); }
61 double FGSteam::the_VSI_case = 29.92;
62 double FGSteam::the_VSI_fps = 0.0;
63 double FGSteam::get_VSI_fps() { _CatchUp(); return the_VSI_fps; }
65 double FGSteam::the_VACUUM_inhg = 0.0;
66 double FGSteam::get_VACUUM_inhg() { _CatchUp(); return the_VACUUM_inhg; }
68 double FGSteam::the_MH_err = 0.0;
69 double FGSteam::the_MH_deg = 0.0;
70 double FGSteam::the_MH_degps = 0.0;
71 double FGSteam::get_MH_deg () { _CatchUp(); return the_MH_deg; }
73 double FGSteam::the_DG_err = 0.0;
74 double FGSteam::the_DG_deg = 0.0;
75 double FGSteam::the_DG_degps = 0.0;
76 double FGSteam::the_DG_inhg = 0.0;
77 double FGSteam::get_DG_deg () { _CatchUp(); return the_DG_deg; }
78 double FGSteam::get_DG_err () { _CatchUp(); return the_DG_err; }
80 void FGSteam::set_DG_err ( double approx_magvar ) {
81 the_DG_err = approx_magvar;
84 double FGSteam::the_TC_rad = 0.0;
85 double FGSteam::the_TC_std = 0.0;
86 double FGSteam::get_TC_rad () { _CatchUp(); return the_TC_rad; }
87 double FGSteam::get_TC_std () { _CatchUp(); return the_TC_std; }
90 ////////////////////////////////////////////////////////////////////////
91 // Recording the current time
92 ////////////////////////////////////////////////////////////////////////
95 int FGSteam::_UpdatesPending = 1000000; /* Forces filter to reset */
98 // FIXME: no need to use static
99 // functions any longer.
101 void FGSteam::update ( int timesteps )
105 fgTie("/steam/airspeed-kt", FGSteam::get_ASI_kias);
106 fgSetArchivable("/steam/airspeed-kt");
107 fgTie("/steam/altitude-ft", FGSteam::get_ALT_ft);
108 fgSetArchivable("/steam/altitude-ft");
109 fgTie("/steam/altimeter-datum-mb",
110 FGSteam::get_ALT_datum_mb, FGSteam::set_ALT_datum_mb,
111 false); /* don't modify the value */
112 fgSetArchivable("/steam/altimeter-datum-mb");
113 fgTie("/steam/turn-rate", FGSteam::get_TC_std);
114 fgSetArchivable("/steam/turn-rate");
115 fgTie("/steam/slip-skid", FGSteam::get_TC_rad);
116 fgSetArchivable("/steam/slip-skid");
117 fgTie("/steam/vertical-speed-fpm", FGSteam::get_VSI_fps);
118 fgSetArchivable("/steam/vertical-speed-fpm");
119 fgTie("/steam/gyro-compass-deg", FGSteam::get_DG_deg);
120 fgSetArchivable("/steam/gyro-compass-deg");
121 fgTie("/steam/adf-deg", FGSteam::get_HackADF_deg);
122 fgSetArchivable("/steam/adf-deg");
123 fgTie("/steam/gyro-compass-error-deg",
124 FGSteam::get_DG_err, FGSteam::set_DG_err,
125 false); /* don't modify the value */
126 fgSetArchivable("/steam/gyro-compass-error-deg");
127 fgTie("/steam/mag-compass-deg", FGSteam::get_MH_deg);
128 fgSetArchivable("/steam/mag-compass-deg");
130 _UpdatesPending += timesteps;
134 /* tc should be (elapsed_time_between_updates / desired_smoothing_time) */
135 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
139 { /* time went backwards; kill the filter */
142 { /* ignore mildly negative time */
146 { /* Normal mode of operation; fast approximation to exp(-tc) */
147 (*outthe) = (*outthe) * ( 1.0 - tc )
151 { /* Huge time step; assume filter has settled */
154 { /* Moderate time step; non linear response */
155 double keep = exp ( -tc );
156 // printf ( "ARP: Keep is %f\n", keep );
157 (*outthe) = (*outthe) * keep
158 + inthe * ( 1.0 - keep );
163 #define INHG_TO_MB 33.86388 /* Inches_of_mercury * INHG_TO_MB == millibars. */
165 // Convert air pressure to altitude by ICAO Standard Atmosphere
166 double pressInHgToAltFt(double p_inhg)
168 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
169 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
170 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
171 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
173 if (p_inhg > p_Tr) // 0.0 to 11.0 km
174 return (1.0 - pow((p_inhg / P_0), 1.0 / 5.2558797)) / 6.8755856e-6;
175 return h_Tr + log10(p_inhg / p_Tr) / -4.806346e-5; // 11.0 to 20.0 km
176 // We could put more code for higher altitudes here.
180 // Convert altitude to air pressure by ICAO Standard Atmosphere
181 double altFtToPressInHg(double alt_ft)
183 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
184 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
185 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
186 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
188 if (alt_ft < h_Tr) // 0.0 to 11.0 km
189 return P_0 * pow(1.0 - 6.8755856e-6 * alt_ft, 5.2558797);
190 return p_Tr * exp(-4.806346e-5 * (alt_ft - h_Tr)); // 11.0 to 20.0 km
191 // We could put more code for higher altitudes here.
196 ////////////////////////////////////////////////////////////////////////
197 // Here the fun really begins
198 ////////////////////////////////////////////////////////////////////////
201 void FGSteam::_CatchUp()
202 { if ( _UpdatesPending != 0 )
203 { double dt = _UpdatesPending * 1.0 /
204 fgGetInt("/sim/model-hz"); // FIXME: inefficient
205 double AccN, AccE, AccU;
207 double d, the_ENGINE_rpm;
210 /**************************
211 There is the possibility that this is the first call.
212 If this is the case, we will emit the feature registrations
213 just to be on the safe side. Doing it more than once will
214 waste CPU time but doesn't hurt anything really.
216 if ( _UpdatesPending > 999999 )
217 { FGFeature::register_int ( "Avionics/NAV1/Localizer", &NAV1_LOC );
218 FGFeature::register_double ( "Avionics/NAV1/Latitude", &NAV1_Lat );
219 FGFeature::register_double ( "Avionics/NAV1/Longitude", &NAV1_Lon );
220 FGFeature::register_double ( "Avionics/NAV1/Radial", &NAV1_Rad );
221 FGFeature::register_double ( "Avionics/NAV1/Altitude", &NAV1_Alt );
222 FGFeature::register_int ( "Avionics/NAV2/Localizer", &NAV2_LOC );
223 FGFeature::register_double ( "Avionics/NAV2/Latitude", &NAV2_Lat );
224 FGFeature::register_double ( "Avionics/NAV2/Longitude", &NAV2_Lon );
225 FGFeature::register_double ( "Avionics/NAV2/Radial", &NAV2_Rad );
226 FGFeature::register_double ( "Avionics/NAV2/Altitude", &NAV2_Alt );
227 FGFeature::register_double ( "Avionics/ADF/Latitude", &ADF_Lat );
228 FGFeature::register_double ( "Avionics/ADF/Longitude", &ADF_Lon );
232 /**************************
233 Someone has called our update function and
234 it turns out that we are running somewhat behind.
235 Here, we recalculate everything for a 'dt' second step.
238 /**************************
239 The ball responds to the acceleration vector in the body
240 frame, only the components perpendicular to the longitudinal
241 axis of the aircraft. This is only related to actual
242 side slip for a symmetrical aircraft which is not touching
243 the ground and not changing its attitude. Math simplifies
244 by assuming (for small angles) arctan(x)=x in radians.
245 Obvious failure mode is the absence of liquid in the
246 tube, which is there to damp the motion, so that instead
247 the ball will bounce around, hitting the tube ends.
248 More subtle flaw is having it not move or a travel limit
249 occasionally due to some dirt in the tube or on the ball.
251 d = - current_aircraft.fdm_state->get_A_Z_pilot();
253 set_lowpass ( & the_TC_rad,
254 current_aircraft.fdm_state->get_A_Y_pilot () / d,
257 /**************************
258 The rate of turn indication is from an electric gyro.
259 We should have it spin up with the master switch.
260 It is mounted at a funny angle so that it detects
261 both rate of bank (i.e. rolling into and out of turns)
262 and the rate of turn (i.e. how fast heading is changing).
264 set_lowpass ( & the_TC_std,
265 current_aircraft.fdm_state->get_Phi_dot ()
266 * SGD_RADIANS_TO_DEGREES / 20.0 +
267 current_aircraft.fdm_state->get_Psi_dot ()
268 * SGD_RADIANS_TO_DEGREES / 3.0 , dt );
270 /**************************
271 We want to know the pilot accelerations,
272 to compute the magnetic compass errors.
274 AccN = current_aircraft.fdm_state->get_V_dot_north();
275 AccE = current_aircraft.fdm_state->get_V_dot_east();
276 AccU = current_aircraft.fdm_state->get_V_dot_down()
278 if ( fabs(the_TC_rad) > 0.2 /* 2.0 */ )
279 { /* Massive sideslip jams it; it stops turning */
281 the_MH_err = fgGetDouble("/orientation/heading-deg") - the_MH_deg;
283 { double MagDip, MagVar, CosDip;
284 double FrcN, FrcE, FrcU, AccTot;
285 double EdgN, EdgE, EdgU;
286 double TrqN, TrqE, TrqU, Torque;
287 /* Find a force vector towards exact magnetic north */
288 MagVar = fgGetDouble("/environment/magnetic-variation-deg")
289 / SGD_RADIANS_TO_DEGREES;
290 MagDip = fgGetDouble("/environment/magnetic-dip-deg")
291 / SGD_RADIANS_TO_DEGREES;
292 CosDip = cos ( MagDip );
293 FrcN = CosDip * cos ( MagVar );
294 FrcE = CosDip * sin ( MagVar );
295 FrcU = sin ( MagDip );
296 /* Rotation occurs around acceleration axis,
297 but axis magnitude is irrelevant. So compute it. */
298 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
299 if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot );
301 /* Force applies to north marking on compass card */
302 EdgN = cos ( the_MH_err / SGD_RADIANS_TO_DEGREES );
303 EdgE = sin ( the_MH_err / SGD_RADIANS_TO_DEGREES );
305 /* Apply the force to the edge to get torques */
306 TrqN = EdgE * FrcU - EdgU * FrcE;
307 TrqE = EdgU * FrcN - EdgN * FrcU;
308 TrqU = EdgN * FrcE - EdgE * FrcN;
309 /* Select the component parallel to the axis */
310 Torque = ( TrqN * AccN +
312 TrqU * AccU ) * 5.0 / AccTot;
313 /* The magnetic compass has angular momentum,
314 so we apply a torque to it and wait */
316 { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
317 the_MH_err += dt * the_MH_degps;
319 if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else
320 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
321 the_MH_deg = fgGetDouble("/orientation/heading-deg") - the_MH_err;
324 /**************************
325 This is not actually correct, but provides a
326 scaling capability for the vacuum pump later on.
327 When we have a real engine model, we can ask it.
329 the_ENGINE_rpm = globals->get_controls()->get_throttle(0) * 26.0;
331 /**************************
332 First, we need to know what the static line is reporting,
333 which is a whole simulation area in itself. For now, we cheat.
334 We filter the actual value by one second to
335 account for the line impedance of the plumbing.
338 = altFtToPressInHg(fgGetDouble("/position/altitude-ft"));
339 set_lowpass ( & the_STATIC_inhg, static_inhg, dt );
342 NO alternate static source error (student feature),
343 NO possibility of blockage (instructor feature),
344 NO slip-induced error, important for C172 for example.
347 /**************************
349 ICAO standard atmosphere MSL pressure is 1013.25 mb, and pressure
350 gradient is about 28 ft per mb at MSL increasing to about 32 at
351 5000 and 38 at 10000 ft.
352 Standard altimeters apply the subscale offset to the output altitude,
353 not to the input pressure; I don't know exactly what pressure gradient
354 they assume for this. I choose to make it accurate at low altitudes.
355 Remember, we are trying to simulate a real altimeter, not an ideal one.
357 set_lowpass ( & the_ALT_ft,
358 pressInHgToAltFt(the_STATIC_inhg) +
359 (the_ALT_datum_mb - 1013.25) * 28.0, /* accurate at low alt. */
360 dt * 10 ); /* smoothing time 0.1 s */
362 /**************************
363 The VSI case is a low-pass filter of the static line pressure.
364 The instrument reports the difference, scaled to approx ft.
365 NO option for student to break glass when static source fails.
366 NO capability for a fixed non-zero reading when level.
367 NO capability to have a scaling error of maybe a factor of two.
369 the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
370 * 10000.0; /* manual scaling factor */
371 set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
373 /**************************
374 The engine driven vacuum pump is directly attached
375 to the engine shaft, so each engine rotation pumps
376 a fixed volume. The amount of air in that volume
377 is determined by the vacuum line's internal pressure.
378 The instruments are essentially leaking air like
379 a fixed source impedance from atmospheric pressure.
380 The regulator provides a digital limit setting,
381 which is open circuit unless the pressure drop is big.
382 Thus, we can compute the vacuum line pressure directly.
383 We assume that there is negligible reservoir space.
384 NO failure of the pump supported (yet)
386 the_VACUUM_inhg = the_STATIC_inhg *
387 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
388 if ( the_VACUUM_inhg > 5.0 )
389 the_VACUUM_inhg = 5.0;
392 > I was merely going to do the engine rpm driven vacuum pump for both
393 > the AI and DG, have the gyros spin down down in power off descents,
394 > have it tumble when you exceed the usual pitch or bank limits,
395 > put in those insidious turning errors ... for now anyway.
397 if ( _UpdatesPending > 999999 )
398 the_DG_err = fgGetDouble("/environment/magnetic-variation-deg");
399 the_DG_degps = 0.01; /* HACK! */
400 if (dt<1.0) the_DG_err += dt * the_DG_degps;
401 the_DG_deg = fgGetDouble("/orientation/heading-deg") - the_DG_err;
403 /**************************
404 Finished updates, now clear the timer
408 // cout << "0 Updates pending" << endl;
413 ////////////////////////////////////////////////////////////////////////
414 // Everything below is a transient hack; expect it to disappear
415 ////////////////////////////////////////////////////////////////////////
420 double FGSteam::get_HackGS_deg () {
421 if ( current_radiostack->get_nav1_inrange() &&
422 current_radiostack->get_nav1_has_gs() )
424 double x = current_radiostack->get_nav1_gs_dist();
425 double y = (fgGetDouble("/position/altitude-ft")
426 - current_radiostack->get_nav1_elev())
428 double angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
429 return (current_radiostack->get_nav1_target_gs() - angle) * 5.0;
436 double FGSteam::get_HackVOR1_deg () {
439 if ( current_radiostack->get_nav1_inrange() ) {
440 r = current_radiostack->get_nav1_heading()
441 - current_radiostack->get_nav1_radial();
442 // cout << "Radial = " << current_radiostack->get_nav1_radial()
443 // << " Bearing = " << current_radiostack->get_nav1_heading()
446 if (r> 180.0) r-=360.0; else
447 if (r<-180.0) r+=360.0;
448 if ( fabs(r) > 90.0 )
449 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
450 // According to Robin Peel, the ILS is 4x more sensitive than a vor
451 if ( current_radiostack->get_nav1_loc() ) r *= 4.0;
460 double FGSteam::get_HackVOR2_deg () {
463 if ( current_radiostack->get_nav2_inrange() ) {
464 r = current_radiostack->get_nav2_heading()
465 - current_radiostack->get_nav2_radial();
466 // cout << "Radial = " << current_radiostack->get_nav1_radial()
467 // << " Bearing = " << current_radiostack->get_nav1_heading() << endl;
469 if (r> 180.0) r-=360.0; else
470 if (r<-180.0) r+=360.0;
471 if ( fabs(r) > 90.0 )
472 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
482 double FGSteam::get_HackOBS1_deg () {
483 return current_radiostack->get_nav1_radial();
487 double FGSteam::get_HackOBS2_deg () {
488 return current_radiostack->get_nav2_radial();
492 double FGSteam::get_HackADF_deg () {
493 static double last_r = 0;
495 if ( current_radiostack->get_adf_inrange() ) {
496 double r = current_radiostack->get_adf_heading()
497 - fgGetDouble("/orientation/heading-deg");
499 // cout << "Radial = " << current_radiostack->get_adf_heading()
500 // << " Heading = " << FGBFI::getHeading() << endl;