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 <NetworkOLK/features.hxx>
39 FG_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;
57 double FGSteam::get_ALT_ft() { _CatchUp(); return the_ALT_ft; }
59 double FGSteam::get_ASI_kias() { return FGBFI::getAirspeed(); }
61 double FGSteam::the_VSI_case = 29.92;
62 double FGSteam::the_VSI_fps = 0.0;
63 double FGSteam::get_VSI_fps() { _CatchUp(); return the_VSI_fps; }
65 double FGSteam::the_VACUUM_inhg = 0.0;
66 double FGSteam::get_VACUUM_inhg() { _CatchUp(); return the_VACUUM_inhg; }
68 double FGSteam::the_MH_err = 0.0;
69 double FGSteam::the_MH_deg = 0.0;
70 double FGSteam::the_MH_degps = 0.0;
71 double FGSteam::get_MH_deg () { _CatchUp(); return the_MH_deg; }
73 double FGSteam::the_DG_err = 0.0;
74 double FGSteam::the_DG_deg = 0.0;
75 double FGSteam::the_DG_degps = 0.0;
76 double FGSteam::the_DG_inhg = 0.0;
77 double FGSteam::get_DG_deg () { _CatchUp(); return the_DG_deg; }
78 double FGSteam::get_DG_err () { _CatchUp(); return the_DG_err; }
80 void FGSteam::set_DG_err ( double approx_magvar ) {
81 the_DG_err = approx_magvar;
84 double FGSteam::the_TC_rad = 0.0;
85 double FGSteam::the_TC_std = 0.0;
86 double FGSteam::get_TC_rad () { _CatchUp(); return the_TC_rad; }
87 double FGSteam::get_TC_std () { _CatchUp(); return the_TC_std; }
90 ////////////////////////////////////////////////////////////////////////
91 // Recording the current time
92 ////////////////////////////////////////////////////////////////////////
95 int FGSteam::_UpdatesPending = 1000000; /* Forces filter to reset */
98 // FIXME: no need to use static
99 // functions any longer.
101 void FGSteam::update ( int timesteps )
105 fgTie("/steam/airspeed", FGSteam::get_ASI_kias);
106 fgTie("/steam/altitude", FGSteam::get_ALT_ft);
107 fgTie("/steam/turn-rate", FGSteam::get_TC_std);
108 fgTie("/steam/slip-skid", FGSteam::get_TC_rad);
109 fgTie("/steam/vertical-speed", FGSteam::get_VSI_fps);
110 fgTie("/steam/gyro-compass", FGSteam::get_DG_deg);
111 fgTie("/steam/vor1", FGSteam::get_HackVOR1_deg);
112 fgTie("/steam/vor2", FGSteam::get_HackVOR2_deg);
113 fgTie("/steam/glidescope1", FGSteam::get_HackGS_deg);
114 fgTie("/steam/adf", FGSteam::get_HackADF_deg);
115 fgTie("/steam/gyro-compass-error",
117 FGSteam::set_DG_err);
118 fgTie("/steam/mag-compass", FGSteam::get_MH_deg);
120 _UpdatesPending += timesteps;
127 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
131 { /* time went backwards; kill the filter */
134 { /* ignore mildly negative time */
138 { /* Normal mode of operation */
139 (*outthe) = (*outthe) * ( 1.0 - tc )
143 { /* Huge time step; assume filter has settled */
146 { /* Moderate time step; non linear response */
147 double keep = exp ( -tc );
148 // printf ( "ARP: Keep is %f\n", keep );
149 (*outthe) = (*outthe) * keep
150 + inthe * ( 1.0 - keep );
156 ////////////////////////////////////////////////////////////////////////
157 // Here the fun really begins
158 ////////////////////////////////////////////////////////////////////////
161 void FGSteam::_CatchUp()
162 { if ( _UpdatesPending != 0 )
163 { double dt = _UpdatesPending * 1.0 /
164 fgGetInt("/sim/model-hz"); // FIXME: inefficient
165 double AccN, AccE, AccU;
167 double d, the_ENGINE_rpm;
170 /**************************
171 There is the possibility that this is the first call.
172 If this is the case, we will emit the feature registrations
173 just to be on the safe side. Doing it more than once will
174 waste CPU time but doesn't hurt anything really.
176 if ( _UpdatesPending > 999999 )
177 { FGFeature::register_int ( "Avionics/NAV1/Localizer", &NAV1_LOC );
178 FGFeature::register_double ( "Avionics/NAV1/Latitude", &NAV1_Lat );
179 FGFeature::register_double ( "Avionics/NAV1/Longitude", &NAV1_Lon );
180 FGFeature::register_double ( "Avionics/NAV1/Radial", &NAV1_Rad );
181 FGFeature::register_double ( "Avionics/NAV1/Altitude", &NAV1_Alt );
182 FGFeature::register_int ( "Avionics/NAV2/Localizer", &NAV2_LOC );
183 FGFeature::register_double ( "Avionics/NAV2/Latitude", &NAV2_Lat );
184 FGFeature::register_double ( "Avionics/NAV2/Longitude", &NAV2_Lon );
185 FGFeature::register_double ( "Avionics/NAV2/Radial", &NAV2_Rad );
186 FGFeature::register_double ( "Avionics/NAV2/Altitude", &NAV2_Alt );
187 FGFeature::register_double ( "Avionics/ADF/Latitude", &ADF_Lat );
188 FGFeature::register_double ( "Avionics/ADF/Longitude", &ADF_Lon );
192 /**************************
193 Someone has called our update function and
194 it turns out that we are running somewhat behind.
195 Here, we recalculate everything for a 'dt' second step.
198 /**************************
199 The ball responds to the acceleration vector in the body
200 frame, only the components perpendicular to the longitudinal
201 axis of the aircraft. This is only related to actual
202 side slip for a symmetrical aircraft which is not touching
203 the ground and not changing its attitude. Math simplifies
204 by assuming (for small angles) arctan(x)=x in radians.
205 Obvious failure mode is the absence of liquid in the
206 tube, which is there to damp the motion, so that instead
207 the ball will bounce around, hitting the tube ends.
208 More subtle flaw is having it not move or a travel limit
209 occasionally due to some dirt in the tube or on the ball.
211 // the_TC_rad = - ( FGBFI::getSideSlip () ); /* incorrect */
212 d = - current_aircraft.fdm_state->get_A_Z_pilot();
214 set_lowpass ( & the_TC_rad,
215 current_aircraft.fdm_state->get_A_Y_pilot () / d,
218 /**************************
219 The rate of turn indication is from an electric gyro.
220 We should have it spin up with the master switch.
221 It is mounted at a funny angle so that it detects
222 both rate of bank (i.e. rolling into and out of turns)
223 and the rate of turn (i.e. how fast heading is changing).
225 set_lowpass ( & the_TC_std,
226 current_aircraft.fdm_state->get_Phi_dot ()
227 * RAD_TO_DEG / 20.0 +
228 current_aircraft.fdm_state->get_Psi_dot ()
229 * RAD_TO_DEG / 3.0 , dt );
231 /**************************
232 We want to know the pilot accelerations,
233 to compute the magnetic compass errors.
235 AccN = current_aircraft.fdm_state->get_V_dot_north();
236 AccE = current_aircraft.fdm_state->get_V_dot_east();
237 AccU = current_aircraft.fdm_state->get_V_dot_down()
239 if ( fabs(the_TC_rad) > 0.2 )
240 { /* Massive sideslip jams it; it stops turning */
242 the_MH_err = FGBFI::getHeading () - the_MH_deg;
244 { double MagDip, MagVar, CosDip;
245 double FrcN, FrcE, FrcU, AccTot;
246 double EdgN, EdgE, EdgU;
247 double TrqN, TrqE, TrqU, Torque;
248 /* Find a force vector towards exact magnetic north */
249 MagVar = FGBFI::getMagVar() / RAD_TO_DEG;
250 MagDip = FGBFI::getMagDip() / RAD_TO_DEG;
251 CosDip = cos ( MagDip );
252 FrcN = CosDip * cos ( MagVar );
253 FrcE = CosDip * sin ( MagVar );
254 FrcU = sin ( MagDip );
255 /* Rotation occurs around acceleration axis,
256 but axis magnitude is irrelevant. So compute it. */
257 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
258 if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot );
260 /* Force applies to north marking on compass card */
261 EdgN = cos ( the_MH_err / RAD_TO_DEG );
262 EdgE = sin ( the_MH_err / RAD_TO_DEG );
264 /* Apply the force to the edge to get torques */
265 TrqN = EdgE * FrcU - EdgU * FrcE;
266 TrqE = EdgU * FrcN - EdgN * FrcU;
267 TrqU = EdgN * FrcE - EdgE * FrcN;
268 /* Select the component parallel to the axis */
269 Torque = ( TrqN * AccN +
271 TrqU * AccU ) * 5.0 / AccTot;
272 /* The magnetic compass has angular momentum,
273 so we apply a torque to it and wait */
275 { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
276 the_MH_err += dt * the_MH_degps;
278 if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else
279 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
280 the_MH_deg = FGBFI::getHeading () - the_MH_err;
283 /**************************
284 This is not actually correct, but provides a
285 scaling capability for the vacuum pump later on.
286 When we have a real engine model, we can ask it.
288 the_ENGINE_rpm = controls.get_throttle(0) * 26.0;
290 /**************************
291 This is just temporary, until the static source works,
292 so we just filter the actual value by one second to
293 account for the line impedance of the plumbing.
295 set_lowpass ( & the_ALT_ft, FGBFI::getAltitude(), dt );
297 /**************************
298 First, we need to know what the static line is reporting,
299 which is a whole simulation area in itself. For now, we cheat.
301 the_STATIC_inhg = 29.92;
302 i = (int) the_ALT_ft;
304 { the_STATIC_inhg *= 0.707;
307 the_STATIC_inhg *= ( 1.0 - 0.293 * i / 9000.0 );
310 NO alternate static source error (student feature),
311 NO possibility of blockage (instructor feature),
312 NO slip-induced error, important for C172 for example.
315 /**************************
316 The VSI case is a low-pass filter of the static line pressure.
317 The instrument reports the difference, scaled to approx ft.
318 NO option for student to break glass when static source fails.
319 NO capability for a fixed non-zero reading when level.
320 NO capability to have a scaling error of maybe a factor of two.
322 the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
323 * 10000.0; /* manual scaling factor */
324 set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
326 /**************************
327 The engine driven vacuum pump is directly attached
328 to the engine shaft, so each engine rotation pumps
329 a fixed volume. The amount of air in that volume
330 is determined by the vacuum line's internal pressure.
331 The instruments are essentially leaking air like
332 a fixed source impedance from atmospheric pressure.
333 The regulator provides a digital limit setting,
334 which is open circuit unless the pressure drop is big.
335 Thus, we can compute the vacuum line pressure directly.
336 We assume that there is negligible reservoir space.
337 NO failure of the pump supported (yet)
339 the_VACUUM_inhg = the_STATIC_inhg *
340 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
341 if ( the_VACUUM_inhg > 5.0 )
342 the_VACUUM_inhg = 5.0;
345 > I was merely going to do the engine rpm driven vacuum pump for both
346 > the AI and DG, have the gyros spin down down in power off descents,
347 > have it tumble when you exceed the usual pitch or bank limits,
348 > put in those insidious turning errors ... for now anyway.
350 if ( _UpdatesPending > 999999 )
351 the_DG_err = FGBFI::getMagVar();
352 the_DG_degps = 0.01; /* HACK! */
353 if (dt<1.0) the_DG_err += dt * the_DG_degps;
354 the_DG_deg = FGBFI::getHeading () - the_DG_err;
356 /**************************
357 Finished updates, now clear the timer
361 // cout << "0 Updates pending" << endl;
366 ////////////////////////////////////////////////////////////////////////
367 // Everything below is a transient hack; expect it to disappear
368 ////////////////////////////////////////////////////////////////////////
371 double FGSteam::get_HackGS_deg () {
372 if ( current_radiostack->get_nav1_inrange() &&
373 current_radiostack->get_nav1_has_gs() )
375 double x = current_radiostack->get_nav1_gs_dist();
376 double y = (FGBFI::getAltitude() - current_radiostack->get_nav1_elev())
378 double angle = atan2( y, x ) * RAD_TO_DEG;
379 return (current_radiostack->get_nav1_target_gs() - angle) * 5.0;
386 double FGSteam::get_HackVOR1_deg () {
389 if ( current_radiostack->get_nav1_inrange() ) {
392 if ( current_radiostack->get_nav1_loc() ) {
393 // localizer doesn't need magvar offset
394 r = current_radiostack->get_nav1_heading()
395 - current_radiostack->get_nav1_radial();
397 r = current_radiostack->get_nav1_heading() - FGBFI::getMagVar()
398 - current_radiostack->get_nav1_radial();
401 r = current_radiostack->get_nav1_heading()
402 - current_radiostack->get_nav1_radial();
403 // cout << "Radial = " << current_radiostack->get_nav1_radial()
404 // << " Bearing = " << current_radiostack->get_nav1_heading()
407 if (r> 180.0) r-=360.0; else
408 if (r<-180.0) r+=360.0;
409 if ( fabs(r) > 90.0 )
410 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
411 // According to Robin Peel, the ILS is 4x more sensitive than a vor
412 if ( current_radiostack->get_nav1_loc() ) r *= 4.0;
421 double FGSteam::get_HackVOR2_deg () {
424 if ( current_radiostack->get_nav2_inrange() ) {
427 if ( current_radiostack->get_nav2_loc() ) {
428 // localizer doesn't need magvar offset
429 r = current_radiostack->get_nav2_heading()
430 - current_radiostack->get_nav2_radial();
432 r = current_radiostack->get_nav2_heading() - FGBFI::getMagVar()
433 - current_radiostack->get_nav2_radial();
436 r = current_radiostack->get_nav2_heading()
437 - current_radiostack->get_nav2_radial();
438 // cout << "Radial = " << current_radiostack->get_nav1_radial()
439 // << " Bearing = " << current_radiostack->get_nav1_heading() << endl;
441 if (r> 180.0) r-=360.0; else
442 if (r<-180.0) r+=360.0;
443 if ( fabs(r) > 90.0 )
444 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
453 double FGSteam::get_HackOBS1_deg () {
454 return current_radiostack->get_nav1_radial();
458 double FGSteam::get_HackOBS2_deg () {
459 return current_radiostack->get_nav2_radial();
463 double FGSteam::get_HackADF_deg () {
466 if ( current_radiostack->get_adf_inrange() ) {
467 r = current_radiostack->get_adf_heading() - FGBFI::getHeading();
469 // cout << "Radial = " << current_radiostack->get_adf_heading()
470 // << " Heading = " << FGBFI::getHeading() << endl;