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 #if defined( SG_HAVE_NATIVE_SGI_COMPILERS )
27 # include <iostream.h>
32 #include <simgear/constants.h>
33 #include <simgear/math/sg_types.hxx>
34 #include <simgear/misc/props.hxx>
35 #include <Aircraft/aircraft.hxx>
36 #include <Main/bfi.hxx>
37 #include <NetworkOLK/features.hxx>
39 SG_USING_NAMESPACE(std);
41 #include "radiostack.hxx"
44 static bool isTied = false;
48 ////////////////////////////////////////////////////////////////////////
49 // Declare the functions that read the variables
50 ////////////////////////////////////////////////////////////////////////
52 // Anything that reads the BFI directly is not implemented at all!
55 double FGSteam::the_STATIC_inhg = 29.92;
56 double FGSteam::the_ALT_ft = 0.0; // Indicated altitude
57 double FGSteam::get_ALT_ft() { _CatchUp(); return the_ALT_ft; }
59 double FGSteam::the_ALT_datum_mb = 1013.0;
60 double FGSteam::get_ALT_datum_mb() { return the_ALT_datum_mb; }
62 void FGSteam::set_ALT_datum_mb ( double datum_mb ) {
63 the_ALT_datum_mb = datum_mb;
66 double FGSteam::get_ASI_kias() { return FGBFI::getAirspeed(); }
68 double FGSteam::the_VSI_case = 29.92;
69 double FGSteam::the_VSI_fps = 0.0;
70 double FGSteam::get_VSI_fps() { _CatchUp(); return the_VSI_fps; }
72 double FGSteam::the_VACUUM_inhg = 0.0;
73 double FGSteam::get_VACUUM_inhg() { _CatchUp(); return the_VACUUM_inhg; }
75 double FGSteam::the_MH_err = 0.0;
76 double FGSteam::the_MH_deg = 0.0;
77 double FGSteam::the_MH_degps = 0.0;
78 double FGSteam::get_MH_deg () { _CatchUp(); return the_MH_deg; }
80 double FGSteam::the_DG_err = 0.0;
81 double FGSteam::the_DG_deg = 0.0;
82 double FGSteam::the_DG_degps = 0.0;
83 double FGSteam::the_DG_inhg = 0.0;
84 double FGSteam::get_DG_deg () { _CatchUp(); return the_DG_deg; }
85 double FGSteam::get_DG_err () { _CatchUp(); return the_DG_err; }
87 void FGSteam::set_DG_err ( double approx_magvar ) {
88 the_DG_err = approx_magvar;
91 double FGSteam::the_TC_rad = 0.0;
92 double FGSteam::the_TC_std = 0.0;
93 double FGSteam::get_TC_rad () { _CatchUp(); return the_TC_rad; }
94 double FGSteam::get_TC_std () { _CatchUp(); return the_TC_std; }
97 ////////////////////////////////////////////////////////////////////////
98 // Recording the current time
99 ////////////////////////////////////////////////////////////////////////
102 int FGSteam::_UpdatesPending = 1000000; /* Forces filter to reset */
105 // FIXME: no need to use static
106 // functions any longer.
108 void FGSteam::update ( int timesteps )
112 fgTie("/steam/airspeed", FGSteam::get_ASI_kias);
113 fgTie("/steam/altitude", FGSteam::get_ALT_ft);
114 fgTie("/steam/altimeter-datum-mb",
115 FGSteam::get_ALT_datum_mb, FGSteam::set_ALT_datum_mb,
116 false); /* don't modify the value */
117 fgTie("/steam/turn-rate", FGSteam::get_TC_std);
118 fgTie("/steam/slip-skid", FGSteam::get_TC_rad);
119 fgTie("/steam/vertical-speed", FGSteam::get_VSI_fps);
120 fgTie("/steam/gyro-compass", FGSteam::get_DG_deg);
121 fgTie("/steam/adf", FGSteam::get_HackADF_deg);
122 fgTie("/steam/gyro-compass-error",
123 FGSteam::get_DG_err, FGSteam::set_DG_err,
124 false); /* don't modify the value */
125 fgTie("/steam/mag-compass", FGSteam::get_MH_deg);
127 _UpdatesPending += timesteps;
131 /* tc should be (elapsed_time_between_updates / desired_smoothing_time) */
132 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
136 { /* time went backwards; kill the filter */
139 { /* ignore mildly negative time */
143 { /* Normal mode of operation; fast approximation to exp(-tc) */
144 (*outthe) = (*outthe) * ( 1.0 - tc )
148 { /* Huge time step; assume filter has settled */
151 { /* Moderate time step; non linear response */
152 double keep = exp ( -tc );
153 // printf ( "ARP: Keep is %f\n", keep );
154 (*outthe) = (*outthe) * keep
155 + inthe * ( 1.0 - keep );
160 #define INHG_TO_MB 33.86388 /* Inches_of_mercury * INHG_TO_MB == millibars. */
162 // Convert air pressure to altitude by ICAO Standard Atmosphere
163 double pressInHgToAltFt(double p_inhg)
165 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
166 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
167 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
168 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
170 if (p_inhg > p_Tr) // 0.0 to 11.0 km
171 return (1.0 - pow((p_inhg / P_0), 1.0 / 5.2558797)) / 6.8755856e-6;
172 return h_Tr + log10(p_inhg / p_Tr) / -4.806346e-5; // 11.0 to 20.0 km
173 // We could put more code for higher altitudes here.
177 // Convert altitude to air pressure by ICAO Standard Atmosphere
178 double altFtToPressInHg(double alt_ft)
180 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
181 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
182 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
183 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
185 if (alt_ft < h_Tr) // 0.0 to 11.0 km
186 return P_0 * pow(1.0 - 6.8755856e-6 * alt_ft, 5.2558797);
187 return p_Tr * exp(-4.806346e-5 * (alt_ft - h_Tr)); // 11.0 to 20.0 km
188 // We could put more code for higher altitudes here.
193 ////////////////////////////////////////////////////////////////////////
194 // Here the fun really begins
195 ////////////////////////////////////////////////////////////////////////
198 void FGSteam::_CatchUp()
199 { if ( _UpdatesPending != 0 )
200 { double dt = _UpdatesPending * 1.0 /
201 fgGetInt("/sim/model-hz"); // FIXME: inefficient
202 double AccN, AccE, AccU;
204 double d, the_ENGINE_rpm;
207 /**************************
208 There is the possibility that this is the first call.
209 If this is the case, we will emit the feature registrations
210 just to be on the safe side. Doing it more than once will
211 waste CPU time but doesn't hurt anything really.
213 if ( _UpdatesPending > 999999 )
214 { FGFeature::register_int ( "Avionics/NAV1/Localizer", &NAV1_LOC );
215 FGFeature::register_double ( "Avionics/NAV1/Latitude", &NAV1_Lat );
216 FGFeature::register_double ( "Avionics/NAV1/Longitude", &NAV1_Lon );
217 FGFeature::register_double ( "Avionics/NAV1/Radial", &NAV1_Rad );
218 FGFeature::register_double ( "Avionics/NAV1/Altitude", &NAV1_Alt );
219 FGFeature::register_int ( "Avionics/NAV2/Localizer", &NAV2_LOC );
220 FGFeature::register_double ( "Avionics/NAV2/Latitude", &NAV2_Lat );
221 FGFeature::register_double ( "Avionics/NAV2/Longitude", &NAV2_Lon );
222 FGFeature::register_double ( "Avionics/NAV2/Radial", &NAV2_Rad );
223 FGFeature::register_double ( "Avionics/NAV2/Altitude", &NAV2_Alt );
224 FGFeature::register_double ( "Avionics/ADF/Latitude", &ADF_Lat );
225 FGFeature::register_double ( "Avionics/ADF/Longitude", &ADF_Lon );
229 /**************************
230 Someone has called our update function and
231 it turns out that we are running somewhat behind.
232 Here, we recalculate everything for a 'dt' second step.
235 /**************************
236 The ball responds to the acceleration vector in the body
237 frame, only the components perpendicular to the longitudinal
238 axis of the aircraft. This is only related to actual
239 side slip for a symmetrical aircraft which is not touching
240 the ground and not changing its attitude. Math simplifies
241 by assuming (for small angles) arctan(x)=x in radians.
242 Obvious failure mode is the absence of liquid in the
243 tube, which is there to damp the motion, so that instead
244 the ball will bounce around, hitting the tube ends.
245 More subtle flaw is having it not move or a travel limit
246 occasionally due to some dirt in the tube or on the ball.
248 // the_TC_rad = - ( FGBFI::getSideSlip () ); /* incorrect */
249 d = - current_aircraft.fdm_state->get_A_Z_pilot();
251 set_lowpass ( & the_TC_rad,
252 current_aircraft.fdm_state->get_A_Y_pilot () / d,
255 /**************************
256 The rate of turn indication is from an electric gyro.
257 We should have it spin up with the master switch.
258 It is mounted at a funny angle so that it detects
259 both rate of bank (i.e. rolling into and out of turns)
260 and the rate of turn (i.e. how fast heading is changing).
262 set_lowpass ( & the_TC_std,
263 current_aircraft.fdm_state->get_Phi_dot ()
264 * SGD_RADIANS_TO_DEGREES / 20.0 +
265 current_aircraft.fdm_state->get_Psi_dot ()
266 * SGD_RADIANS_TO_DEGREES / 3.0 , dt );
268 /**************************
269 We want to know the pilot accelerations,
270 to compute the magnetic compass errors.
272 AccN = current_aircraft.fdm_state->get_V_dot_north();
273 AccE = current_aircraft.fdm_state->get_V_dot_east();
274 AccU = current_aircraft.fdm_state->get_V_dot_down()
276 if ( fabs(the_TC_rad) > 0.2 )
277 { /* Massive sideslip jams it; it stops turning */
279 the_MH_err = FGBFI::getHeading () - the_MH_deg;
281 { double MagDip, MagVar, CosDip;
282 double FrcN, FrcE, FrcU, AccTot;
283 double EdgN, EdgE, EdgU;
284 double TrqN, TrqE, TrqU, Torque;
285 /* Find a force vector towards exact magnetic north */
286 MagVar = FGBFI::getMagVar() / SGD_RADIANS_TO_DEGREES;
287 MagDip = FGBFI::getMagDip() / SGD_RADIANS_TO_DEGREES;
288 CosDip = cos ( MagDip );
289 FrcN = CosDip * cos ( MagVar );
290 FrcE = CosDip * sin ( MagVar );
291 FrcU = sin ( MagDip );
292 /* Rotation occurs around acceleration axis,
293 but axis magnitude is irrelevant. So compute it. */
294 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
295 if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot );
297 /* Force applies to north marking on compass card */
298 EdgN = cos ( the_MH_err / SGD_RADIANS_TO_DEGREES );
299 EdgE = sin ( the_MH_err / SGD_RADIANS_TO_DEGREES );
301 /* Apply the force to the edge to get torques */
302 TrqN = EdgE * FrcU - EdgU * FrcE;
303 TrqE = EdgU * FrcN - EdgN * FrcU;
304 TrqU = EdgN * FrcE - EdgE * FrcN;
305 /* Select the component parallel to the axis */
306 Torque = ( TrqN * AccN +
308 TrqU * AccU ) * 5.0 / AccTot;
309 /* The magnetic compass has angular momentum,
310 so we apply a torque to it and wait */
312 { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
313 the_MH_err += dt * the_MH_degps;
315 if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else
316 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
317 the_MH_deg = FGBFI::getHeading () - the_MH_err;
320 /**************************
321 This is not actually correct, but provides a
322 scaling capability for the vacuum pump later on.
323 When we have a real engine model, we can ask it.
325 the_ENGINE_rpm = controls.get_throttle(0) * 26.0;
327 /**************************
328 First, we need to know what the static line is reporting,
329 which is a whole simulation area in itself. For now, we cheat.
330 We filter the actual value by one second to
331 account for the line impedance of the plumbing.
333 double static_inhg = altFtToPressInHg(FGBFI::getAltitude());
334 set_lowpass ( & the_STATIC_inhg, static_inhg, dt );
337 NO alternate static source error (student feature),
338 NO possibility of blockage (instructor feature),
339 NO slip-induced error, important for C172 for example.
342 /**************************
344 ICAO standard atmosphere MSL pressure is 1013.25 mb, and pressure
345 gradient is about 28 ft per mb at MSL increasing to about 32 at
346 5000 and 38 at 10000 ft.
347 Standard altimeters apply the subscale offset to the output altitude,
348 not to the input pressure; I don't know exactly what pressure gradient
349 they assume for this. I choose to make it accurate at low altitudes.
350 Remember, we are trying to simulate a real altimeter, not an ideal one.
352 set_lowpass ( & the_ALT_ft,
353 pressInHgToAltFt(the_STATIC_inhg) +
354 (the_ALT_datum_mb - 1013.25) * 28.0, /* accurate at low alt. */
355 dt * 10 ); /* smoothing time 0.1 s */
357 /**************************
358 The VSI case is a low-pass filter of the static line pressure.
359 The instrument reports the difference, scaled to approx ft.
360 NO option for student to break glass when static source fails.
361 NO capability for a fixed non-zero reading when level.
362 NO capability to have a scaling error of maybe a factor of two.
364 the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
365 * 10000.0; /* manual scaling factor */
366 set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
368 /**************************
369 The engine driven vacuum pump is directly attached
370 to the engine shaft, so each engine rotation pumps
371 a fixed volume. The amount of air in that volume
372 is determined by the vacuum line's internal pressure.
373 The instruments are essentially leaking air like
374 a fixed source impedance from atmospheric pressure.
375 The regulator provides a digital limit setting,
376 which is open circuit unless the pressure drop is big.
377 Thus, we can compute the vacuum line pressure directly.
378 We assume that there is negligible reservoir space.
379 NO failure of the pump supported (yet)
381 the_VACUUM_inhg = the_STATIC_inhg *
382 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
383 if ( the_VACUUM_inhg > 5.0 )
384 the_VACUUM_inhg = 5.0;
387 > I was merely going to do the engine rpm driven vacuum pump for both
388 > the AI and DG, have the gyros spin down down in power off descents,
389 > have it tumble when you exceed the usual pitch or bank limits,
390 > put in those insidious turning errors ... for now anyway.
392 if ( _UpdatesPending > 999999 )
393 the_DG_err = FGBFI::getMagVar();
394 the_DG_degps = 0.01; /* HACK! */
395 if (dt<1.0) the_DG_err += dt * the_DG_degps;
396 the_DG_deg = FGBFI::getHeading () - the_DG_err;
398 /**************************
399 Finished updates, now clear the timer
403 // cout << "0 Updates pending" << endl;
408 ////////////////////////////////////////////////////////////////////////
409 // Everything below is a transient hack; expect it to disappear
410 ////////////////////////////////////////////////////////////////////////
415 double FGSteam::get_HackGS_deg () {
416 if ( current_radiostack->get_nav1_inrange() &&
417 current_radiostack->get_nav1_has_gs() )
419 double x = current_radiostack->get_nav1_gs_dist();
420 double y = (FGBFI::getAltitude() - current_radiostack->get_nav1_elev())
422 double angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
423 return (current_radiostack->get_nav1_target_gs() - angle) * 5.0;
430 double FGSteam::get_HackVOR1_deg () {
433 if ( current_radiostack->get_nav1_inrange() ) {
434 r = current_radiostack->get_nav1_heading()
435 - current_radiostack->get_nav1_radial();
436 // cout << "Radial = " << current_radiostack->get_nav1_radial()
437 // << " Bearing = " << current_radiostack->get_nav1_heading()
440 if (r> 180.0) r-=360.0; else
441 if (r<-180.0) r+=360.0;
442 if ( fabs(r) > 90.0 )
443 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
444 // According to Robin Peel, the ILS is 4x more sensitive than a vor
445 if ( current_radiostack->get_nav1_loc() ) r *= 4.0;
454 double FGSteam::get_HackVOR2_deg () {
457 if ( current_radiostack->get_nav2_inrange() ) {
458 r = current_radiostack->get_nav2_heading()
459 - current_radiostack->get_nav2_radial();
460 // cout << "Radial = " << current_radiostack->get_nav1_radial()
461 // << " Bearing = " << current_radiostack->get_nav1_heading() << endl;
463 if (r> 180.0) r-=360.0; else
464 if (r<-180.0) r+=360.0;
465 if ( fabs(r) > 90.0 )
466 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
476 double FGSteam::get_HackOBS1_deg () {
477 return current_radiostack->get_nav1_radial();
481 double FGSteam::get_HackOBS2_deg () {
482 return current_radiostack->get_nav2_radial();
486 double FGSteam::get_HackADF_deg () {
487 static double last_r = 0;
490 if ( current_radiostack->get_adf_inrange() ) {
491 double r = current_radiostack->get_adf_heading() - FGBFI::getHeading();
493 // cout << "Radial = " << current_radiostack->get_adf_heading()
494 // << " Heading = " << FGBFI::getHeading() << endl;