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"); }
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", FGSteam::get_ASI_kias);
106 fgTie("/steam/altitude", FGSteam::get_ALT_ft);
107 fgTie("/steam/altimeter-datum-mb",
108 FGSteam::get_ALT_datum_mb, FGSteam::set_ALT_datum_mb,
109 false); /* don't modify the value */
110 fgTie("/steam/turn-rate", FGSteam::get_TC_std);
111 fgTie("/steam/slip-skid", FGSteam::get_TC_rad);
112 fgTie("/steam/vertical-speed", FGSteam::get_VSI_fps);
113 fgTie("/steam/gyro-compass", FGSteam::get_DG_deg);
114 fgTie("/steam/adf", FGSteam::get_HackADF_deg);
115 fgTie("/steam/gyro-compass-error",
116 FGSteam::get_DG_err, FGSteam::set_DG_err,
117 false); /* don't modify the value */
118 fgTie("/steam/mag-compass", FGSteam::get_MH_deg);
120 _UpdatesPending += timesteps;
124 /* tc should be (elapsed_time_between_updates / desired_smoothing_time) */
125 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
129 { /* time went backwards; kill the filter */
132 { /* ignore mildly negative time */
136 { /* Normal mode of operation; fast approximation to exp(-tc) */
137 (*outthe) = (*outthe) * ( 1.0 - tc )
141 { /* Huge time step; assume filter has settled */
144 { /* Moderate time step; non linear response */
145 double keep = exp ( -tc );
146 // printf ( "ARP: Keep is %f\n", keep );
147 (*outthe) = (*outthe) * keep
148 + inthe * ( 1.0 - keep );
153 #define INHG_TO_MB 33.86388 /* Inches_of_mercury * INHG_TO_MB == millibars. */
155 // Convert air pressure to altitude by ICAO Standard Atmosphere
156 double pressInHgToAltFt(double p_inhg)
158 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
159 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
160 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
161 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
163 if (p_inhg > p_Tr) // 0.0 to 11.0 km
164 return (1.0 - pow((p_inhg / P_0), 1.0 / 5.2558797)) / 6.8755856e-6;
165 return h_Tr + log10(p_inhg / p_Tr) / -4.806346e-5; // 11.0 to 20.0 km
166 // We could put more code for higher altitudes here.
170 // Convert altitude to air pressure by ICAO Standard Atmosphere
171 double altFtToPressInHg(double alt_ft)
173 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
174 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
175 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
176 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
178 if (alt_ft < h_Tr) // 0.0 to 11.0 km
179 return P_0 * pow(1.0 - 6.8755856e-6 * alt_ft, 5.2558797);
180 return p_Tr * exp(-4.806346e-5 * (alt_ft - h_Tr)); // 11.0 to 20.0 km
181 // We could put more code for higher altitudes here.
186 ////////////////////////////////////////////////////////////////////////
187 // Here the fun really begins
188 ////////////////////////////////////////////////////////////////////////
191 void FGSteam::_CatchUp()
192 { if ( _UpdatesPending != 0 )
193 { double dt = _UpdatesPending * 1.0 /
194 fgGetInt("/sim/model-hz"); // FIXME: inefficient
195 double AccN, AccE, AccU;
197 double d, the_ENGINE_rpm;
200 /**************************
201 There is the possibility that this is the first call.
202 If this is the case, we will emit the feature registrations
203 just to be on the safe side. Doing it more than once will
204 waste CPU time but doesn't hurt anything really.
206 if ( _UpdatesPending > 999999 )
207 { FGFeature::register_int ( "Avionics/NAV1/Localizer", &NAV1_LOC );
208 FGFeature::register_double ( "Avionics/NAV1/Latitude", &NAV1_Lat );
209 FGFeature::register_double ( "Avionics/NAV1/Longitude", &NAV1_Lon );
210 FGFeature::register_double ( "Avionics/NAV1/Radial", &NAV1_Rad );
211 FGFeature::register_double ( "Avionics/NAV1/Altitude", &NAV1_Alt );
212 FGFeature::register_int ( "Avionics/NAV2/Localizer", &NAV2_LOC );
213 FGFeature::register_double ( "Avionics/NAV2/Latitude", &NAV2_Lat );
214 FGFeature::register_double ( "Avionics/NAV2/Longitude", &NAV2_Lon );
215 FGFeature::register_double ( "Avionics/NAV2/Radial", &NAV2_Rad );
216 FGFeature::register_double ( "Avionics/NAV2/Altitude", &NAV2_Alt );
217 FGFeature::register_double ( "Avionics/ADF/Latitude", &ADF_Lat );
218 FGFeature::register_double ( "Avionics/ADF/Longitude", &ADF_Lon );
222 /**************************
223 Someone has called our update function and
224 it turns out that we are running somewhat behind.
225 Here, we recalculate everything for a 'dt' second step.
228 /**************************
229 The ball responds to the acceleration vector in the body
230 frame, only the components perpendicular to the longitudinal
231 axis of the aircraft. This is only related to actual
232 side slip for a symmetrical aircraft which is not touching
233 the ground and not changing its attitude. Math simplifies
234 by assuming (for small angles) arctan(x)=x in radians.
235 Obvious failure mode is the absence of liquid in the
236 tube, which is there to damp the motion, so that instead
237 the ball will bounce around, hitting the tube ends.
238 More subtle flaw is having it not move or a travel limit
239 occasionally due to some dirt in the tube or on the ball.
241 d = - current_aircraft.fdm_state->get_A_Z_pilot();
243 set_lowpass ( & the_TC_rad,
244 current_aircraft.fdm_state->get_A_Y_pilot () / d,
247 /**************************
248 The rate of turn indication is from an electric gyro.
249 We should have it spin up with the master switch.
250 It is mounted at a funny angle so that it detects
251 both rate of bank (i.e. rolling into and out of turns)
252 and the rate of turn (i.e. how fast heading is changing).
254 set_lowpass ( & the_TC_std,
255 current_aircraft.fdm_state->get_Phi_dot ()
256 * SGD_RADIANS_TO_DEGREES / 20.0 +
257 current_aircraft.fdm_state->get_Psi_dot ()
258 * SGD_RADIANS_TO_DEGREES / 3.0 , dt );
260 /**************************
261 We want to know the pilot accelerations,
262 to compute the magnetic compass errors.
264 AccN = current_aircraft.fdm_state->get_V_dot_north();
265 AccE = current_aircraft.fdm_state->get_V_dot_east();
266 AccU = current_aircraft.fdm_state->get_V_dot_down()
268 if ( fabs(the_TC_rad) > 0.2 )
269 { /* Massive sideslip jams it; it stops turning */
271 the_MH_err = fgGetDouble("/orientation/heading") - the_MH_deg;
273 { double MagDip, MagVar, CosDip;
274 double FrcN, FrcE, FrcU, AccTot;
275 double EdgN, EdgE, EdgU;
276 double TrqN, TrqE, TrqU, Torque;
277 /* Find a force vector towards exact magnetic north */
278 MagVar = fgGetDouble("/environment/magnetic-variation")
279 / SGD_RADIANS_TO_DEGREES;
280 MagDip = fgGetDouble("/environment/magnetic-dip")
281 / SGD_RADIANS_TO_DEGREES;
282 CosDip = cos ( MagDip );
283 FrcN = CosDip * cos ( MagVar );
284 FrcE = CosDip * sin ( MagVar );
285 FrcU = sin ( MagDip );
286 /* Rotation occurs around acceleration axis,
287 but axis magnitude is irrelevant. So compute it. */
288 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
289 if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot );
291 /* Force applies to north marking on compass card */
292 EdgN = cos ( the_MH_err / SGD_RADIANS_TO_DEGREES );
293 EdgE = sin ( the_MH_err / SGD_RADIANS_TO_DEGREES );
295 /* Apply the force to the edge to get torques */
296 TrqN = EdgE * FrcU - EdgU * FrcE;
297 TrqE = EdgU * FrcN - EdgN * FrcU;
298 TrqU = EdgN * FrcE - EdgE * FrcN;
299 /* Select the component parallel to the axis */
300 Torque = ( TrqN * AccN +
302 TrqU * AccU ) * 5.0 / AccTot;
303 /* The magnetic compass has angular momentum,
304 so we apply a torque to it and wait */
306 { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
307 the_MH_err += dt * the_MH_degps;
309 if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else
310 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
311 the_MH_deg = fgGetDouble("/orientation/heading") - the_MH_err;
314 /**************************
315 This is not actually correct, but provides a
316 scaling capability for the vacuum pump later on.
317 When we have a real engine model, we can ask it.
319 the_ENGINE_rpm = controls.get_throttle(0) * 26.0;
321 /**************************
322 First, we need to know what the static line is reporting,
323 which is a whole simulation area in itself. For now, we cheat.
324 We filter the actual value by one second to
325 account for the line impedance of the plumbing.
328 = altFtToPressInHg(fgGetDouble("/position/altitude"));
329 set_lowpass ( & the_STATIC_inhg, static_inhg, dt );
332 NO alternate static source error (student feature),
333 NO possibility of blockage (instructor feature),
334 NO slip-induced error, important for C172 for example.
337 /**************************
339 ICAO standard atmosphere MSL pressure is 1013.25 mb, and pressure
340 gradient is about 28 ft per mb at MSL increasing to about 32 at
341 5000 and 38 at 10000 ft.
342 Standard altimeters apply the subscale offset to the output altitude,
343 not to the input pressure; I don't know exactly what pressure gradient
344 they assume for this. I choose to make it accurate at low altitudes.
345 Remember, we are trying to simulate a real altimeter, not an ideal one.
347 set_lowpass ( & the_ALT_ft,
348 pressInHgToAltFt(the_STATIC_inhg) +
349 (the_ALT_datum_mb - 1013.25) * 28.0, /* accurate at low alt. */
350 dt * 10 ); /* smoothing time 0.1 s */
352 /**************************
353 The VSI case is a low-pass filter of the static line pressure.
354 The instrument reports the difference, scaled to approx ft.
355 NO option for student to break glass when static source fails.
356 NO capability for a fixed non-zero reading when level.
357 NO capability to have a scaling error of maybe a factor of two.
359 the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
360 * 10000.0; /* manual scaling factor */
361 set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
363 /**************************
364 The engine driven vacuum pump is directly attached
365 to the engine shaft, so each engine rotation pumps
366 a fixed volume. The amount of air in that volume
367 is determined by the vacuum line's internal pressure.
368 The instruments are essentially leaking air like
369 a fixed source impedance from atmospheric pressure.
370 The regulator provides a digital limit setting,
371 which is open circuit unless the pressure drop is big.
372 Thus, we can compute the vacuum line pressure directly.
373 We assume that there is negligible reservoir space.
374 NO failure of the pump supported (yet)
376 the_VACUUM_inhg = the_STATIC_inhg *
377 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
378 if ( the_VACUUM_inhg > 5.0 )
379 the_VACUUM_inhg = 5.0;
382 > I was merely going to do the engine rpm driven vacuum pump for both
383 > the AI and DG, have the gyros spin down down in power off descents,
384 > have it tumble when you exceed the usual pitch or bank limits,
385 > put in those insidious turning errors ... for now anyway.
387 if ( _UpdatesPending > 999999 )
388 the_DG_err = fgGetDouble("/environment/magnetic-variation");
389 the_DG_degps = 0.01; /* HACK! */
390 if (dt<1.0) the_DG_err += dt * the_DG_degps;
391 the_DG_deg = fgGetDouble("/orientation/heading") - the_DG_err;
393 /**************************
394 Finished updates, now clear the timer
398 // cout << "0 Updates pending" << endl;
403 ////////////////////////////////////////////////////////////////////////
404 // Everything below is a transient hack; expect it to disappear
405 ////////////////////////////////////////////////////////////////////////
410 double FGSteam::get_HackGS_deg () {
411 if ( current_radiostack->get_nav1_inrange() &&
412 current_radiostack->get_nav1_has_gs() )
414 double x = current_radiostack->get_nav1_gs_dist();
415 double y = (fgGetDouble("/position/altitude")
416 - current_radiostack->get_nav1_elev())
418 double angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
419 return (current_radiostack->get_nav1_target_gs() - angle) * 5.0;
426 double FGSteam::get_HackVOR1_deg () {
429 if ( current_radiostack->get_nav1_inrange() ) {
430 r = current_radiostack->get_nav1_heading()
431 - current_radiostack->get_nav1_radial();
432 // cout << "Radial = " << current_radiostack->get_nav1_radial()
433 // << " Bearing = " << current_radiostack->get_nav1_heading()
436 if (r> 180.0) r-=360.0; else
437 if (r<-180.0) r+=360.0;
438 if ( fabs(r) > 90.0 )
439 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
440 // According to Robin Peel, the ILS is 4x more sensitive than a vor
441 if ( current_radiostack->get_nav1_loc() ) r *= 4.0;
450 double FGSteam::get_HackVOR2_deg () {
453 if ( current_radiostack->get_nav2_inrange() ) {
454 r = current_radiostack->get_nav2_heading()
455 - current_radiostack->get_nav2_radial();
456 // cout << "Radial = " << current_radiostack->get_nav1_radial()
457 // << " Bearing = " << current_radiostack->get_nav1_heading() << endl;
459 if (r> 180.0) r-=360.0; else
460 if (r<-180.0) r+=360.0;
461 if ( fabs(r) > 90.0 )
462 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
472 double FGSteam::get_HackOBS1_deg () {
473 return current_radiostack->get_nav1_radial();
477 double FGSteam::get_HackOBS2_deg () {
478 return current_radiostack->get_nav2_radial();
482 double FGSteam::get_HackADF_deg () {
483 static double last_r = 0;
485 if ( current_radiostack->get_adf_inrange() ) {
486 double r = current_radiostack->get_adf_heading()
487 - fgGetDouble("orientation/heading");
489 // cout << "Radial = " << current_radiostack->get_adf_heading()
490 // << " Heading = " << FGBFI::getHeading() << endl;