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 (double dt)
103 int timesteps = int(dt * fgGetInt("/sim/model-hz")); // FIXME
106 fgTie("/steam/airspeed-kt", FGSteam::get_ASI_kias);
107 fgSetArchivable("/steam/airspeed-kt");
108 fgTie("/steam/altitude-ft", FGSteam::get_ALT_ft);
109 fgSetArchivable("/steam/altitude-ft");
110 fgTie("/steam/altimeter-datum-mb",
111 FGSteam::get_ALT_datum_mb, FGSteam::set_ALT_datum_mb,
112 false); /* don't modify the value */
113 fgSetArchivable("/steam/altimeter-datum-mb");
114 fgTie("/steam/turn-rate", FGSteam::get_TC_std);
115 fgSetArchivable("/steam/turn-rate");
116 fgTie("/steam/slip-skid", FGSteam::get_TC_rad);
117 fgSetArchivable("/steam/slip-skid");
118 fgTie("/steam/vertical-speed-fps", FGSteam::get_VSI_fps);
119 fgSetArchivable("/steam/vertical-speed-fps");
120 fgTie("/steam/gyro-compass-deg", FGSteam::get_DG_deg);
121 fgSetArchivable("/steam/gyro-compass-deg");
122 // fgTie("/steam/adf-deg", FGSteam::get_HackADF_deg);
123 // fgSetArchivable("/steam/adf-deg");
124 fgTie("/steam/gyro-compass-error-deg",
125 FGSteam::get_DG_err, FGSteam::set_DG_err,
126 false); /* don't modify the value */
127 fgSetArchivable("/steam/gyro-compass-error-deg");
128 fgTie("/steam/mag-compass-deg", FGSteam::get_MH_deg);
129 fgSetArchivable("/steam/mag-compass-deg");
131 _UpdatesPending += timesteps;
135 /* tc should be (elapsed_time_between_updates / desired_smoothing_time) */
136 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
140 { /* time went backwards; kill the filter */
143 { /* ignore mildly negative time */
147 { /* Normal mode of operation; fast approximation to exp(-tc) */
148 (*outthe) = (*outthe) * ( 1.0 - tc )
152 { /* Huge time step; assume filter has settled */
155 { /* Moderate time step; non linear response */
156 double keep = exp ( -tc );
157 // printf ( "ARP: Keep is %f\n", keep );
158 (*outthe) = (*outthe) * keep
159 + inthe * ( 1.0 - keep );
164 #define INHG_TO_MB 33.86388 /* Inches_of_mercury * INHG_TO_MB == millibars. */
166 // Convert air pressure to altitude by ICAO Standard Atmosphere
167 double pressInHgToAltFt(double p_inhg)
169 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
170 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
171 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
172 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
174 if (p_inhg > p_Tr) // 0.0 to 11.0 km
175 return (1.0 - pow((p_inhg / P_0), 1.0 / 5.2558797)) / 6.8755856e-6;
176 return h_Tr + log10(p_inhg / p_Tr) / -4.806346e-5; // 11.0 to 20.0 km
177 // We could put more code for higher altitudes here.
181 // Convert altitude to air pressure by ICAO Standard Atmosphere
182 double altFtToPressInHg(double alt_ft)
184 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
185 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
186 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
187 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
189 if (alt_ft < h_Tr) // 0.0 to 11.0 km
190 return P_0 * pow(1.0 - 6.8755856e-6 * alt_ft, 5.2558797);
191 return p_Tr * exp(-4.806346e-5 * (alt_ft - h_Tr)); // 11.0 to 20.0 km
192 // We could put more code for higher altitudes here.
197 ////////////////////////////////////////////////////////////////////////
198 // Here the fun really begins
199 ////////////////////////////////////////////////////////////////////////
202 void FGSteam::_CatchUp()
203 { if ( _UpdatesPending != 0 )
204 { double dt = _UpdatesPending * 1.0 /
205 fgGetInt("/sim/model-hz"); // FIXME: inefficient
206 double AccN, AccE, AccU;
208 double d, the_ENGINE_rpm;
210 /**************************
211 Someone has called our update function and
212 it turns out that we are running somewhat behind.
213 Here, we recalculate everything for a 'dt' second step.
216 /**************************
217 The ball responds to the acceleration vector in the body
218 frame, only the components perpendicular to the longitudinal
219 axis of the aircraft. This is only related to actual
220 side slip for a symmetrical aircraft which is not touching
221 the ground and not changing its attitude. Math simplifies
222 by assuming (for small angles) arctan(x)=x in radians.
223 Obvious failure mode is the absence of liquid in the
224 tube, which is there to damp the motion, so that instead
225 the ball will bounce around, hitting the tube ends.
226 More subtle flaw is having it not move or a travel limit
227 occasionally due to some dirt in the tube or on the ball.
229 d = - current_aircraft.fdm_state->get_A_Z_pilot();
231 set_lowpass ( & the_TC_rad,
232 current_aircraft.fdm_state->get_A_Y_pilot () / d,
235 /**************************
236 The rate of turn indication is from an electric gyro.
237 We should have it spin up with the master switch.
238 It is mounted at a funny angle so that it detects
239 both rate of bank (i.e. rolling into and out of turns)
240 and the rate of turn (i.e. how fast heading is changing).
242 set_lowpass ( & the_TC_std,
243 current_aircraft.fdm_state->get_Phi_dot ()
244 * SGD_RADIANS_TO_DEGREES / 20.0 +
245 current_aircraft.fdm_state->get_Psi_dot ()
246 * SGD_RADIANS_TO_DEGREES / 3.0 , dt );
248 /**************************
249 We want to know the pilot accelerations,
250 to compute the magnetic compass errors.
252 AccN = current_aircraft.fdm_state->get_V_dot_north();
253 AccE = current_aircraft.fdm_state->get_V_dot_east();
254 AccU = current_aircraft.fdm_state->get_V_dot_down()
256 if ( fabs(the_TC_rad) > 0.2 /* 2.0 */ )
257 { /* Massive sideslip jams it; it stops turning */
259 the_MH_err = fgGetDouble("/orientation/heading-deg") - the_MH_deg;
261 { double MagDip, MagVar, CosDip;
262 double FrcN, FrcE, FrcU, AccTot;
263 double EdgN, EdgE, EdgU;
264 double TrqN, TrqE, TrqU, Torque;
265 /* Find a force vector towards exact magnetic north */
266 MagVar = fgGetDouble("/environment/magnetic-variation-deg")
267 / SGD_RADIANS_TO_DEGREES;
268 MagDip = fgGetDouble("/environment/magnetic-dip-deg")
269 / SGD_RADIANS_TO_DEGREES;
270 CosDip = cos ( MagDip );
271 FrcN = CosDip * cos ( MagVar );
272 FrcE = CosDip * sin ( MagVar );
273 FrcU = sin ( MagDip );
274 /* Rotation occurs around acceleration axis,
275 but axis magnitude is irrelevant. So compute it. */
276 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
277 if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot );
279 /* Force applies to north marking on compass card */
280 EdgN = cos ( the_MH_err / SGD_RADIANS_TO_DEGREES );
281 EdgE = sin ( the_MH_err / SGD_RADIANS_TO_DEGREES );
283 /* Apply the force to the edge to get torques */
284 TrqN = EdgE * FrcU - EdgU * FrcE;
285 TrqE = EdgU * FrcN - EdgN * FrcU;
286 TrqU = EdgN * FrcE - EdgE * FrcN;
287 /* Select the component parallel to the axis */
288 Torque = ( TrqN * AccN +
290 TrqU * AccU ) * 5.0 / AccTot;
291 /* The magnetic compass has angular momentum,
292 so we apply a torque to it and wait */
294 { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
295 the_MH_err += dt * the_MH_degps;
297 if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else
298 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
299 the_MH_deg = fgGetDouble("/orientation/heading-deg") - the_MH_err;
302 /**************************
303 This is not actually correct, but provides a
304 scaling capability for the vacuum pump later on.
305 When we have a real engine model, we can ask it.
307 the_ENGINE_rpm = globals->get_controls()->get_throttle(0) * 26.0;
309 /**************************
310 First, we need to know what the static line is reporting,
311 which is a whole simulation area in itself. For now, we cheat.
312 We filter the actual value by one second to
313 account for the line impedance of the plumbing.
316 = altFtToPressInHg(fgGetDouble("/position/altitude-ft"));
317 set_lowpass ( & the_STATIC_inhg, static_inhg, dt );
320 NO alternate static source error (student feature),
321 NO possibility of blockage (instructor feature),
322 NO slip-induced error, important for C172 for example.
325 /**************************
327 ICAO standard atmosphere MSL pressure is 1013.25 mb, and pressure
328 gradient is about 28 ft per mb at MSL increasing to about 32 at
329 5000 and 38 at 10000 ft.
330 Standard altimeters apply the subscale offset to the output altitude,
331 not to the input pressure; I don't know exactly what pressure gradient
332 they assume for this. I choose to make it accurate at low altitudes.
333 Remember, we are trying to simulate a real altimeter, not an ideal one.
335 set_lowpass ( & the_ALT_ft,
336 pressInHgToAltFt(the_STATIC_inhg) +
337 (the_ALT_datum_mb - 1013.25) * 28.0, /* accurate at low alt. */
338 dt * 10 ); /* smoothing time 0.1 s */
340 /**************************
341 The VSI case is a low-pass filter of the static line pressure.
342 The instrument reports the difference, scaled to approx ft.
343 NO option for student to break glass when static source fails.
344 NO capability for a fixed non-zero reading when level.
345 NO capability to have a scaling error of maybe a factor of two.
347 the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
348 * 10000.0; /* manual scaling factor */
349 set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
351 /**************************
352 The engine driven vacuum pump is directly attached
353 to the engine shaft, so each engine rotation pumps
354 a fixed volume. The amount of air in that volume
355 is determined by the vacuum line's internal pressure.
356 The instruments are essentially leaking air like
357 a fixed source impedance from atmospheric pressure.
358 The regulator provides a digital limit setting,
359 which is open circuit unless the pressure drop is big.
360 Thus, we can compute the vacuum line pressure directly.
361 We assume that there is negligible reservoir space.
362 NO failure of the pump supported (yet)
364 the_VACUUM_inhg = the_STATIC_inhg *
365 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
366 if ( the_VACUUM_inhg > 5.0 )
367 the_VACUUM_inhg = 5.0;
370 > I was merely going to do the engine rpm driven vacuum pump for both
371 > the AI and DG, have the gyros spin down down in power off descents,
372 > have it tumble when you exceed the usual pitch or bank limits,
373 > put in those insidious turning errors ... for now anyway.
375 if ( _UpdatesPending > 999999 )
376 the_DG_err = fgGetDouble("/environment/magnetic-variation-deg");
377 the_DG_degps = 0.01; /* HACK! */
378 if (dt<1.0) the_DG_err += dt * the_DG_degps;
379 the_DG_deg = fgGetDouble("/orientation/heading-deg") - the_DG_err;
381 /**************************
382 Finished updates, now clear the timer
386 // cout << "0 Updates pending" << endl;
391 ////////////////////////////////////////////////////////////////////////
392 // Everything below is a transient hack; expect it to disappear
393 ////////////////////////////////////////////////////////////////////////
398 double FGSteam::get_HackGS_deg () {
399 if ( current_radiostack->get_nav1_inrange() &&
400 current_radiostack->get_nav1_has_gs() )
402 double x = current_radiostack->get_nav1_gs_dist();
403 double y = (fgGetDouble("/position/altitude-ft")
404 - current_radiostack->get_nav1_elev())
406 double angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
407 return (current_radiostack->get_nav1_target_gs() - angle) * 5.0;
414 double FGSteam::get_HackVOR1_deg () {
417 if ( current_radiostack->get_nav1_inrange() ) {
418 r = current_radiostack->get_nav1_heading()
419 - current_radiostack->get_nav1_radial();
420 // cout << "Radial = " << current_radiostack->get_nav1_radial()
421 // << " Bearing = " << current_radiostack->get_nav1_heading()
424 if (r> 180.0) r-=360.0; else
425 if (r<-180.0) r+=360.0;
426 if ( fabs(r) > 90.0 )
427 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
428 // According to Robin Peel, the ILS is 4x more sensitive than a vor
429 if ( current_radiostack->get_nav1_loc() ) r *= 4.0;
438 double FGSteam::get_HackVOR2_deg () {
441 if ( current_radiostack->get_nav2_inrange() ) {
442 r = current_radiostack->get_nav2_heading()
443 - current_radiostack->get_nav2_radial();
444 // cout << "Radial = " << current_radiostack->get_nav1_radial()
445 // << " Bearing = " << current_radiostack->get_nav1_heading() << endl;
447 if (r> 180.0) r-=360.0; else
448 if (r<-180.0) r+=360.0;
449 if ( fabs(r) > 90.0 )
450 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
460 double FGSteam::get_HackOBS1_deg () {
461 return current_radiostack->get_nav1_radial();
465 double FGSteam::get_HackOBS2_deg () {
466 return current_radiostack->get_nav2_radial();
471 double FGSteam::get_HackADF_deg () {
472 static SGPropertyNode *adf_inrange = fgGetNode("/radios/adf/inrange", true);
473 static SGPropertyNode *adf_heading = fgGetNode("/radios/adf/heading", true);
474 static double last_r = 0;
476 if ( adf_inrange->getBoolValue() ) {
477 double r = adf_heading->getDoubleValue()
478 - fgGetDouble("/orientation/heading-deg");
480 // cout << "Radial = " << adf_heading->getDoubleValue() << endl;
481 // cout << "/orientation/heading-deg = "
482 // << fgGetDouble("/orientation/heading-deg") << endl;