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( FG_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 <Main/globals.hxx>
38 #include <NetworkOLK/features.hxx>
40 FG_USING_NAMESPACE(std);
42 #include "radiostack.hxx"
45 static bool isTied = false;
49 ////////////////////////////////////////////////////////////////////////
50 // Declare the functions that read the variables
51 ////////////////////////////////////////////////////////////////////////
53 // Anything that reads the BFI directly is not implemented at all!
56 double FGSteam::the_STATIC_inhg = 29.92;
57 double FGSteam::the_ALT_ft = 0.0;
58 double FGSteam::get_ALT_ft() { _CatchUp(); return the_ALT_ft; }
60 double FGSteam::get_ASI_kias() { return FGBFI::getAirspeed(); }
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.
101 #define DF1(getter) SGRawValueFunctions<double>(getter,0)
102 #define DF2(getter, setter) SGRawValueFunctions<double>(getter,setter)
104 void FGSteam::update ( int timesteps )
108 globals->get_props()->tie("/steam/airspeed",
109 DF1(FGSteam::get_ASI_kias));
110 globals->get_props()->tie("/steam/altitude",
111 DF1(FGSteam::get_ALT_ft));
112 globals->get_props()->tie("/steam/turn-rate",
113 DF1(FGSteam::get_TC_std));
114 globals->get_props()->tie("/steam/slip-skid",
115 DF1(FGSteam::get_TC_rad));
116 globals->get_props()->tie("/steam/vertical-speed",
117 DF1(FGSteam::get_VSI_fps));
118 globals->get_props()->tie("/steam/gyro-compass",
119 DF1(FGSteam::get_DG_deg));
120 globals->get_props()->tie("/steam/vor1",
121 DF1(FGSteam::get_HackVOR1_deg));
122 globals->get_props()->tie("/steam/vor2",
123 DF1(FGSteam::get_HackVOR2_deg));
124 globals->get_props()->tie("/steam/glidescope1",
125 DF1(FGSteam::get_HackGS_deg));
126 globals->get_props()->tie("/steam/adf",
127 DF1(FGSteam::get_HackADF_deg));
128 globals->get_props()->tie("/steam/gyro-compass-error",
129 DF2(FGSteam::get_DG_err,
130 FGSteam::set_DG_err));
131 globals->get_props()->tie("/steam/mag-compass",
132 DF1(FGSteam::get_MH_deg));
134 _UpdatesPending += timesteps;
141 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
145 { /* time went backwards; kill the filter */
148 { /* ignore mildly negative time */
152 { /* Normal mode of operation */
153 (*outthe) = (*outthe) * ( 1.0 - tc )
157 { /* Huge time step; assume filter has settled */
160 { /* Moderate time step; non linear response */
161 double keep = exp ( -tc );
162 // printf ( "ARP: Keep is %f\n", keep );
163 (*outthe) = (*outthe) * keep
164 + inthe * ( 1.0 - keep );
170 ////////////////////////////////////////////////////////////////////////
171 // Here the fun really begins
172 ////////////////////////////////////////////////////////////////////////
175 void FGSteam::_CatchUp()
176 { if ( _UpdatesPending != 0 )
177 { double dt = _UpdatesPending * 1.0 /
178 globals->get_options()->get_model_hz();
179 double AccN, AccE, AccU;
181 double d, the_ENGINE_rpm;
184 /**************************
185 There is the possibility that this is the first call.
186 If this is the case, we will emit the feature registrations
187 just to be on the safe side. Doing it more than once will
188 waste CPU time but doesn't hurt anything really.
190 if ( _UpdatesPending > 999999 )
191 { FGFeature::register_int ( "Avionics/NAV1/Localizer", &NAV1_LOC );
192 FGFeature::register_double ( "Avionics/NAV1/Latitude", &NAV1_Lat );
193 FGFeature::register_double ( "Avionics/NAV1/Longitude", &NAV1_Lon );
194 FGFeature::register_double ( "Avionics/NAV1/Radial", &NAV1_Rad );
195 FGFeature::register_double ( "Avionics/NAV1/Altitude", &NAV1_Alt );
196 FGFeature::register_int ( "Avionics/NAV2/Localizer", &NAV2_LOC );
197 FGFeature::register_double ( "Avionics/NAV2/Latitude", &NAV2_Lat );
198 FGFeature::register_double ( "Avionics/NAV2/Longitude", &NAV2_Lon );
199 FGFeature::register_double ( "Avionics/NAV2/Radial", &NAV2_Rad );
200 FGFeature::register_double ( "Avionics/NAV2/Altitude", &NAV2_Alt );
201 FGFeature::register_double ( "Avionics/ADF/Latitude", &ADF_Lat );
202 FGFeature::register_double ( "Avionics/ADF/Longitude", &ADF_Lon );
206 /**************************
207 Someone has called our update function and
208 it turns out that we are running somewhat behind.
209 Here, we recalculate everything for a 'dt' second step.
212 /**************************
213 The ball responds to the acceleration vector in the body
214 frame, only the components perpendicular to the longitudinal
215 axis of the aircraft. This is only related to actual
216 side slip for a symmetrical aircraft which is not touching
217 the ground and not changing its attitude. Math simplifies
218 by assuming (for small angles) arctan(x)=x in radians.
219 Obvious failure mode is the absence of liquid in the
220 tube, which is there to damp the motion, so that instead
221 the ball will bounce around, hitting the tube ends.
222 More subtle flaw is having it not move or a travel limit
223 occasionally due to some dirt in the tube or on the ball.
225 // the_TC_rad = - ( FGBFI::getSideSlip () ); /* incorrect */
226 d = - current_aircraft.fdm_state->get_A_Z_pilot();
228 set_lowpass ( & the_TC_rad,
229 current_aircraft.fdm_state->get_A_Y_pilot () / d,
232 /**************************
233 The rate of turn indication is from an electric gyro.
234 We should have it spin up with the master switch.
235 It is mounted at a funny angle so that it detects
236 both rate of bank (i.e. rolling into and out of turns)
237 and the rate of turn (i.e. how fast heading is changing).
239 set_lowpass ( & the_TC_std,
240 current_aircraft.fdm_state->get_Phi_dot ()
241 * RAD_TO_DEG / 20.0 +
242 current_aircraft.fdm_state->get_Psi_dot ()
243 * RAD_TO_DEG / 3.0 , dt );
245 /**************************
246 We want to know the pilot accelerations,
247 to compute the magnetic compass errors.
249 AccN = current_aircraft.fdm_state->get_V_dot_north();
250 AccE = current_aircraft.fdm_state->get_V_dot_east();
251 AccU = current_aircraft.fdm_state->get_V_dot_down()
253 if ( fabs(the_TC_rad) > 0.2 )
254 { /* Massive sideslip jams it; it stops turning */
256 the_MH_err = FGBFI::getHeading () - the_MH_deg;
258 { double MagDip, MagVar, CosDip;
259 double FrcN, FrcE, FrcU, AccTot;
260 double EdgN, EdgE, EdgU;
261 double TrqN, TrqE, TrqU, Torque;
262 /* Find a force vector towards exact magnetic north */
263 MagVar = FGBFI::getMagVar() / RAD_TO_DEG;
264 MagDip = FGBFI::getMagDip() / RAD_TO_DEG;
265 CosDip = cos ( MagDip );
266 FrcN = CosDip * cos ( MagVar );
267 FrcE = CosDip * sin ( MagVar );
268 FrcU = sin ( MagDip );
269 /* Rotation occurs around acceleration axis,
270 but axis magnitude is irrelevant. So compute it. */
271 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
272 if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot );
274 /* Force applies to north marking on compass card */
275 EdgN = cos ( the_MH_err / RAD_TO_DEG );
276 EdgE = sin ( the_MH_err / RAD_TO_DEG );
278 /* Apply the force to the edge to get torques */
279 TrqN = EdgE * FrcU - EdgU * FrcE;
280 TrqE = EdgU * FrcN - EdgN * FrcU;
281 TrqU = EdgN * FrcE - EdgE * FrcN;
282 /* Select the component parallel to the axis */
283 Torque = ( TrqN * AccN +
285 TrqU * AccU ) * 5.0 / AccTot;
286 /* The magnetic compass has angular momentum,
287 so we apply a torque to it and wait */
289 { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
290 the_MH_err += dt * the_MH_degps;
292 if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else
293 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
294 the_MH_deg = FGBFI::getHeading () - the_MH_err;
297 /**************************
298 This is not actually correct, but provides a
299 scaling capability for the vacuum pump later on.
300 When we have a real engine model, we can ask it.
302 the_ENGINE_rpm = controls.get_throttle(0) * 26.0;
304 /**************************
305 This is just temporary, until the static source works,
306 so we just filter the actual value by one second to
307 account for the line impedance of the plumbing.
309 set_lowpass ( & the_ALT_ft, FGBFI::getAltitude(), dt );
311 /**************************
312 First, we need to know what the static line is reporting,
313 which is a whole simulation area in itself. For now, we cheat.
315 the_STATIC_inhg = 29.92;
316 i = (int) the_ALT_ft;
318 { the_STATIC_inhg *= 0.707;
321 the_STATIC_inhg *= ( 1.0 - 0.293 * i / 9000.0 );
324 NO alternate static source error (student feature),
325 NO possibility of blockage (instructor feature),
326 NO slip-induced error, important for C172 for example.
329 /**************************
330 The VSI case is a low-pass filter of the static line pressure.
331 The instrument reports the difference, scaled to approx ft.
332 NO option for student to break glass when static source fails.
333 NO capability for a fixed non-zero reading when level.
334 NO capability to have a scaling error of maybe a factor of two.
336 the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
337 * 10000.0; /* manual scaling factor */
338 set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
340 /**************************
341 The engine driven vacuum pump is directly attached
342 to the engine shaft, so each engine rotation pumps
343 a fixed volume. The amount of air in that volume
344 is determined by the vacuum line's internal pressure.
345 The instruments are essentially leaking air like
346 a fixed source impedance from atmospheric pressure.
347 The regulator provides a digital limit setting,
348 which is open circuit unless the pressure drop is big.
349 Thus, we can compute the vacuum line pressure directly.
350 We assume that there is negligible reservoir space.
351 NO failure of the pump supported (yet)
353 the_VACUUM_inhg = the_STATIC_inhg *
354 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
355 if ( the_VACUUM_inhg > 5.0 )
356 the_VACUUM_inhg = 5.0;
359 > I was merely going to do the engine rpm driven vacuum pump for both
360 > the AI and DG, have the gyros spin down down in power off descents,
361 > have it tumble when you exceed the usual pitch or bank limits,
362 > put in those insidious turning errors ... for now anyway.
364 if ( _UpdatesPending > 999999 )
365 the_DG_err = FGBFI::getMagVar();
366 the_DG_degps = 0.01; /* HACK! */
367 if (dt<1.0) the_DG_err += dt * the_DG_degps;
368 the_DG_deg = FGBFI::getHeading () - the_DG_err;
370 /**************************
371 Finished updates, now clear the timer
375 // cout << "0 Updates pending" << endl;
380 ////////////////////////////////////////////////////////////////////////
381 // Everything below is a transient hack; expect it to disappear
382 ////////////////////////////////////////////////////////////////////////
385 double FGSteam::get_HackGS_deg () {
386 if ( current_radiostack->get_nav1_inrange() &&
387 current_radiostack->get_nav1_has_gs() )
389 double x = current_radiostack->get_nav1_gs_dist();
390 double y = (FGBFI::getAltitude() - current_radiostack->get_nav1_elev())
392 double angle = atan2( y, x ) * RAD_TO_DEG;
393 return (current_radiostack->get_nav1_target_gs() - angle) * 5.0;
400 double FGSteam::get_HackVOR1_deg () {
403 if ( current_radiostack->get_nav1_inrange() ) {
406 if ( current_radiostack->get_nav1_loc() ) {
407 // localizer doesn't need magvar offset
408 r = current_radiostack->get_nav1_heading()
409 - current_radiostack->get_nav1_radial();
411 r = current_radiostack->get_nav1_heading() - FGBFI::getMagVar()
412 - current_radiostack->get_nav1_radial();
415 r = current_radiostack->get_nav1_heading()
416 - current_radiostack->get_nav1_radial();
417 // cout << "Radial = " << current_radiostack->get_nav1_radial()
418 // << " Bearing = " << current_radiostack->get_nav1_heading()
421 if (r> 180.0) r-=360.0; else
422 if (r<-180.0) r+=360.0;
423 if ( fabs(r) > 90.0 )
424 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
425 // According to Robin Peel, the ILS is 4x more sensitive than a vor
426 if ( current_radiostack->get_nav1_loc() ) r *= 4.0;
435 double FGSteam::get_HackVOR2_deg () {
438 if ( current_radiostack->get_nav2_inrange() ) {
441 if ( current_radiostack->get_nav2_loc() ) {
442 // localizer doesn't need magvar offset
443 r = current_radiostack->get_nav2_heading()
444 - current_radiostack->get_nav2_radial();
446 r = current_radiostack->get_nav2_heading() - FGBFI::getMagVar()
447 - current_radiostack->get_nav2_radial();
450 r = current_radiostack->get_nav2_heading()
451 - current_radiostack->get_nav2_radial();
452 // cout << "Radial = " << current_radiostack->get_nav1_radial()
453 // << " Bearing = " << current_radiostack->get_nav1_heading() << endl;
455 if (r> 180.0) r-=360.0; else
456 if (r<-180.0) r+=360.0;
457 if ( fabs(r) > 90.0 )
458 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
467 double FGSteam::get_HackOBS1_deg () {
468 return current_radiostack->get_nav1_radial();
472 double FGSteam::get_HackOBS2_deg () {
473 return current_radiostack->get_nav2_radial();
477 double FGSteam::get_HackADF_deg () {
480 if ( current_radiostack->get_adf_inrange() ) {
481 r = current_radiostack->get_adf_heading() - FGBFI::getHeading();
483 // cout << "Radial = " << current_radiostack->get_adf_heading()
484 // << " Heading = " << FGBFI::getHeading() << endl;