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 <NetworkOLK/features.hxx>
36 SG_USING_NAMESPACE(std);
38 #include "radiostack.hxx"
41 static bool isTied = false;
45 ////////////////////////////////////////////////////////////////////////
46 // Declare the functions that read the variables
47 ////////////////////////////////////////////////////////////////////////
49 double FGSteam::the_STATIC_inhg = 29.92;
50 double FGSteam::the_ALT_ft = 0.0; // Indicated altitude
51 double FGSteam::get_ALT_ft() { _CatchUp(); return the_ALT_ft; }
53 double FGSteam::the_ALT_datum_mb = 1013.0;
54 double FGSteam::get_ALT_datum_mb() { return the_ALT_datum_mb; }
56 void FGSteam::set_ALT_datum_mb ( double datum_mb ) {
57 the_ALT_datum_mb = datum_mb;
60 double FGSteam::get_ASI_kias() { return fgGetDouble("/velocities/airspeed"); }
62 double FGSteam::the_VSI_case = 29.92;
63 double FGSteam::the_VSI_fps = 0.0;
64 double FGSteam::get_VSI_fps() { _CatchUp(); return the_VSI_fps; }
66 double FGSteam::the_VACUUM_inhg = 0.0;
67 double FGSteam::get_VACUUM_inhg() { _CatchUp(); return the_VACUUM_inhg; }
69 double FGSteam::the_MH_err = 0.0;
70 double FGSteam::the_MH_deg = 0.0;
71 double FGSteam::the_MH_degps = 0.0;
72 double FGSteam::get_MH_deg () { _CatchUp(); return the_MH_deg; }
74 double FGSteam::the_DG_err = 0.0;
75 double FGSteam::the_DG_deg = 0.0;
76 double FGSteam::the_DG_degps = 0.0;
77 double FGSteam::the_DG_inhg = 0.0;
78 double FGSteam::get_DG_deg () { _CatchUp(); return the_DG_deg; }
79 double FGSteam::get_DG_err () { _CatchUp(); return the_DG_err; }
81 void FGSteam::set_DG_err ( double approx_magvar ) {
82 the_DG_err = approx_magvar;
85 double FGSteam::the_TC_rad = 0.0;
86 double FGSteam::the_TC_std = 0.0;
87 double FGSteam::get_TC_rad () { _CatchUp(); return the_TC_rad; }
88 double FGSteam::get_TC_std () { _CatchUp(); return the_TC_std; }
91 ////////////////////////////////////////////////////////////////////////
92 // Recording the current time
93 ////////////////////////////////////////////////////////////////////////
96 int FGSteam::_UpdatesPending = 1000000; /* Forces filter to reset */
99 // FIXME: no need to use static
100 // functions any longer.
102 void FGSteam::update ( int timesteps )
106 fgTie("/steam/airspeed", FGSteam::get_ASI_kias);
107 fgTie("/steam/altitude", FGSteam::get_ALT_ft);
108 fgTie("/steam/altimeter-datum-mb",
109 FGSteam::get_ALT_datum_mb, FGSteam::set_ALT_datum_mb,
110 false); /* don't modify the value */
111 fgTie("/steam/turn-rate", FGSteam::get_TC_std);
112 fgTie("/steam/slip-skid", FGSteam::get_TC_rad);
113 fgTie("/steam/vertical-speed", FGSteam::get_VSI_fps);
114 fgTie("/steam/gyro-compass", FGSteam::get_DG_deg);
115 fgTie("/steam/adf", FGSteam::get_HackADF_deg);
116 fgTie("/steam/gyro-compass-error",
117 FGSteam::get_DG_err, FGSteam::set_DG_err,
118 false); /* don't modify the value */
119 fgTie("/steam/mag-compass", FGSteam::get_MH_deg);
121 _UpdatesPending += timesteps;
125 /* tc should be (elapsed_time_between_updates / desired_smoothing_time) */
126 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
130 { /* time went backwards; kill the filter */
133 { /* ignore mildly negative time */
137 { /* Normal mode of operation; fast approximation to exp(-tc) */
138 (*outthe) = (*outthe) * ( 1.0 - tc )
142 { /* Huge time step; assume filter has settled */
145 { /* Moderate time step; non linear response */
146 double keep = exp ( -tc );
147 // printf ( "ARP: Keep is %f\n", keep );
148 (*outthe) = (*outthe) * keep
149 + inthe * ( 1.0 - keep );
154 #define INHG_TO_MB 33.86388 /* Inches_of_mercury * INHG_TO_MB == millibars. */
156 // Convert air pressure to altitude by ICAO Standard Atmosphere
157 double pressInHgToAltFt(double p_inhg)
159 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
160 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
161 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
162 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
164 if (p_inhg > p_Tr) // 0.0 to 11.0 km
165 return (1.0 - pow((p_inhg / P_0), 1.0 / 5.2558797)) / 6.8755856e-6;
166 return h_Tr + log10(p_inhg / p_Tr) / -4.806346e-5; // 11.0 to 20.0 km
167 // We could put more code for higher altitudes here.
171 // Convert altitude to air pressure by ICAO Standard Atmosphere
172 double altFtToPressInHg(double alt_ft)
174 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
175 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
176 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
177 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
179 if (alt_ft < h_Tr) // 0.0 to 11.0 km
180 return P_0 * pow(1.0 - 6.8755856e-6 * alt_ft, 5.2558797);
181 return p_Tr * exp(-4.806346e-5 * (alt_ft - h_Tr)); // 11.0 to 20.0 km
182 // We could put more code for higher altitudes here.
187 ////////////////////////////////////////////////////////////////////////
188 // Here the fun really begins
189 ////////////////////////////////////////////////////////////////////////
192 void FGSteam::_CatchUp()
193 { if ( _UpdatesPending != 0 )
194 { double dt = _UpdatesPending * 1.0 /
195 fgGetInt("/sim/model-hz"); // FIXME: inefficient
196 double AccN, AccE, AccU;
198 double d, the_ENGINE_rpm;
201 /**************************
202 There is the possibility that this is the first call.
203 If this is the case, we will emit the feature registrations
204 just to be on the safe side. Doing it more than once will
205 waste CPU time but doesn't hurt anything really.
207 if ( _UpdatesPending > 999999 )
208 { FGFeature::register_int ( "Avionics/NAV1/Localizer", &NAV1_LOC );
209 FGFeature::register_double ( "Avionics/NAV1/Latitude", &NAV1_Lat );
210 FGFeature::register_double ( "Avionics/NAV1/Longitude", &NAV1_Lon );
211 FGFeature::register_double ( "Avionics/NAV1/Radial", &NAV1_Rad );
212 FGFeature::register_double ( "Avionics/NAV1/Altitude", &NAV1_Alt );
213 FGFeature::register_int ( "Avionics/NAV2/Localizer", &NAV2_LOC );
214 FGFeature::register_double ( "Avionics/NAV2/Latitude", &NAV2_Lat );
215 FGFeature::register_double ( "Avionics/NAV2/Longitude", &NAV2_Lon );
216 FGFeature::register_double ( "Avionics/NAV2/Radial", &NAV2_Rad );
217 FGFeature::register_double ( "Avionics/NAV2/Altitude", &NAV2_Alt );
218 FGFeature::register_double ( "Avionics/ADF/Latitude", &ADF_Lat );
219 FGFeature::register_double ( "Avionics/ADF/Longitude", &ADF_Lon );
223 /**************************
224 Someone has called our update function and
225 it turns out that we are running somewhat behind.
226 Here, we recalculate everything for a 'dt' second step.
229 /**************************
230 The ball responds to the acceleration vector in the body
231 frame, only the components perpendicular to the longitudinal
232 axis of the aircraft. This is only related to actual
233 side slip for a symmetrical aircraft which is not touching
234 the ground and not changing its attitude. Math simplifies
235 by assuming (for small angles) arctan(x)=x in radians.
236 Obvious failure mode is the absence of liquid in the
237 tube, which is there to damp the motion, so that instead
238 the ball will bounce around, hitting the tube ends.
239 More subtle flaw is having it not move or a travel limit
240 occasionally due to some dirt in the tube or on the ball.
242 d = - current_aircraft.fdm_state->get_A_Z_pilot();
244 set_lowpass ( & the_TC_rad,
245 current_aircraft.fdm_state->get_A_Y_pilot () / d,
248 /**************************
249 The rate of turn indication is from an electric gyro.
250 We should have it spin up with the master switch.
251 It is mounted at a funny angle so that it detects
252 both rate of bank (i.e. rolling into and out of turns)
253 and the rate of turn (i.e. how fast heading is changing).
255 set_lowpass ( & the_TC_std,
256 current_aircraft.fdm_state->get_Phi_dot ()
257 * SGD_RADIANS_TO_DEGREES / 20.0 +
258 current_aircraft.fdm_state->get_Psi_dot ()
259 * SGD_RADIANS_TO_DEGREES / 3.0 , dt );
261 /**************************
262 We want to know the pilot accelerations,
263 to compute the magnetic compass errors.
265 AccN = current_aircraft.fdm_state->get_V_dot_north();
266 AccE = current_aircraft.fdm_state->get_V_dot_east();
267 AccU = current_aircraft.fdm_state->get_V_dot_down()
269 if ( fabs(the_TC_rad) > 0.2 )
270 { /* Massive sideslip jams it; it stops turning */
272 the_MH_err = fgGetDouble("/orientation/heading") - the_MH_deg;
274 { double MagDip, MagVar, CosDip;
275 double FrcN, FrcE, FrcU, AccTot;
276 double EdgN, EdgE, EdgU;
277 double TrqN, TrqE, TrqU, Torque;
278 /* Find a force vector towards exact magnetic north */
279 MagVar = fgGetDouble("/environment/magnetic-variation")
280 / SGD_RADIANS_TO_DEGREES;
281 MagDip = fgGetDouble("/environment/magnetic-dip")
282 / SGD_RADIANS_TO_DEGREES;
283 CosDip = cos ( MagDip );
284 FrcN = CosDip * cos ( MagVar );
285 FrcE = CosDip * sin ( MagVar );
286 FrcU = sin ( MagDip );
287 /* Rotation occurs around acceleration axis,
288 but axis magnitude is irrelevant. So compute it. */
289 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
290 if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot );
292 /* Force applies to north marking on compass card */
293 EdgN = cos ( the_MH_err / SGD_RADIANS_TO_DEGREES );
294 EdgE = sin ( the_MH_err / SGD_RADIANS_TO_DEGREES );
296 /* Apply the force to the edge to get torques */
297 TrqN = EdgE * FrcU - EdgU * FrcE;
298 TrqE = EdgU * FrcN - EdgN * FrcU;
299 TrqU = EdgN * FrcE - EdgE * FrcN;
300 /* Select the component parallel to the axis */
301 Torque = ( TrqN * AccN +
303 TrqU * AccU ) * 5.0 / AccTot;
304 /* The magnetic compass has angular momentum,
305 so we apply a torque to it and wait */
307 { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
308 the_MH_err += dt * the_MH_degps;
310 if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else
311 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
312 the_MH_deg = fgGetDouble("/orientation/heading") - the_MH_err;
315 /**************************
316 This is not actually correct, but provides a
317 scaling capability for the vacuum pump later on.
318 When we have a real engine model, we can ask it.
320 the_ENGINE_rpm = controls.get_throttle(0) * 26.0;
322 /**************************
323 First, we need to know what the static line is reporting,
324 which is a whole simulation area in itself. For now, we cheat.
325 We filter the actual value by one second to
326 account for the line impedance of the plumbing.
329 = altFtToPressInHg(fgGetDouble("/position/altitude"));
330 set_lowpass ( & the_STATIC_inhg, static_inhg, dt );
333 NO alternate static source error (student feature),
334 NO possibility of blockage (instructor feature),
335 NO slip-induced error, important for C172 for example.
338 /**************************
340 ICAO standard atmosphere MSL pressure is 1013.25 mb, and pressure
341 gradient is about 28 ft per mb at MSL increasing to about 32 at
342 5000 and 38 at 10000 ft.
343 Standard altimeters apply the subscale offset to the output altitude,
344 not to the input pressure; I don't know exactly what pressure gradient
345 they assume for this. I choose to make it accurate at low altitudes.
346 Remember, we are trying to simulate a real altimeter, not an ideal one.
348 set_lowpass ( & the_ALT_ft,
349 pressInHgToAltFt(the_STATIC_inhg) +
350 (the_ALT_datum_mb - 1013.25) * 28.0, /* accurate at low alt. */
351 dt * 10 ); /* smoothing time 0.1 s */
353 /**************************
354 The VSI case is a low-pass filter of the static line pressure.
355 The instrument reports the difference, scaled to approx ft.
356 NO option for student to break glass when static source fails.
357 NO capability for a fixed non-zero reading when level.
358 NO capability to have a scaling error of maybe a factor of two.
360 the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
361 * 10000.0; /* manual scaling factor */
362 set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
364 /**************************
365 The engine driven vacuum pump is directly attached
366 to the engine shaft, so each engine rotation pumps
367 a fixed volume. The amount of air in that volume
368 is determined by the vacuum line's internal pressure.
369 The instruments are essentially leaking air like
370 a fixed source impedance from atmospheric pressure.
371 The regulator provides a digital limit setting,
372 which is open circuit unless the pressure drop is big.
373 Thus, we can compute the vacuum line pressure directly.
374 We assume that there is negligible reservoir space.
375 NO failure of the pump supported (yet)
377 the_VACUUM_inhg = the_STATIC_inhg *
378 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
379 if ( the_VACUUM_inhg > 5.0 )
380 the_VACUUM_inhg = 5.0;
383 > I was merely going to do the engine rpm driven vacuum pump for both
384 > the AI and DG, have the gyros spin down down in power off descents,
385 > have it tumble when you exceed the usual pitch or bank limits,
386 > put in those insidious turning errors ... for now anyway.
388 if ( _UpdatesPending > 999999 )
389 the_DG_err = fgGetDouble("/environment/magnetic-variation");
390 the_DG_degps = 0.01; /* HACK! */
391 if (dt<1.0) the_DG_err += dt * the_DG_degps;
392 the_DG_deg = fgGetDouble("/orientation/heading") - the_DG_err;
394 /**************************
395 Finished updates, now clear the timer
399 // cout << "0 Updates pending" << endl;
404 ////////////////////////////////////////////////////////////////////////
405 // Everything below is a transient hack; expect it to disappear
406 ////////////////////////////////////////////////////////////////////////
411 double FGSteam::get_HackGS_deg () {
412 if ( current_radiostack->get_nav1_inrange() &&
413 current_radiostack->get_nav1_has_gs() )
415 double x = current_radiostack->get_nav1_gs_dist();
416 double y = (fgGetDouble("/position/altitude")
417 - current_radiostack->get_nav1_elev())
419 double angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
420 return (current_radiostack->get_nav1_target_gs() - angle) * 5.0;
427 double FGSteam::get_HackVOR1_deg () {
430 if ( current_radiostack->get_nav1_inrange() ) {
431 r = current_radiostack->get_nav1_heading()
432 - current_radiostack->get_nav1_radial();
433 // cout << "Radial = " << current_radiostack->get_nav1_radial()
434 // << " Bearing = " << current_radiostack->get_nav1_heading()
437 if (r> 180.0) r-=360.0; else
438 if (r<-180.0) r+=360.0;
439 if ( fabs(r) > 90.0 )
440 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
441 // According to Robin Peel, the ILS is 4x more sensitive than a vor
442 if ( current_radiostack->get_nav1_loc() ) r *= 4.0;
451 double FGSteam::get_HackVOR2_deg () {
454 if ( current_radiostack->get_nav2_inrange() ) {
455 r = current_radiostack->get_nav2_heading()
456 - current_radiostack->get_nav2_radial();
457 // cout << "Radial = " << current_radiostack->get_nav1_radial()
458 // << " Bearing = " << current_radiostack->get_nav1_heading() << endl;
460 if (r> 180.0) r-=360.0; else
461 if (r<-180.0) r+=360.0;
462 if ( fabs(r) > 90.0 )
463 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
473 double FGSteam::get_HackOBS1_deg () {
474 return current_radiostack->get_nav1_radial();
478 double FGSteam::get_HackOBS2_deg () {
479 return current_radiostack->get_nav2_radial();
483 double FGSteam::get_HackADF_deg () {
484 static double last_r = 0;
487 if ( current_radiostack->get_adf_inrange() ) {
488 double r = current_radiostack->get_adf_heading()
489 - fgGetDouble("orientation/heading");
491 // cout << "Radial = " << current_radiostack->get_adf_heading()
492 // << " Heading = " << FGBFI::getHeading() << endl;