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 void FGSteam::update ( int timesteps )
103 current_properties.tieDouble("/steam/airspeed",
104 FGSteam::get_ASI_kias);
105 current_properties.tieDouble("/steam/altitude",
106 FGSteam::get_ALT_ft);
107 current_properties.tieDouble("/steam/turn-rate",
108 FGSteam::get_TC_std);
109 current_properties.tieDouble("/steam/slip-skid",
110 FGSteam::get_TC_rad);
111 current_properties.tieDouble("/steam/vertical-speed",
112 FGSteam::get_VSI_fps);
113 current_properties.tieDouble("/steam/gyro-compass",
114 FGSteam::get_DG_deg);
115 current_properties.tieDouble("/steam/vor1",
116 FGSteam::get_HackVOR1_deg);
117 current_properties.tieDouble("/steam/vor2",
118 FGSteam::get_HackVOR2_deg);
119 current_properties.tieDouble("/steam/glidescope1",
120 FGSteam::get_HackGS_deg);
121 current_properties.tieDouble("/steam/adf",
122 FGSteam::get_HackADF_deg);
123 current_properties.tieDouble("/steam/gyro-compass-error",
125 FGSteam::set_DG_err);
126 current_properties.tieDouble("/steam/mag-compass",
127 FGSteam::get_MH_deg);
129 _UpdatesPending += timesteps;
133 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
137 { /* time went backwards; kill the filter */
140 { /* ignore mildly negative time */
144 { /* Normal mode of operation */
145 (*outthe) = (*outthe) * ( 1.0 - tc )
149 { /* Huge time step; assume filter has settled */
152 { /* Moderate time step; non linear response */
153 double keep = exp ( -tc );
154 // printf ( "ARP: Keep is %f\n", keep );
155 (*outthe) = (*outthe) * keep
156 + inthe * ( 1.0 - keep );
162 ////////////////////////////////////////////////////////////////////////
163 // Here the fun really begins
164 ////////////////////////////////////////////////////////////////////////
167 void FGSteam::_CatchUp()
168 { if ( _UpdatesPending != 0 )
169 { double dt = _UpdatesPending * 1.0 /
170 globals->get_options()->get_model_hz();
171 double AccN, AccE, AccU;
173 double d, the_ENGINE_rpm;
176 /**************************
177 There is the possibility that this is the first call.
178 If this is the case, we will emit the feature registrations
179 just to be on the safe side. Doing it more than once will
180 waste CPU time but doesn't hurt anything really.
182 if ( _UpdatesPending > 999999 )
183 { FGFeature::register_int ( "Avionics/NAV1/Localizer", &NAV1_LOC );
184 FGFeature::register_double ( "Avionics/NAV1/Latitude", &NAV1_Lat );
185 FGFeature::register_double ( "Avionics/NAV1/Longitude", &NAV1_Lon );
186 FGFeature::register_double ( "Avionics/NAV1/Radial", &NAV1_Rad );
187 FGFeature::register_double ( "Avionics/NAV1/Altitude", &NAV1_Alt );
188 FGFeature::register_int ( "Avionics/NAV2/Localizer", &NAV2_LOC );
189 FGFeature::register_double ( "Avionics/NAV2/Latitude", &NAV2_Lat );
190 FGFeature::register_double ( "Avionics/NAV2/Longitude", &NAV2_Lon );
191 FGFeature::register_double ( "Avionics/NAV2/Radial", &NAV2_Rad );
192 FGFeature::register_double ( "Avionics/NAV2/Altitude", &NAV2_Alt );
193 FGFeature::register_double ( "Avionics/ADF/Latitude", &ADF_Lat );
194 FGFeature::register_double ( "Avionics/ADF/Longitude", &ADF_Lon );
198 /**************************
199 Someone has called our update function and
200 it turns out that we are running somewhat behind.
201 Here, we recalculate everything for a 'dt' second step.
204 /**************************
205 The ball responds to the acceleration vector in the body
206 frame, only the components perpendicular to the longitudinal
207 axis of the aircraft. This is only related to actual
208 side slip for a symmetrical aircraft which is not touching
209 the ground and not changing its attitude. Math simplifies
210 by assuming (for small angles) arctan(x)=x in radians.
211 Obvious failure mode is the absence of liquid in the
212 tube, which is there to damp the motion, so that instead
213 the ball will bounce around, hitting the tube ends.
214 More subtle flaw is having it not move or a travel limit
215 occasionally due to some dirt in the tube or on the ball.
217 // the_TC_rad = - ( FGBFI::getSideSlip () ); /* incorrect */
218 d = - current_aircraft.fdm_state->get_A_Z_pilot();
220 set_lowpass ( & the_TC_rad,
221 current_aircraft.fdm_state->get_A_Y_pilot () / d,
224 /**************************
225 The rate of turn indication is from an electric gyro.
226 We should have it spin up with the master switch.
227 It is mounted at a funny angle so that it detects
228 both rate of bank (i.e. rolling into and out of turns)
229 and the rate of turn (i.e. how fast heading is changing).
231 set_lowpass ( & the_TC_std,
232 current_aircraft.fdm_state->get_Phi_dot ()
233 * RAD_TO_DEG / 20.0 +
234 current_aircraft.fdm_state->get_Psi_dot ()
235 * RAD_TO_DEG / 3.0 , dt );
237 /**************************
238 We want to know the pilot accelerations,
239 to compute the magnetic compass errors.
241 AccN = current_aircraft.fdm_state->get_V_dot_north();
242 AccE = current_aircraft.fdm_state->get_V_dot_east();
243 AccU = current_aircraft.fdm_state->get_V_dot_down()
245 if ( fabs(the_TC_rad) > 0.2 )
246 { /* Massive sideslip jams it; it stops turning */
248 the_MH_err = FGBFI::getHeading () - the_MH_deg;
250 { double MagDip, MagVar, CosDip;
251 double FrcN, FrcE, FrcU, AccTot;
252 double EdgN, EdgE, EdgU;
253 double TrqN, TrqE, TrqU, Torque;
254 /* Find a force vector towards exact magnetic north */
255 MagVar = FGBFI::getMagVar() / RAD_TO_DEG;
256 MagDip = FGBFI::getMagDip() / RAD_TO_DEG;
257 CosDip = cos ( MagDip );
258 FrcN = CosDip * cos ( MagVar );
259 FrcE = CosDip * sin ( MagVar );
260 FrcU = sin ( MagDip );
261 /* Rotation occurs around acceleration axis,
262 but axis magnitude is irrelevant. So compute it. */
263 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
264 if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot );
266 /* Force applies to north marking on compass card */
267 EdgN = cos ( the_MH_err / RAD_TO_DEG );
268 EdgE = sin ( the_MH_err / RAD_TO_DEG );
270 /* Apply the force to the edge to get torques */
271 TrqN = EdgE * FrcU - EdgU * FrcE;
272 TrqE = EdgU * FrcN - EdgN * FrcU;
273 TrqU = EdgN * FrcE - EdgE * FrcN;
274 /* Select the component parallel to the axis */
275 Torque = ( TrqN * AccN +
277 TrqU * AccU ) * 5.0 / AccTot;
278 /* The magnetic compass has angular momentum,
279 so we apply a torque to it and wait */
281 { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
282 the_MH_err += dt * the_MH_degps;
284 if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else
285 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
286 the_MH_deg = FGBFI::getHeading () - the_MH_err;
289 /**************************
290 This is not actually correct, but provides a
291 scaling capability for the vacuum pump later on.
292 When we have a real engine model, we can ask it.
294 the_ENGINE_rpm = FGBFI::getThrottle() * 26.0;
296 /**************************
297 This is just temporary, until the static source works,
298 so we just filter the actual value by one second to
299 account for the line impedance of the plumbing.
301 set_lowpass ( & the_ALT_ft, FGBFI::getAltitude(), dt );
303 /**************************
304 First, we need to know what the static line is reporting,
305 which is a whole simulation area in itself. For now, we cheat.
307 the_STATIC_inhg = 29.92;
308 i = (int) the_ALT_ft;
310 { the_STATIC_inhg *= 0.707;
313 the_STATIC_inhg *= ( 1.0 - 0.293 * i / 9000.0 );
316 NO alternate static source error (student feature),
317 NO possibility of blockage (instructor feature),
318 NO slip-induced error, important for C172 for example.
321 /**************************
322 The VSI case is a low-pass filter of the static line pressure.
323 The instrument reports the difference, scaled to approx ft.
324 NO option for student to break glass when static source fails.
325 NO capability for a fixed non-zero reading when level.
326 NO capability to have a scaling error of maybe a factor of two.
328 the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
329 * 10000.0; /* manual scaling factor */
330 set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
332 /**************************
333 The engine driven vacuum pump is directly attached
334 to the engine shaft, so each engine rotation pumps
335 a fixed volume. The amount of air in that volume
336 is determined by the vacuum line's internal pressure.
337 The instruments are essentially leaking air like
338 a fixed source impedance from atmospheric pressure.
339 The regulator provides a digital limit setting,
340 which is open circuit unless the pressure drop is big.
341 Thus, we can compute the vacuum line pressure directly.
342 We assume that there is negligible reservoir space.
343 NO failure of the pump supported (yet)
345 the_VACUUM_inhg = the_STATIC_inhg *
346 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
347 if ( the_VACUUM_inhg > 5.0 )
348 the_VACUUM_inhg = 5.0;
351 > I was merely going to do the engine rpm driven vacuum pump for both
352 > the AI and DG, have the gyros spin down down in power off descents,
353 > have it tumble when you exceed the usual pitch or bank limits,
354 > put in those insidious turning errors ... for now anyway.
356 if ( _UpdatesPending > 999999 )
357 the_DG_err = FGBFI::getMagVar();
358 the_DG_degps = 0.01; /* HACK! */
359 if (dt<1.0) the_DG_err += dt * the_DG_degps;
360 the_DG_deg = FGBFI::getHeading () - the_DG_err;
362 /**************************
363 Finished updates, now clear the timer
367 // cout << "0 Updates pending" << endl;
372 ////////////////////////////////////////////////////////////////////////
373 // Everything below is a transient hack; expect it to disappear
374 ////////////////////////////////////////////////////////////////////////
377 double FGSteam::get_HackGS_deg () {
378 if ( current_radiostack->get_nav1_inrange() &&
379 current_radiostack->get_nav1_has_gs() )
381 double x = current_radiostack->get_nav1_gs_dist();
382 double y = (FGBFI::getAltitude() - current_radiostack->get_nav1_elev())
384 double angle = atan2( y, x ) * RAD_TO_DEG;
385 return (current_radiostack->get_nav1_target_gs() - angle) * 5.0;
392 double FGSteam::get_HackVOR1_deg () {
395 if ( current_radiostack->get_nav1_inrange() ) {
396 if ( current_radiostack->get_nav1_loc() ) {
397 // localizer doesn't need magvar offset
398 r = current_radiostack->get_nav1_heading()
399 - current_radiostack->get_nav1_radial();
401 r = current_radiostack->get_nav1_heading() - FGBFI::getMagVar()
402 - current_radiostack->get_nav1_radial();
404 // cout << "Radial = " << current_radiostack->get_nav1_radial()
405 // << " Bearing = " << current_radiostack->get_nav1_heading()
408 if (r> 180.0) r-=360.0; else
409 if (r<-180.0) r+=360.0;
410 if ( fabs(r) > 90.0 )
411 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
412 // According to Robin Peel, the ILS is 4x more sensitive than a vor
413 if ( current_radiostack->get_nav1_loc() ) r *= 4.0;
422 double FGSteam::get_HackVOR2_deg () {
425 if ( current_radiostack->get_nav2_inrange() ) {
426 if ( current_radiostack->get_nav2_loc() ) {
427 // localizer doesn't need magvar offset
428 r = current_radiostack->get_nav2_heading()
429 - current_radiostack->get_nav2_radial();
431 r = current_radiostack->get_nav2_heading() - FGBFI::getMagVar()
432 - current_radiostack->get_nav2_radial();
434 // cout << "Radial = " << current_radiostack->get_nav1_radial()
435 // << " Bearing = " << current_radiostack->get_nav1_heading() << endl;
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 );
449 double FGSteam::get_HackOBS1_deg () {
450 return current_radiostack->get_nav1_radial();
454 double FGSteam::get_HackOBS2_deg () {
455 return current_radiostack->get_nav2_radial();
459 double FGSteam::get_HackADF_deg () {
462 if ( current_radiostack->get_adf_inrange() ) {
463 r = current_radiostack->get_adf_heading() - FGBFI::getHeading();
465 // cout << "Radial = " << current_radiostack->get_adf_heading()
466 // << " Heading = " << FGBFI::getHeading() << endl;