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 // cout << current_aircraft.fdm_state->get_A_Z_pilot() << endl;
322 // cout << "Ay = " << current_aircraft.fdm_state->get_A_Y_pilot()
323 // << " Az = " << current_aircraft.fdm_state->get_A_Z_pilot() << endl;
324 d = -current_aircraft.fdm_state->get_A_Z_pilot();
326 set_lowpass ( & the_TC_rad,
327 current_aircraft.fdm_state->get_A_Y_pilot () / d,
330 /**************************
331 The rate of turn indication is from an electric gyro.
332 We should have it spin up with the master switch.
333 It is mounted at a funny angle so that it detects
334 both rate of bank (i.e. rolling into and out of turns)
335 and the rate of turn (i.e. how fast heading is changing).
337 set_lowpass ( & the_TC_std,
338 current_aircraft.fdm_state->get_Phi_dot ()
339 * SGD_RADIANS_TO_DEGREES / 20.0 +
340 current_aircraft.fdm_state->get_Psi_dot ()
341 * SGD_RADIANS_TO_DEGREES / 3.0 , dt );
343 /**************************
344 We want to know the pilot accelerations,
345 to compute the magnetic compass errors.
347 AccN = current_aircraft.fdm_state->get_V_dot_north();
348 AccE = current_aircraft.fdm_state->get_V_dot_east();
349 AccU = current_aircraft.fdm_state->get_V_dot_down()
350 - 9.81 * SG_METER_TO_FEET;
351 if ( fabs(the_TC_rad) > 0.2 /* 2.0 */ )
352 { /* Massive sideslip jams it; it stops turning */
354 the_MH_err = _heading_deg_node->getDoubleValue() - the_MH_deg;
356 { double MagDip, MagVar, CosDip;
357 double FrcN, FrcE, FrcU, AccTot;
358 double EdgN, EdgE, EdgU;
359 double TrqN, TrqE, TrqU, Torque;
360 /* Find a force vector towards exact magnetic north */
361 MagVar = _mag_var_deg_node->getDoubleValue()
362 / SGD_RADIANS_TO_DEGREES;
363 MagDip = _mag_var_deg_node->getDoubleValue()
364 / SGD_RADIANS_TO_DEGREES;
365 CosDip = cos ( MagDip );
366 FrcN = CosDip * cos ( MagVar );
367 FrcE = CosDip * sin ( MagVar );
368 FrcU = sin ( MagDip );
369 /* Rotation occurs around acceleration axis,
370 but axis magnitude is irrelevant. So compute it. */
371 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
372 if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot );
374 /* Force applies to north marking on compass card */
375 EdgN = cos ( the_MH_err / SGD_RADIANS_TO_DEGREES );
376 EdgE = sin ( the_MH_err / SGD_RADIANS_TO_DEGREES );
378 /* Apply the force to the edge to get torques */
379 TrqN = EdgE * FrcU - EdgU * FrcE;
380 TrqE = EdgU * FrcN - EdgN * FrcU;
381 TrqU = EdgN * FrcE - EdgE * FrcN;
382 /* Select the component parallel to the axis */
383 Torque = ( TrqN * AccN +
385 TrqU * AccU ) * 5.0 / AccTot;
386 /* The magnetic compass has angular momentum,
387 so we apply a torque to it and wait */
389 { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
390 the_MH_err += dt * the_MH_degps;
392 if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else
393 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
394 the_MH_deg = _heading_deg_node->getDoubleValue() - the_MH_err;
397 /**************************
398 This is not actually correct, but provides a
399 scaling capability for the vacuum pump later on.
400 When we have a real engine model, we can ask it.
402 the_ENGINE_rpm = _engine_0_rpm_node->getDoubleValue();
404 /**************************
405 First, we need to know what the static line is reporting,
406 which is a whole simulation area in itself.
407 We filter the actual value by one second to
408 account for the line impedance of the plumbing.
411 sgVec3 plane_pos = { cur_fdm_state->get_Latitude(),
412 cur_fdm_state->get_Longitude(),
413 cur_fdm_state->get_Altitude() * SG_FEET_TO_METER };
414 double static_inhg = WeatherDatabase->get(plane_pos).AirPressure *
417 double static_inhg = _pressure_inhg_node->getDoubleValue();
420 set_lowpass ( & the_STATIC_inhg, static_inhg, dt );
423 NO alternate static source error (student feature),
424 NO possibility of blockage (instructor feature),
425 NO slip-induced error, important for C172 for example.
428 /**************************
430 ICAO standard atmosphere MSL pressure is 1013.25 mb, and pressure
431 gradient is about 28 ft per mb at MSL increasing to about 32 at
432 5000 and 38 at 10000 ft.
433 Standard altimeters apply the subscale offset to the output altitude,
434 not to the input pressure; I don't know exactly what pressure gradient
435 they assume for this. I choose to make it accurate at low altitudes.
436 Remember, we are trying to simulate a real altimeter, not an ideal one.
438 set_lowpass ( & the_ALT_ft,
439 pressInHgToAltFt(the_STATIC_inhg) +
440 (the_ALT_datum_mb - 1013.25) * 28.0, /* accurate at low alt. */
441 dt * 10 ); /* smoothing time 0.1 s */
443 /**************************
444 The VSI case is a low-pass filter of the static line pressure.
445 The instrument reports the difference, scaled to approx ft.
446 NO option for student to break glass when static source fails.
447 NO capability for a fixed non-zero reading when level.
448 NO capability to have a scaling error of maybe a factor of two.
450 the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
451 * 10000.0; /* manual scaling factor */
452 set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
454 /**************************
455 The engine driven vacuum pump is directly attached
456 to the engine shaft, so each engine rotation pumps
457 a fixed volume. The amount of air in that volume
458 is determined by the vacuum line's internal pressure.
459 The instruments are essentially leaking air like
460 a fixed source impedance from atmospheric pressure.
461 The regulator provides a digital limit setting,
462 which is open circuit unless the pressure drop is big.
463 Thus, we can compute the vacuum line pressure directly.
464 We assume that there is negligible reservoir space.
465 NO failure of the pump supported (yet)
467 the_VACUUM_inhg = the_STATIC_inhg *
468 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
469 if ( the_VACUUM_inhg > 5.0 )
470 the_VACUUM_inhg = 5.0;
473 > I was merely going to do the engine rpm driven vacuum pump for both
474 > the AI and DG, have the gyros spin down down in power off descents,
475 > have it tumble when you exceed the usual pitch or bank limits,
476 > put in those insidious turning errors ... for now anyway.
478 if ( _UpdateTimePending > 999999 )
479 the_DG_err = fgGetDouble("/environment/magnetic-variation-deg");
480 the_DG_degps = 0.01; /* HACK! */
481 if (dt<1.0) the_DG_err += dt * the_DG_degps;
482 the_DG_deg = _heading_deg_node->getDoubleValue() - the_DG_err;
484 /**************************
485 Finished updates, now clear the timer
487 _UpdateTimePending = 0;
489 // cout << "0 Updates pending" << endl;
494 ////////////////////////////////////////////////////////////////////////
495 // Everything below is a transient hack; expect it to disappear
496 ////////////////////////////////////////////////////////////////////////
498 double FGSteam::get_HackOBS1_deg () const
500 return current_radiostack->get_navcom1()->get_nav_radial();
503 double FGSteam::get_HackOBS2_deg () const
505 return current_radiostack->get_navcom2()->get_nav_radial();