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_err = 0.0;
72 double FGSteam::the_DG_deg = 0.0;
73 double FGSteam::the_DG_degps = 0.0;
74 double FGSteam::the_DG_inhg = 0.0;
75 double FGSteam::get_DG_deg () { _CatchUp(); return the_DG_deg; }
76 double FGSteam::get_DG_err () { _CatchUp(); return the_DG_err; }
78 void FGSteam::set_DG_err ( double approx_magvar ) {
79 the_DG_err = approx_magvar;
82 double FGSteam::the_TC_rad = 0.0;
83 double FGSteam::the_TC_std = 0.0;
84 double FGSteam::get_TC_rad () { _CatchUp(); return the_TC_rad; }
85 double FGSteam::get_TC_std () { _CatchUp(); return the_TC_std; }
88 ////////////////////////////////////////////////////////////////////////
89 // Recording the current time
90 ////////////////////////////////////////////////////////////////////////
93 int FGSteam::_UpdatesPending = 1000000; /* Forces filter to reset */
96 void FGSteam::update ( int timesteps )
98 _UpdatesPending += timesteps;
102 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
106 { /* time went backwards; kill the filter */
109 { /* ignore mildly negative time */
113 { /* Normal mode of operation */
114 (*outthe) = (*outthe) * ( 1.0 - tc )
118 { /* Huge time step; assume filter has settled */
121 { /* Moderate time step; non linear response */
122 double keep = exp ( -tc );
123 // printf ( "ARP: Keep is %f\n", keep );
124 (*outthe) = (*outthe) * keep
125 + inthe * ( 1.0 - keep );
131 ////////////////////////////////////////////////////////////////////////
132 // Here the fun really begins
133 ////////////////////////////////////////////////////////////////////////
136 void FGSteam::_CatchUp()
137 { if ( _UpdatesPending != 0 )
138 { double dt = _UpdatesPending * 1.0 / current_options.get_model_hz();
139 double AccN, AccE, AccU;
141 double d, the_ENGINE_rpm;
144 /**************************
145 There is the possibility that this is the first call.
146 If this is the case, we will emit the feature registrations
147 just to be on the safe side. Doing it more than once will
148 waste CPU time but doesn't hurt anything really.
150 if ( _UpdatesPending > 999999 )
151 { FGFeature::register_int ( "Avionics/NAV1/Localizer", &NAV1_LOC );
152 FGFeature::register_double ( "Avionics/NAV1/Latitude", &NAV1_Lat );
153 FGFeature::register_double ( "Avionics/NAV1/Longitude", &NAV1_Lon );
154 FGFeature::register_double ( "Avionics/NAV1/Radial", &NAV1_Rad );
155 FGFeature::register_double ( "Avionics/NAV1/Altitude", &NAV1_Alt );
156 FGFeature::register_int ( "Avionics/NAV2/Localizer", &NAV2_LOC );
157 FGFeature::register_double ( "Avionics/NAV2/Latitude", &NAV2_Lat );
158 FGFeature::register_double ( "Avionics/NAV2/Longitude", &NAV2_Lon );
159 FGFeature::register_double ( "Avionics/NAV2/Radial", &NAV2_Rad );
160 FGFeature::register_double ( "Avionics/NAV2/Altitude", &NAV2_Alt );
161 FGFeature::register_double ( "Avionics/ADF/Latitude", &ADF_Lat );
162 FGFeature::register_double ( "Avionics/ADF/Longitude", &ADF_Lon );
166 /**************************
167 Someone has called our update function and
168 it turns out that we are running somewhat behind.
169 Here, we recalculate everything for a 'dt' second step.
172 /**************************
173 The ball responds to the acceleration vector in the body
174 frame, only the components perpendicular to the longitudinal
175 axis of the aircraft. This is only related to actual
176 side slip for a symmetrical aircraft which is not touching
177 the ground and not changing its attitude. Math simplifies
178 by assuming (for small angles) arctan(x)=x in radians.
179 Obvious failure mode is the absence of liquid in the
180 tube, which is there to damp the motion, so that instead
181 the ball will bounce around, hitting the tube ends.
182 More subtle flaw is having it not move or a travel limit
183 occasionally due to some dirt in the tube or on the ball.
185 // the_TC_rad = - ( FGBFI::getSideSlip () ); /* incorrect */
186 d = - current_aircraft.fdm_state->get_A_Z_pilot();
188 set_lowpass ( & the_TC_rad,
189 current_aircraft.fdm_state->get_A_Y_pilot () / d,
192 /**************************
193 The rate of turn indication is from an electric gyro.
194 We should have it spin up with the master switch.
195 It is mounted at a funny angle so that it detects
196 both rate of bank (i.e. rolling into and out of turns)
197 and the rate of turn (i.e. how fast heading is changing).
199 set_lowpass ( & the_TC_std,
200 current_aircraft.fdm_state->get_Phi_dot ()
201 * RAD_TO_DEG / 20.0 +
202 current_aircraft.fdm_state->get_Psi_dot ()
203 * RAD_TO_DEG / 3.0 , dt );
205 /**************************
206 We want to know the pilot accelerations,
207 to compute the magnetic compass errors.
209 AccN = current_aircraft.fdm_state->get_V_dot_north();
210 AccE = current_aircraft.fdm_state->get_V_dot_east();
211 AccU = current_aircraft.fdm_state->get_V_dot_down()
213 if ( fabs(the_TC_rad) > 0.2 )
214 { /* Massive sideslip jams it; it stops turning */
216 the_MH_err = FGBFI::getHeading () - the_MH_deg;
218 { double MagDip, MagVar, CosDip;
219 double FrcN, FrcE, FrcU, AccTot;
220 double EdgN, EdgE, EdgU;
221 double TrqN, TrqE, TrqU, Torque;
222 /* Find a force vector towards exact magnetic north */
223 MagVar = FGBFI::getMagVar() / RAD_TO_DEG;
224 MagDip = FGBFI::getMagDip() / RAD_TO_DEG;
225 CosDip = cos ( MagDip );
226 FrcN = CosDip * cos ( MagVar );
227 FrcE = CosDip * sin ( MagVar );
228 FrcU = sin ( MagDip );
229 /* Rotation occurs around acceleration axis,
230 but axis magnitude is irrelevant. So compute it. */
231 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
232 if ( AccTot > 1.0 ) AccTot = sqrt ( AccTot );
234 /* Force applies to north marking on compass card */
235 EdgN = cos ( the_MH_err / RAD_TO_DEG );
236 EdgE = sin ( the_MH_err / RAD_TO_DEG );
238 /* Apply the force to the edge to get torques */
239 TrqN = EdgE * FrcU - EdgU * FrcE;
240 TrqE = EdgU * FrcN - EdgN * FrcU;
241 TrqU = EdgN * FrcE - EdgE * FrcN;
242 /* Select the component parallel to the axis */
243 Torque = ( TrqN * AccN +
245 TrqU * AccU ) * 5.0 / AccTot;
246 /* The magnetic compass has angular momentum,
247 so we apply a torque to it and wait */
249 { the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
250 the_MH_err += dt * the_MH_degps;
252 if ( the_MH_err > 180.0 ) the_MH_err -= 360.0; else
253 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
254 the_MH_deg = FGBFI::getHeading () - the_MH_err;
257 /**************************
258 This is not actually correct, but provides a
259 scaling capability for the vacuum pump later on.
260 When we have a real engine model, we can ask it.
262 the_ENGINE_rpm = FGBFI::getThrottle() * 26.0;
264 /**************************
265 This is just temporary, until the static source works,
266 so we just filter the actual value by one second to
267 account for the line impedance of the plumbing.
269 set_lowpass ( & the_ALT_ft, FGBFI::getAltitude(), dt );
271 /**************************
272 First, we need to know what the static line is reporting,
273 which is a whole simulation area in itself. For now, we cheat.
275 the_STATIC_inhg = 29.92;
276 i = (int) the_ALT_ft;
278 { the_STATIC_inhg *= 0.707;
281 the_STATIC_inhg *= ( 1.0 - 0.293 * i / 9000.0 );
284 NO alternate static source error (student feature),
285 NO possibility of blockage (instructor feature),
286 NO slip-induced error, important for C172 for example.
289 /**************************
290 The VSI case is a low-pass filter of the static line pressure.
291 The instrument reports the difference, scaled to approx ft.
292 NO option for student to break glass when static source fails.
293 NO capability for a fixed non-zero reading when level.
294 NO capability to have a scaling error of maybe a factor of two.
296 the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
297 * 10000.0; /* manual scaling factor */
298 set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
300 /**************************
301 The engine driven vacuum pump is directly attached
302 to the engine shaft, so each engine rotation pumps
303 a fixed volume. The amount of air in that volume
304 is determined by the vacuum line's internal pressure.
305 The instruments are essentially leaking air like
306 a fixed source impedance from atmospheric pressure.
307 The regulator provides a digital limit setting,
308 which is open circuit unless the pressure drop is big.
309 Thus, we can compute the vacuum line pressure directly.
310 We assume that there is negligible reservoir space.
311 NO failure of the pump supported (yet)
313 the_VACUUM_inhg = the_STATIC_inhg *
314 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
315 if ( the_VACUUM_inhg > 5.0 )
316 the_VACUUM_inhg = 5.0;
319 > I was merely going to do the engine rpm driven vacuum pump for both
320 > the AI and DG, have the gyros spin down down in power off descents,
321 > have it tumble when you exceed the usual pitch or bank limits,
322 > put in those insidious turning errors ... for now anyway.
324 if ( _UpdatesPending > 999999 )
325 the_DG_err = FGBFI::getMagVar();
326 the_DG_degps = 0.0; /* HACK! */
327 if (dt<1.0) the_DG_err += dt * the_DG_degps;
328 the_DG_deg = FGBFI::getHeading () - the_DG_err;
330 /**************************
331 Finished updates, now clear the timer
335 // cout << "0 Updates pending" << endl;
340 ////////////////////////////////////////////////////////////////////////
341 // Everything below is a transient hack; expect it to disappear
342 ////////////////////////////////////////////////////////////////////////
345 double FGSteam::get_HackGS_deg () {
346 if ( current_radiostack->get_nav1_inrange() &&
347 current_radiostack->get_nav1_has_gs() )
349 double x = current_radiostack->get_nav1_gs_dist();
350 double y = (FGBFI::getAltitude() - current_radiostack->get_nav1_elev())
352 double angle = atan2( y, x ) * RAD_TO_DEG;
353 return (current_radiostack->get_nav1_target_gs() - angle) * 5.0;
360 double FGSteam::get_HackVOR1_deg () {
363 if ( current_radiostack->get_nav1_inrange() ) {
364 if ( current_radiostack->get_nav1_loc() ) {
365 // localizer doesn't need magvar offset
366 r = current_radiostack->get_nav1_heading()
367 - current_radiostack->get_nav1_radial();
369 r = current_radiostack->get_nav1_heading() - FGBFI::getMagVar()
370 - current_radiostack->get_nav1_radial();
372 // cout << "Radial = " << current_radiostack->get_nav1_radial()
373 // << " Bearing = " << current_radiostack->get_nav1_heading()
376 if (r> 180.0) r-=360.0; else
377 if (r<-180.0) r+=360.0;
378 if ( fabs(r) > 90.0 )
379 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
380 // According to Robin Peel, the ILS is 4x more sensitive than a vor
381 if ( current_radiostack->get_nav1_loc() ) r *= 4.0;
390 double FGSteam::get_HackVOR2_deg () {
393 if ( current_radiostack->get_nav2_inrange() ) {
394 if ( current_radiostack->get_nav2_loc() ) {
395 // localizer doesn't need magvar offset
396 r = current_radiostack->get_nav2_heading()
397 - current_radiostack->get_nav2_radial();
399 r = current_radiostack->get_nav2_heading() - FGBFI::getMagVar()
400 - current_radiostack->get_nav2_radial();
402 // cout << "Radial = " << current_radiostack->get_nav1_radial()
403 // << " Bearing = " << current_radiostack->get_nav1_heading() << endl;
405 if (r> 180.0) r-=360.0; else
406 if (r<-180.0) r+=360.0;
407 if ( fabs(r) > 90.0 )
408 r = ( r<0.0 ? -r-180.0 : -r+180.0 );
417 double FGSteam::get_HackOBS1_deg () {
418 return current_radiostack->get_nav1_radial();
422 double FGSteam::get_HackOBS2_deg () {
423 return current_radiostack->get_nav2_radial();
427 double FGSteam::get_HackADF_deg () {
430 if ( current_radiostack->get_adf_inrange() ) {
431 r = current_radiostack->get_adf_heading() - FGBFI::getHeading();
433 // cout << "Radial = " << current_radiostack->get_adf_heading()
434 // << " Heading = " << FGBFI::getHeading() << endl;