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/fg_types.hxx>
34 #include <Aircraft/aircraft.hxx>
35 #include <Main/options.hxx>
36 #include <Main/bfi.hxx>
37 #include <NetworkOLK/features.hxx>
39 FG_USING_NAMESPACE(std);
41 #include "radiostack.hxx"
46 ////////////////////////////////////////////////////////////////////////
47 // Declare the functions that read the variables
48 ////////////////////////////////////////////////////////////////////////
50 // Anything that reads the BFI directly is not implemented at all!
53 double FGSteam::the_STATIC_inhg = 29.92;
54 double FGSteam::the_ALT_ft = 0.0;
55 double FGSteam::get_ALT_ft() { _CatchUp(); return the_ALT_ft; }
57 double FGSteam::get_ASI_kias() { return FGBFI::getAirspeed(); }
59 double FGSteam::the_VSI_case = 29.92;
60 double FGSteam::the_VSI_fps = 0.0;
61 double FGSteam::get_VSI_fps() { _CatchUp(); return the_VSI_fps; }
63 double FGSteam::the_VACUUM_inhg = 0.0;
64 double FGSteam::get_VACUUM_inhg() { _CatchUp(); return the_VACUUM_inhg; }
66 double FGSteam::the_MH_err = 0.0;
67 double FGSteam::the_MH_deg = 0.0;
68 double FGSteam::the_MH_degps = 0.0;
69 double FGSteam::get_MH_deg () { _CatchUp(); return the_MH_deg; }
71 double FGSteam::the_DG_deg = 0.0;
72 double FGSteam::the_DG_degps = 0.0;
73 double FGSteam::the_DG_inhg = 0.0;
74 double FGSteam::get_DG_deg () { _CatchUp(); return the_DG_deg; }
76 double FGSteam::the_TC_rad = 0.0;
77 double FGSteam::the_TC_std = 0.0;
78 double FGSteam::get_TC_rad () { _CatchUp(); return the_TC_rad; }
79 double FGSteam::get_TC_std () { _CatchUp(); return the_TC_std; }
82 ////////////////////////////////////////////////////////////////////////
83 // Recording the current time
84 ////////////////////////////////////////////////////////////////////////
87 int FGSteam::_UpdatesPending = 9999; /* Forces filter to reset */
90 void FGSteam::update ( int timesteps )
92 _UpdatesPending += timesteps;
96 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
100 { /* time went backwards; kill the filter */
103 { /* ignore mildly negative time */
107 { /* Normal mode of operation */
108 (*outthe) = (*outthe) * ( 1.0 - tc )
112 { /* Huge time step; assume filter has settled */
115 { /* Moderate time step; non linear response */
116 double keep = exp ( -tc );
117 // printf ( "ARP: Keep is %f\n", keep );
118 (*outthe) = (*outthe) * keep
119 + inthe * ( 1.0 - keep );
125 ////////////////////////////////////////////////////////////////////////
126 // Here the fun really begins
127 ////////////////////////////////////////////////////////////////////////
130 void FGSteam::_CatchUp()
131 { if ( _UpdatesPending != 0 )
132 { double dt = _UpdatesPending * 1.0 / current_options.get_model_hz();
133 double AccN, AccE, AccU;
135 double d, the_ENGINE_rpm;
138 /**************************
139 There is the possibility that this is the first call.
140 If this is the case, we will emit the feature registrations
141 just to be on the safe side. Doing it more than once will
142 waste CPU time but doesn't hurt anything really.
144 if ( _UpdatesPending == 999 )
145 { FGFeature::register_int ( "Avionics/NAV1/Localizer", &NAV1_LOC );
146 FGFeature::register_double ( "Avionics/NAV1/Latitude", &NAV1_Lat );
147 FGFeature::register_double ( "Avionics/NAV1/Longitude", &NAV1_Lon );
148 FGFeature::register_double ( "Avionics/NAV1/Radial", &NAV1_Rad );
149 FGFeature::register_double ( "Avionics/NAV1/Altitude", &NAV1_Alt );
150 FGFeature::register_int ( "Avionics/NAV2/Localizer", &NAV2_LOC );
151 FGFeature::register_double ( "Avionics/NAV2/Latitude", &NAV2_Lat );
152 FGFeature::register_double ( "Avionics/NAV2/Longitude", &NAV2_Lon );
153 FGFeature::register_double ( "Avionics/NAV2/Radial", &NAV2_Rad );
154 FGFeature::register_double ( "Avionics/NAV2/Altitude", &NAV2_Alt );
155 FGFeature::register_double ( "Avionics/ADF/Latitude", &ADF_Lat );
156 FGFeature::register_double ( "Avionics/ADF/Longitude", &ADF_Lon );
160 /**************************
161 Someone has called our update function and
162 it turns out that we are running somewhat behind.
163 Here, we recalculate everything for a 'dt' second step.
166 /**************************
167 The ball responds to the acceleration vector in the body
168 frame, only the components perpendicular to the longitudinal
169 axis of the aircraft. This is only related to actual
170 side slip for a symmetrical aircraft which is not touching
171 the ground and not changing its attitude. Math simplifies
172 by assuming (for small angles) arctan(x)=x in radians.
173 Obvious failure mode is the absence of liquid in the
174 tube, which is there to damp the motion, so that instead
175 the ball will bounce around, hitting the tube ends.
176 More subtle flaw is having it not move or a travel limit
177 occasionally due to some dirt in the tube or on the ball.
179 // the_TC_rad = - ( FGBFI::getSideSlip () ); /* incorrect */
180 d = - current_aircraft.fdm_state->get_A_Z_pilot();
182 set_lowpass ( & the_TC_rad,
183 current_aircraft.fdm_state->get_A_Y_pilot () / d,
186 /**************************
187 The rate of turn indication is from an electric gyro.
188 We should have it spin up with the master switch.
189 It is mounted at a funny angle so that it detects
190 both rate of bank (i.e. rolling into and out of turns)
191 and the rate of turn (i.e. how fast heading is changing).
193 set_lowpass ( & the_TC_std,
194 current_aircraft.fdm_state->get_Phi_dot ()
195 * RAD_TO_DEG / 20.0 +
196 current_aircraft.fdm_state->get_Psi_dot ()
197 * RAD_TO_DEG / 3.0 , dt );
199 /**************************
200 We want to know the pilot accelerations,
201 to compute the magnetic compass errors.
203 AccN = current_aircraft.fdm_state->get_V_dot_north();
204 AccE = current_aircraft.fdm_state->get_V_dot_east();
205 AccU = current_aircraft.fdm_state->get_V_dot_down()
207 if ( fabs(the_TC_rad) > 0.2 )
208 { /* Massive sideslip jams it; it stops turning */
210 the_MH_err = FGBFI::getHeading () - the_MH_deg;
212 { double MagDip, MagVar, CosDip;
213 double FrcN, FrcE, FrcU, AccTot;
214 double EdgN, EdgE, EdgU;
215 double TrqN, TrqE, TrqU, Torque;
216 /* Find a force vector towards exact magnetic north */
217 MagVar = FGBFI::getMagVar() / RAD_TO_DEG;
218 MagDip = FGBFI::getMagDip() / RAD_TO_DEG;
219 CosDip = cos ( MagDip );
220 FrcN = CosDip * cos ( MagVar );
221 FrcE = CosDip * sin ( MagVar );
222 FrcU = sin ( MagDip );
223 /* Rotation occurs around acceleration axis,
224 but axis magnitude is irrelevant. So compute it. */
225 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
226 if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot );
228 /* Force applies to north marking on compass card */
229 EdgN = cos ( the_MH_err / RAD_TO_DEG );
230 EdgE = sin ( the_MH_err / RAD_TO_DEG );
232 /* Apply the force to the edge to get torques */
233 TrqN = EdgE * FrcU - EdgU * FrcE;
234 TrqE = EdgU * FrcN - EdgN * FrcU;
235 TrqU = EdgN * FrcE - EdgE * FrcN;
236 /* Select the component parallel to the axis */
237 Torque = ( TrqN * AccN +
239 TrqU * AccU ) * 5.0 / AccTot;
240 /* The magnetic compass has angular momentum,
241 so we apply a torque to it and wait */
243 { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
244 the_MH_err += dt * the_MH_degps;
246 if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else
247 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
248 the_MH_deg = FGBFI::getHeading () - the_MH_err;
251 /**************************
252 This is not actually correct, but provides a
253 scaling capability for the vacuum pump later on.
254 When we have a real engine model, we can ask it.
256 the_ENGINE_rpm = FGBFI::getThrottle() * 26.0;
258 /**************************
259 This is just temporary, until the static source works,
260 so we just filter the actual value by one second to
261 account for the line impedance of the plumbing.
263 set_lowpass ( & the_ALT_ft, FGBFI::getAltitude(), dt );
265 /**************************
266 First, we need to know what the static line is reporting,
267 which is a whole simulation area in itself. For now, we cheat.
269 the_STATIC_inhg = 29.92;
270 i = (int) the_ALT_ft;
272 { the_STATIC_inhg *= 0.707;
275 the_STATIC_inhg *= ( 1.0 - 0.293 * i / 9000.0 );
278 NO alternate static source error (student feature),
279 NO possibility of blockage (instructor feature),
280 NO slip-induced error, important for C172 for example.
283 /**************************
284 The VSI case is a low-pass filter of the static line pressure.
285 The instrument reports the difference, scaled to approx ft.
286 NO option for student to break glass when static source fails.
287 NO capability for a fixed non-zero reading when level.
288 NO capability to have a scaling error of maybe a factor of two.
290 the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
291 * 10000.0; /* manual scaling factor */
292 set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
294 /**************************
295 The engine driven vacuum pump is directly attached
296 to the engine shaft, so each engine rotation pumps
297 a fixed volume. The amount of air in that volume
298 is determined by the vacuum line's internal pressure.
299 The instruments are essentially leaking air like
300 a fixed source impedance from atmospheric pressure.
301 The regulator provides a digital limit setting,
302 which is open circuit unless the pressure drop is big.
303 Thus, we can compute the vacuum line pressure directly.
304 We assume that there is negligible reservoir space.
305 NO failure of the pump supported (yet)
307 the_VACUUM_inhg = the_STATIC_inhg *
308 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
309 if ( the_VACUUM_inhg > 5.0 )
310 the_VACUUM_inhg = 5.0;
313 > I was merely going to do the engine rpm driven vacuum pump for both
314 > the AI and DG, have the gyros spin down down in power off descents,
315 > have it tumble when you exceed the usual pitch or bank limits,
316 > put in those insidious turning errors ... for now anyway.
318 the_DG_deg = FGBFI::getHeading () - FGBFI::getMagVar();
320 /**************************
321 Finished updates, now clear the timer
328 ////////////////////////////////////////////////////////////////////////
329 // Everything below is a transient hack; expect it to disappear
330 ////////////////////////////////////////////////////////////////////////
333 double FGSteam::get_HackGS_deg () {
334 if ( current_radiostack->get_nav1_inrange() &&
335 current_radiostack->get_nav1_has_gs() )
337 double x = current_radiostack->get_nav1_gs_dist();
338 double y = (FGBFI::getAltitude() - current_radiostack->get_nav1_elev())
340 double angle = atan2( y, x ) * RAD_TO_DEG;
341 return (current_radiostack->get_nav1_target_gs() - angle) * 5.0;
348 double FGSteam::get_HackVOR1_deg () {
351 if ( current_radiostack->get_nav1_inrange() ) {
352 if ( current_radiostack->get_nav1_loc() ) {
353 // localizer doesn't need magvar offset
354 r = current_radiostack->get_nav1_heading()
355 - current_radiostack->get_nav1_radial();
357 r = current_radiostack->get_nav1_heading() - FGBFI::getMagVar()
358 - current_radiostack->get_nav1_radial();
360 // cout << "Radial = " << current_radiostack->get_nav1_radial()
361 // << " Bearing = " << current_radiostack->get_nav1_heading()
364 if (r> 180.0) r-=360.0; else
365 if (r<-180.0) r+=360.0;
366 if ( fabs(r) > 90.0 )
367 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
368 // According to Robin Peel, the ILS is 4x more sensitive than a vor
369 if ( current_radiostack->get_nav1_loc() ) r *= 4.0;
378 double FGSteam::get_HackVOR2_deg () {
381 if ( current_radiostack->get_nav2_inrange() ) {
382 if ( current_radiostack->get_nav2_loc() ) {
383 // localizer doesn't need magvar offset
384 r = current_radiostack->get_nav2_heading()
385 - current_radiostack->get_nav2_radial();
387 r = current_radiostack->get_nav2_heading() - FGBFI::getMagVar()
388 - current_radiostack->get_nav2_radial();
390 // cout << "Radial = " << current_radiostack->get_nav1_radial()
391 // << " Bearing = " << current_radiostack->get_nav1_heading() << endl;
393 if (r> 180.0) r-=360.0; else
394 if (r<-180.0) r+=360.0;
395 if ( fabs(r) > 90.0 )
396 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
405 double FGSteam::get_HackOBS1_deg () {
406 return current_radiostack->get_nav1_radial();
410 double FGSteam::get_HackOBS2_deg () {
411 return current_radiostack->get_nav2_radial();
415 double FGSteam::get_HackADF_deg () {
418 if ( current_radiostack->get_adf_inrange() ) {
419 r = current_radiostack->get_adf_heading() - FGBFI::getHeading();
421 // cout << "Radial = " << current_radiostack->get_adf_heading()
422 // << " Heading = " << FGBFI::getHeading() << endl;