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>
34 #include <Main/fg_props.hxx>
35 #include <Aircraft/aircraft.hxx>
37 # include <WeatherCM/FGLocalWeatherDatabase.h>
39 # include <Environment/environment_mgr.hxx>
40 # include <Environment/environment.hxx>
43 SG_USING_NAMESPACE(std);
45 #include "radiostack.hxx"
48 static bool isTied = false;
52 ////////////////////////////////////////////////////////////////////////
53 // Constructor and destructor.
54 ////////////////////////////////////////////////////////////////////////
59 the_ALT_datum_mb(1013.0),
62 the_STATIC_inhg(29.92),
73 _UpdateTimePending(1000000)
84 _heading_deg_node = fgGetNode("/orientation/heading-deg", true);
85 _mag_var_deg_node = fgGetNode("/environment/magnetic-variation-deg", true);
86 _mag_dip_deg_node = fgGetNode("/environment/magnetic-dip-deg", true);
87 _engine_0_rpm_node = fgGetNode("/engines/engine[0]/rpm", true);
88 _pressure_inhg_node = fgGetNode("environment/pressure-inhg", true);
92 FGSteam::update (double dt_sec)
94 _UpdateTimePending += dt_sec;
101 fgTie("/steam/airspeed-kt", this, &FGSteam::get_ASI_kias);
102 fgSetArchivable("/steam/airspeed-kt");
103 fgTie("/steam/altitude-ft", this, &FGSteam::get_ALT_ft);
104 fgSetArchivable("/steam/altitude-ft");
105 fgTie("/steam/altimeter-datum-mb", this,
106 &FGSteam::get_ALT_datum_mb, &FGSteam::set_ALT_datum_mb,
107 false); /* don't modify the value */
108 fgSetArchivable("/steam/altimeter-datum-mb");
109 fgTie("/steam/turn-rate", this, &FGSteam::get_TC_std);
110 fgSetArchivable("/steam/turn-rate");
111 fgTie("/steam/slip-skid",this, &FGSteam::get_TC_rad);
112 fgSetArchivable("/steam/slip-skid");
113 fgTie("/steam/vertical-speed-fps", this, &FGSteam::get_VSI_fps);
114 fgSetArchivable("/steam/vertical-speed-fps");
115 fgTie("/steam/gyro-compass-deg", this, &FGSteam::get_DG_deg);
116 fgSetArchivable("/steam/gyro-compass-deg");
117 // fgTie("/steam/adf-deg", FGSteam::get_HackADF_deg);
118 // fgSetArchivable("/steam/adf-deg");
119 fgTie("/steam/gyro-compass-error-deg", this,
120 &FGSteam::get_DG_err, &FGSteam::set_DG_err,
121 false); /* don't modify the value */
122 fgSetArchivable("/steam/gyro-compass-error-deg");
123 fgTie("/steam/mag-compass-deg", this, &FGSteam::get_MH_deg);
124 fgSetArchivable("/steam/mag-compass-deg");
130 fgUntie("/steam/airspeed-kt");
131 fgUntie("/steam/altitude-ft");
132 fgUntie("/steam/altimeter-datum-mb");
133 fgUntie("/steam/turn-rate");
134 fgUntie("/steam/slip-skid");
135 fgUntie("/steam/vertical-speed-fps");
136 fgUntie("/steam/gyro-compass-deg");
137 fgUntie("/steam/gyro-compass-error-deg");
138 fgUntie("/steam/mag-compass-deg");
143 ////////////////////////////////////////////////////////////////////////
144 // Declare the functions that read the variables
145 ////////////////////////////////////////////////////////////////////////
148 FGSteam::get_ALT_ft () const
154 FGSteam::get_ALT_datum_mb () const
156 return the_ALT_datum_mb;
160 FGSteam::set_ALT_datum_mb (double datum_mb)
162 the_ALT_datum_mb = datum_mb;
166 FGSteam::get_ASI_kias () const
168 return fgGetDouble("/velocities/airspeed-kt");
172 FGSteam::get_VSI_fps () const
178 FGSteam::get_VACUUM_inhg () const
180 return the_VACUUM_inhg;
184 FGSteam::get_MH_deg () const
190 FGSteam::get_DG_deg () const
196 FGSteam::get_DG_err () const
202 FGSteam::set_DG_err (double approx_magvar)
204 the_DG_err = approx_magvar;
208 FGSteam::get_TC_rad () const
214 FGSteam::get_TC_std () const
221 ////////////////////////////////////////////////////////////////////////
222 // Recording the current time
223 ////////////////////////////////////////////////////////////////////////
226 /* tc should be (elapsed_time_between_updates / desired_smoothing_time) */
227 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
231 { /* time went backwards; kill the filter */
234 { /* ignore mildly negative time */
238 { /* Normal mode of operation; fast approximation to exp(-tc) */
239 (*outthe) = (*outthe) * ( 1.0 - tc )
243 { /* Huge time step; assume filter has settled */
246 { /* Moderate time step; non linear response */
247 double keep = exp ( -tc );
248 // printf ( "ARP: Keep is %f\n", keep );
249 (*outthe) = (*outthe) * keep
250 + inthe * ( 1.0 - keep );
255 #define INHG_TO_MB 33.86388 /* Inches_of_mercury * INHG_TO_MB == millibars. */
257 // Convert air pressure to altitude by ICAO Standard Atmosphere
258 double pressInHgToAltFt(double p_inhg)
260 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
261 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
262 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
263 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
265 if (p_inhg > p_Tr) // 0.0 to 11.0 km
266 return (1.0 - pow((p_inhg / P_0), 1.0 / 5.2558797)) / 6.8755856e-6;
267 return h_Tr + log10(p_inhg / p_Tr) / -4.806346e-5; // 11.0 to 20.0 km
268 // We could put more code for higher altitudes here.
272 // Convert altitude to air pressure by ICAO Standard Atmosphere
273 double altFtToPressInHg(double alt_ft)
275 // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
276 const double P_0 = 29.92126; // Std. MSL pressure, inHg. (=1013.25 mb)
277 const double p_Tr = 0.2233609 * P_0; // Pressure at tropopause, same units.
278 const double h_Tr = 36089.24; // Alt of tropopause, ft. (=11.0 km)
280 if (alt_ft < h_Tr) // 0.0 to 11.0 km
281 return P_0 * pow(1.0 - 6.8755856e-6 * alt_ft, 5.2558797);
282 return p_Tr * exp(-4.806346e-5 * (alt_ft - h_Tr)); // 11.0 to 20.0 km
283 // We could put more code for higher altitudes here.
288 ////////////////////////////////////////////////////////////////////////
289 // Here the fun really begins
290 ////////////////////////////////////////////////////////////////////////
293 void FGSteam::_CatchUp()
295 if ( _UpdateTimePending != 0 )
297 double dt = _UpdateTimePending;
298 double AccN, AccE, AccU;
300 double d, the_ENGINE_rpm;
302 /**************************
303 Someone has called our update function and
304 it turns out that we are running somewhat behind.
305 Here, we recalculate everything for a 'dt' second step.
308 /**************************
309 The ball responds to the acceleration vector in the body
310 frame, only the components perpendicular to the longitudinal
311 axis of the aircraft. This is only related to actual
312 side slip for a symmetrical aircraft which is not touching
313 the ground and not changing its attitude. Math simplifies
314 by assuming (for small angles) arctan(x)=x in radians.
315 Obvious failure mode is the absence of liquid in the
316 tube, which is there to damp the motion, so that instead
317 the ball will bounce around, hitting the tube ends.
318 More subtle flaw is having it not move or a travel limit
319 occasionally due to some dirt in the tube or on the ball.
321 d = - current_aircraft.fdm_state->get_A_Z_pilot();
323 set_lowpass ( & the_TC_rad,
324 current_aircraft.fdm_state->get_A_Y_pilot () / d,
327 /**************************
328 The rate of turn indication is from an electric gyro.
329 We should have it spin up with the master switch.
330 It is mounted at a funny angle so that it detects
331 both rate of bank (i.e. rolling into and out of turns)
332 and the rate of turn (i.e. how fast heading is changing).
334 set_lowpass ( & the_TC_std,
335 current_aircraft.fdm_state->get_Phi_dot ()
336 * SGD_RADIANS_TO_DEGREES / 20.0 +
337 current_aircraft.fdm_state->get_Psi_dot ()
338 * SGD_RADIANS_TO_DEGREES / 3.0 , dt );
340 /**************************
341 We want to know the pilot accelerations,
342 to compute the magnetic compass errors.
344 AccN = current_aircraft.fdm_state->get_V_dot_north();
345 AccE = current_aircraft.fdm_state->get_V_dot_east();
346 AccU = current_aircraft.fdm_state->get_V_dot_down()
347 - 9.81 * SG_METER_TO_FEET;
348 if ( fabs(the_TC_rad) > 0.2 /* 2.0 */ )
349 { /* Massive sideslip jams it; it stops turning */
351 the_MH_err = _heading_deg_node->getDoubleValue() - the_MH_deg;
353 { double MagDip, MagVar, CosDip;
354 double FrcN, FrcE, FrcU, AccTot;
355 double EdgN, EdgE, EdgU;
356 double TrqN, TrqE, TrqU, Torque;
357 /* Find a force vector towards exact magnetic north */
358 MagVar = _mag_var_deg_node->getDoubleValue()
359 / SGD_RADIANS_TO_DEGREES;
360 MagDip = _mag_var_deg_node->getDoubleValue()
361 / SGD_RADIANS_TO_DEGREES;
362 CosDip = cos ( MagDip );
363 FrcN = CosDip * cos ( MagVar );
364 FrcE = CosDip * sin ( MagVar );
365 FrcU = sin ( MagDip );
366 /* Rotation occurs around acceleration axis,
367 but axis magnitude is irrelevant. So compute it. */
368 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
369 if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot );
371 /* Force applies to north marking on compass card */
372 EdgN = cos ( the_MH_err / SGD_RADIANS_TO_DEGREES );
373 EdgE = sin ( the_MH_err / SGD_RADIANS_TO_DEGREES );
375 /* Apply the force to the edge to get torques */
376 TrqN = EdgE * FrcU - EdgU * FrcE;
377 TrqE = EdgU * FrcN - EdgN * FrcU;
378 TrqU = EdgN * FrcE - EdgE * FrcN;
379 /* Select the component parallel to the axis */
380 Torque = ( TrqN * AccN +
382 TrqU * AccU ) * 5.0 / AccTot;
383 /* The magnetic compass has angular momentum,
384 so we apply a torque to it and wait */
386 { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
387 the_MH_err += dt * the_MH_degps;
389 if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else
390 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
391 the_MH_deg = _heading_deg_node->getDoubleValue() - the_MH_err;
394 /**************************
395 This is not actually correct, but provides a
396 scaling capability for the vacuum pump later on.
397 When we have a real engine model, we can ask it.
399 the_ENGINE_rpm = _engine_0_rpm_node->getDoubleValue();
401 /**************************
402 First, we need to know what the static line is reporting,
403 which is a whole simulation area in itself.
404 We filter the actual value by one second to
405 account for the line impedance of the plumbing.
408 sgVec3 plane_pos = { cur_fdm_state->get_Latitude(),
409 cur_fdm_state->get_Longitude(),
410 cur_fdm_state->get_Altitude() * SG_FEET_TO_METER };
411 double static_inhg = WeatherDatabase->get(plane_pos).AirPressure *
414 double static_inhg = _pressure_inhg_node->getDoubleValue();
417 set_lowpass ( & the_STATIC_inhg, static_inhg, dt );
420 NO alternate static source error (student feature),
421 NO possibility of blockage (instructor feature),
422 NO slip-induced error, important for C172 for example.
425 /**************************
427 ICAO standard atmosphere MSL pressure is 1013.25 mb, and pressure
428 gradient is about 28 ft per mb at MSL increasing to about 32 at
429 5000 and 38 at 10000 ft.
430 Standard altimeters apply the subscale offset to the output altitude,
431 not to the input pressure; I don't know exactly what pressure gradient
432 they assume for this. I choose to make it accurate at low altitudes.
433 Remember, we are trying to simulate a real altimeter, not an ideal one.
435 set_lowpass ( & the_ALT_ft,
436 pressInHgToAltFt(the_STATIC_inhg) +
437 (the_ALT_datum_mb - 1013.25) * 28.0, /* accurate at low alt. */
438 dt * 10 ); /* smoothing time 0.1 s */
440 /**************************
441 The VSI case is a low-pass filter of the static line pressure.
442 The instrument reports the difference, scaled to approx ft.
443 NO option for student to break glass when static source fails.
444 NO capability for a fixed non-zero reading when level.
445 NO capability to have a scaling error of maybe a factor of two.
447 the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
448 * 10000.0; /* manual scaling factor */
449 set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
451 /**************************
452 The engine driven vacuum pump is directly attached
453 to the engine shaft, so each engine rotation pumps
454 a fixed volume. The amount of air in that volume
455 is determined by the vacuum line's internal pressure.
456 The instruments are essentially leaking air like
457 a fixed source impedance from atmospheric pressure.
458 The regulator provides a digital limit setting,
459 which is open circuit unless the pressure drop is big.
460 Thus, we can compute the vacuum line pressure directly.
461 We assume that there is negligible reservoir space.
462 NO failure of the pump supported (yet)
464 the_VACUUM_inhg = the_STATIC_inhg *
465 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
466 if ( the_VACUUM_inhg > 5.0 )
467 the_VACUUM_inhg = 5.0;
470 > I was merely going to do the engine rpm driven vacuum pump for both
471 > the AI and DG, have the gyros spin down down in power off descents,
472 > have it tumble when you exceed the usual pitch or bank limits,
473 > put in those insidious turning errors ... for now anyway.
475 if ( _UpdateTimePending > 999999 )
476 the_DG_err = fgGetDouble("/environment/magnetic-variation-deg");
477 the_DG_degps = 0.01; /* HACK! */
478 if (dt<1.0) the_DG_err += dt * the_DG_degps;
479 the_DG_deg = _heading_deg_node->getDoubleValue() - the_DG_err;
481 /**************************
482 Finished updates, now clear the timer
484 _UpdateTimePending = 0;
486 // cout << "0 Updates pending" << endl;
491 ////////////////////////////////////////////////////////////////////////
492 // Everything below is a transient hack; expect it to disappear
493 ////////////////////////////////////////////////////////////////////////
495 double FGSteam::get_HackOBS1_deg () const
497 return current_radiostack->get_nav1_radial();
500 double FGSteam::get_HackOBS2_deg () const
502 return current_radiostack->get_nav2_radial();