src/Cockpit/steam.cxx

   1 // steam.cxx - Steam Gauge Calculations
   2 //
   3 // Copyright (C) 2000  Alexander Perry - alex.perry@ieee.org
   4 //
   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.
   9 //
  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.
  14 //
  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.
  18 //
  19 // $Id$
  20
  21
  22 #ifdef HAVE_CONFIG_H
  23 #  include <config.h>
  24 #endif
  25
  26 #include <simgear/compiler.h>
  27
  28 #include STL_IOSTREAM
  29
  30 #include <simgear/constants.h>
  31 #include <simgear/math/sg_types.hxx>
  32 #include <simgear/misc/props.hxx>
  33 #include <Aircraft/aircraft.hxx>
  34
  35 SG_USING_NAMESPACE(std);
  36
  37 #include "radiostack.hxx"
  38 #include "steam.hxx"
  39
  40 static bool isTied = false;
  41
  42
  43 \f
  44 ////////////////////////////////////////////////////////////////////////
  45 // Declare the functions that read the variables
  46 ////////////////////////////////////////////////////////////////////////
  47
  48 double FGSteam::the_STATIC_inhg = 29.92;
  49 double FGSteam::the_ALT_ft = 0.0;  // Indicated altitude
  50 double FGSteam::get_ALT_ft() { _CatchUp(); return the_ALT_ft; }
  51
  52 double FGSteam::the_ALT_datum_mb = 1013.0;
  53 double FGSteam::get_ALT_datum_mb() { return the_ALT_datum_mb; }
  54
  55 void FGSteam::set_ALT_datum_mb ( double datum_mb ) {
  56     the_ALT_datum_mb = datum_mb;
  57 }
  58
  59 double FGSteam::get_ASI_kias() { return fgGetDouble("/velocities/airspeed-kt"); }
  60
  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; }
  64
  65 double FGSteam::the_VACUUM_inhg = 0.0;
  66 double FGSteam::get_VACUUM_inhg() { _CatchUp(); return the_VACUUM_inhg; }
  67
  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; }
  72
  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; }
  79
  80 void FGSteam::set_DG_err ( double approx_magvar ) {
  81     the_DG_err = approx_magvar;
  82 }
  83
  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; }
  88
  89 \f
  90 ////////////////////////////////////////////////////////////////////////
  91 // Recording the current time
  92 ////////////////////////////////////////////////////////////////////////
  93
  94
  95 int FGSteam::_UpdatesPending = 1000000;  /* Forces filter to reset */
  96
  97
  98                                 // FIXME: no need to use static
  99                                 // functions any longer.
 100
 101 void FGSteam::update ( int timesteps )
 102 {
 103     if (!isTied) {
 104         isTied = true;
 105         fgTie("/steam/airspeed-kt", FGSteam::get_ASI_kias);
 106         fgTie("/steam/altitude-ft", FGSteam::get_ALT_ft);
 107         fgTie("/steam/altimeter-datum-mb",
 108               FGSteam::get_ALT_datum_mb, FGSteam::set_ALT_datum_mb,
 109               false);  /* don't modify the value */
 110         fgTie("/steam/turn-rate", FGSteam::get_TC_std);
 111         fgTie("/steam/slip-skid", FGSteam::get_TC_rad);
 112         fgTie("/steam/vertical-speed-fpm", FGSteam::get_VSI_fps);
 113         fgTie("/steam/gyro-compass-deg", FGSteam::get_DG_deg);
 114         fgTie("/steam/adf-deg", FGSteam::get_HackADF_deg);
 115         fgTie("/steam/gyro-compass-error-deg",
 116               FGSteam::get_DG_err, FGSteam::set_DG_err,
 117               false);  /* don't modify the value */
 118         fgTie("/steam/mag-compass-deg", FGSteam::get_MH_deg);
 119     }
 120     _UpdatesPending += timesteps;
 121 }
 122
 123
 124 /* tc should be (elapsed_time_between_updates / desired_smoothing_time) */
 125 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
 126 {
 127         if ( tc < 0.0 )
 128         {       if ( tc < -1.0 )
 129                 {       /* time went backwards; kill the filter */
 130                         (*outthe) = inthe;
 131                 } else
 132                 {       /* ignore mildly negative time */
 133                 }
 134         } else
 135         if ( tc < 0.2 )
 136         {       /* Normal mode of operation; fast approximation to exp(-tc) */
 137                 (*outthe) = (*outthe) * ( 1.0 - tc )
 138                           +    inthe  * tc;
 139         } else
 140         if ( tc > 5.0 )
 141         {       /* Huge time step; assume filter has settled */
 142                 (*outthe) = inthe;
 143         } else
 144         {       /* Moderate time step; non linear response */
 145                 double keep = exp ( -tc );
 146                 // printf ( "ARP: Keep is %f\n", keep );
 147                 (*outthe) = (*outthe) * keep
 148                           +    inthe  * ( 1.0 - keep );
 149         }
 150 }
 151
 152
 153 #define INHG_TO_MB 33.86388  /* Inches_of_mercury * INHG_TO_MB == millibars. */
 154
 155 // Convert air pressure to altitude by ICAO Standard Atmosphere
 156 double pressInHgToAltFt(double p_inhg)
 157 {
 158     // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
 159     const double P_0 = 29.92126;  // Std. MSL pressure, inHg. (=1013.25 mb)
 160     const double p_Tr = 0.2233609 * P_0;  // Pressure at tropopause, same units.
 161     const double h_Tr = 36089.24;  // Alt of tropopause, ft. (=11.0 km)
 162
 163     if (p_inhg > p_Tr)  // 0.0 to 11.0 km
 164         return (1.0 - pow((p_inhg / P_0), 1.0 / 5.2558797)) / 6.8755856e-6;
 165     return h_Tr + log10(p_inhg / p_Tr) / -4.806346e-5;  // 11.0 to 20.0 km
 166     // We could put more code for higher altitudes here.
 167 }
 168
 169
 170 // Convert altitude to air pressure by ICAO Standard Atmosphere
 171 double altFtToPressInHg(double alt_ft)
 172 {
 173     // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
 174     const double P_0 = 29.92126;  // Std. MSL pressure, inHg. (=1013.25 mb)
 175     const double p_Tr = 0.2233609 * P_0;  // Pressure at tropopause, same units.
 176     const double h_Tr = 36089.24;  // Alt of tropopause, ft. (=11.0 km)
 177
 178     if (alt_ft < h_Tr)  // 0.0 to 11.0 km
 179         return P_0 * pow(1.0 - 6.8755856e-6 * alt_ft, 5.2558797);
 180     return p_Tr * exp(-4.806346e-5 * (alt_ft - h_Tr));  // 11.0 to 20.0 km
 181     // We could put more code for higher altitudes here.
 182 }
 183
 184
 185 \f
 186 ////////////////////////////////////////////////////////////////////////
 187 // Here the fun really begins
 188 ////////////////////////////////////////////////////////////////////////
 189
 190
 191 void FGSteam::_CatchUp()
 192 { if ( _UpdatesPending != 0 )
 193   {     double dt = _UpdatesPending * 1.0 /
 194             fgGetInt("/sim/model-hz"); // FIXME: inefficient
 195         double AccN, AccE, AccU;
 196         /* int i, j; */
 197         double d, the_ENGINE_rpm;
 198
 199 #if 0
 200         /**************************
 201         There is the possibility that this is the first call.
 202         If this is the case, we will emit the feature registrations
 203         just to be on the safe side.  Doing it more than once will
 204         waste CPU time but doesn't hurt anything really.
 205         */
 206         if ( _UpdatesPending > 999999 )
 207         { FGFeature::register_int (    "Avionics/NAV1/Localizer", &NAV1_LOC );
 208           FGFeature::register_double ( "Avionics/NAV1/Latitude",  &NAV1_Lat );
 209           FGFeature::register_double ( "Avionics/NAV1/Longitude", &NAV1_Lon );
 210           FGFeature::register_double ( "Avionics/NAV1/Radial",    &NAV1_Rad );
 211           FGFeature::register_double ( "Avionics/NAV1/Altitude",  &NAV1_Alt );
 212           FGFeature::register_int (    "Avionics/NAV2/Localizer", &NAV2_LOC );
 213           FGFeature::register_double ( "Avionics/NAV2/Latitude",  &NAV2_Lat );
 214           FGFeature::register_double ( "Avionics/NAV2/Longitude", &NAV2_Lon );
 215           FGFeature::register_double ( "Avionics/NAV2/Radial",    &NAV2_Rad );
 216           FGFeature::register_double ( "Avionics/NAV2/Altitude",  &NAV2_Alt );
 217           FGFeature::register_double ( "Avionics/ADF/Latitude",   &ADF_Lat );
 218           FGFeature::register_double ( "Avionics/ADF/Longitude",  &ADF_Lon );
 219         }
 220 #endif
 221
 222         /**************************
 223         Someone has called our update function and
 224         it turns out that we are running somewhat behind.
 225         Here, we recalculate everything for a 'dt' second step.
 226         */
 227
 228         /**************************
 229         The ball responds to the acceleration vector in the body
 230         frame, only the components perpendicular to the longitudinal
 231         axis of the aircraft.  This is only related to actual
 232         side slip for a symmetrical aircraft which is not touching
 233         the ground and not changing its attitude.  Math simplifies
 234         by assuming (for small angles) arctan(x)=x in radians.
 235         Obvious failure mode is the absence of liquid in the
 236         tube, which is there to damp the motion, so that instead
 237         the ball will bounce around, hitting the tube ends.
 238         More subtle flaw is having it not move or a travel limit
 239         occasionally due to some dirt in the tube or on the ball.
 240         */
 241         d = - current_aircraft.fdm_state->get_A_Z_pilot();
 242         if ( d < 1 ) d = 1;
 243         set_lowpass ( & the_TC_rad,
 244                 current_aircraft.fdm_state->get_A_Y_pilot () / d,
 245                 dt );
 246
 247         /**************************
 248         The rate of turn indication is from an electric gyro.
 249         We should have it spin up with the master switch.
 250         It is mounted at a funny angle so that it detects
 251         both rate of bank (i.e. rolling into and out of turns)
 252         and the rate of turn (i.e. how fast heading is changing).
 253         */
 254         set_lowpass ( & the_TC_std,
 255                 current_aircraft.fdm_state->get_Phi_dot ()
 256                         * SGD_RADIANS_TO_DEGREES / 20.0 +
 257                 current_aircraft.fdm_state->get_Psi_dot ()
 258                         * SGD_RADIANS_TO_DEGREES / 3.0  , dt );
 259
 260         /**************************
 261         We want to know the pilot accelerations,
 262         to compute the magnetic compass errors.
 263         */
 264         AccN = current_aircraft.fdm_state->get_V_dot_north();
 265         AccE = current_aircraft.fdm_state->get_V_dot_east();
 266         AccU = current_aircraft.fdm_state->get_V_dot_down()
 267              - 9.81 / 0.3;
 268         if ( fabs(the_TC_rad) > 0.2 )
 269         {       /* Massive sideslip jams it; it stops turning */
 270                 the_MH_degps = 0.0;
 271                 the_MH_err   = fgGetDouble("/orientation/heading-deg") - the_MH_deg;
 272         } else
 273         {       double MagDip, MagVar, CosDip;
 274                 double FrcN, FrcE, FrcU, AccTot;
 275                 double EdgN, EdgE, EdgU;
 276                 double TrqN, TrqE, TrqU, Torque;
 277                 /* Find a force vector towards exact magnetic north */
 278                 MagVar = fgGetDouble("/environment/magnetic-variation-deg")
 279                     / SGD_RADIANS_TO_DEGREES;
 280                 MagDip = fgGetDouble("/environment/magnetic-dip-deg")
 281                     / SGD_RADIANS_TO_DEGREES;
 282                 CosDip = cos ( MagDip );
 283                 FrcN = CosDip * cos ( MagVar );
 284                 FrcE = CosDip * sin ( MagVar );
 285                 FrcU = sin ( MagDip );
 286                 /* Rotation occurs around acceleration axis,
 287                    but axis magnitude is irrelevant.  So compute it. */
 288                 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
 289                 if ( AccTot > 1.0 )  AccTot = sqrt ( AccTot );
 290                                 else AccTot = 1.0;
 291                 /* Force applies to north marking on compass card */
 292                 EdgN = cos ( the_MH_err / SGD_RADIANS_TO_DEGREES );
 293                 EdgE = sin ( the_MH_err / SGD_RADIANS_TO_DEGREES );
 294                 EdgU = 0.0;
 295                 /* Apply the force to the edge to get torques */
 296                 TrqN = EdgE * FrcU - EdgU * FrcE;
 297                 TrqE = EdgU * FrcN - EdgN * FrcU;
 298                 TrqU = EdgN * FrcE - EdgE * FrcN;
 299                 /* Select the component parallel to the axis */
 300                 Torque = ( TrqN * AccN +
 301                            TrqE * AccE +
 302                            TrqU * AccU ) * 5.0 / AccTot;
 303                 /* The magnetic compass has angular momentum,
 304                    so we apply a torque to it and wait */
 305                 if ( dt < 1.0 )
 306                 {       the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
 307                         the_MH_err += dt * the_MH_degps;
 308                 }
 309                 if ( the_MH_err >  180.0 ) the_MH_err -= 360.0; else
 310                 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
 311                 the_MH_deg  = fgGetDouble("/orientation/heading-deg") - the_MH_err;
 312         }
 313
 314         /**************************
 315         This is not actually correct, but provides a
 316         scaling capability for the vacuum pump later on.
 317         When we have a real engine model, we can ask it.
 318         */
 319         the_ENGINE_rpm = controls.get_throttle(0) * 26.0;
 320
 321         /**************************
 322         First, we need to know what the static line is reporting,
 323         which is a whole simulation area in itself.  For now, we cheat.
 324         We filter the actual value by one second to
 325         account for the line impedance of the plumbing.
 326         */
 327         double static_inhg
 328             = altFtToPressInHg(fgGetDouble("/position/altitude-ft"));
 329         set_lowpass ( & the_STATIC_inhg, static_inhg, dt );
 330
 331         /*
 332         NO alternate static source error (student feature),
 333         NO possibility of blockage (instructor feature),
 334         NO slip-induced error, important for C172 for example.
 335         */
 336
 337         /**************************
 338         Altimeter.
 339         ICAO standard atmosphere MSL pressure is 1013.25 mb, and pressure
 340         gradient is about 28 ft per mb at MSL increasing to about 32 at
 341         5000 and 38 at 10000 ft.
 342         Standard altimeters apply the subscale offset to the output altitude,
 343         not to the input pressure; I don't know exactly what pressure gradient
 344         they assume for this.  I choose to make it accurate at low altitudes.
 345         Remember, we are trying to simulate a real altimeter, not an ideal one.
 346         */
 347         set_lowpass ( & the_ALT_ft,
 348             pressInHgToAltFt(the_STATIC_inhg) +
 349             (the_ALT_datum_mb - 1013.25) * 28.0, /* accurate at low alt. */
 350             dt * 10 ); /* smoothing time 0.1 s */
 351
 352         /**************************
 353         The VSI case is a low-pass filter of the static line pressure.
 354         The instrument reports the difference, scaled to approx ft.
 355         NO option for student to break glass when static source fails.
 356         NO capability for a fixed non-zero reading when level.
 357         NO capability to have a scaling error of maybe a factor of two.
 358         */
 359         the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
 360                     * 10000.0; /* manual scaling factor */
 361         set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
 362
 363         /**************************
 364         The engine driven vacuum pump is directly attached
 365         to the engine shaft, so each engine rotation pumps
 366         a fixed volume.  The amount of air in that volume
 367         is determined by the vacuum line's internal pressure.
 368         The instruments are essentially leaking air like
 369         a fixed source impedance from atmospheric pressure.
 370         The regulator provides a digital limit setting,
 371         which is open circuit unless the pressure drop is big.
 372         Thus, we can compute the vacuum line pressure directly.
 373         We assume that there is negligible reservoir space.
 374         NO failure of the pump supported (yet)
 375         */
 376         the_VACUUM_inhg = the_STATIC_inhg *
 377                 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
 378         if ( the_VACUUM_inhg > 5.0 )
 379              the_VACUUM_inhg = 5.0;
 380
 381 /*
 382 > I was merely going to do the engine rpm driven vacuum pump for both
 383 > the AI and DG, have the gyros spin down down in power off descents,
 384 > have it tumble when you exceed the usual pitch or bank limits,
 385 > put in those insidious turning errors ... for now anyway.
 386 */
 387         if ( _UpdatesPending > 999999 )
 388             the_DG_err = fgGetDouble("/environment/magnetic-variation-deg");
 389         the_DG_degps = 0.01; /* HACK! */
 390         if (dt<1.0) the_DG_err += dt * the_DG_degps;
 391         the_DG_deg = fgGetDouble("/orientation/heading-deg") - the_DG_err;
 392
 393         /**************************
 394         Finished updates, now clear the timer
 395         */
 396         _UpdatesPending = 0;
 397   } else {
 398       // cout << "0 Updates pending" << endl;
 399   }
 400 }
 401
 402 \f
 403 ////////////////////////////////////////////////////////////////////////
 404 // Everything below is a transient hack; expect it to disappear
 405 ////////////////////////////////////////////////////////////////////////
 406
 407
 408 #if 0
 409
 410 double FGSteam::get_HackGS_deg () {
 411     if ( current_radiostack->get_nav1_inrange() &&
 412          current_radiostack->get_nav1_has_gs() )
 413     {
 414         double x = current_radiostack->get_nav1_gs_dist();
 415         double y = (fgGetDouble("/position/altitude-ft")
 416                     - current_radiostack->get_nav1_elev())
 417             * SG_FEET_TO_METER;
 418         double angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
 419         return (current_radiostack->get_nav1_target_gs() - angle) * 5.0;
 420     } else {
 421         return 0.0;
 422     }
 423 }
 424
 425
 426 double FGSteam::get_HackVOR1_deg () {
 427     double r;
 428
 429     if ( current_radiostack->get_nav1_inrange() ) {
 430         r = current_radiostack->get_nav1_heading()
 431             - current_radiostack->get_nav1_radial();
 432         // cout << "Radial = " << current_radiostack->get_nav1_radial()
 433         //      << "  Bearing = " << current_radiostack->get_nav1_heading()
 434         //      << endl;
 435
 436         if (r> 180.0) r-=360.0; else
 437             if (r<-180.0) r+=360.0;
 438         if ( fabs(r) > 90.0 )
 439             r = ( r<0.0 ? -r-180.0 : -r+180.0 );
 440         // According to Robin Peel, the ILS is 4x more sensitive than a vor
 441         if ( current_radiostack->get_nav1_loc() ) r *= 4.0;
 442     } else {
 443         r = 0.0;
 444     }
 445
 446     return r;
 447 }
 448
 449
 450 double FGSteam::get_HackVOR2_deg () {
 451     double r;
 452
 453     if ( current_radiostack->get_nav2_inrange() ) {
 454         r = current_radiostack->get_nav2_heading()
 455             - current_radiostack->get_nav2_radial();
 456         // cout << "Radial = " << current_radiostack->get_nav1_radial()
 457         // << "  Bearing = " << current_radiostack->get_nav1_heading() << endl;
 458
 459         if (r> 180.0) r-=360.0; else
 460             if (r<-180.0) r+=360.0;
 461         if ( fabs(r) > 90.0 )
 462             r = ( r<0.0 ? -r-180.0 : -r+180.0 );
 463     } else {
 464         r = 0.0;
 465     }
 466
 467     return r;
 468 }
 469 #endif
 470
 471
 472 double FGSteam::get_HackOBS1_deg () {
 473     return current_radiostack->get_nav1_radial();
 474 }
 475
 476
 477 double FGSteam::get_HackOBS2_deg () {
 478     return current_radiostack->get_nav2_radial();
 479 }
 480
 481
 482 double FGSteam::get_HackADF_deg () {
 483     static double last_r = 0;
 484
 485     if ( current_radiostack->get_adf_inrange() ) {
 486         double r = current_radiostack->get_adf_heading()
 487             - fgGetDouble("orientation/heading");
 488         last_r = r;
 489         // cout << "Radial = " << current_radiostack->get_adf_heading()
 490         //      << "  Heading = " << FGBFI::getHeading() << endl;
 491         return r;
 492     } else {
 493         return last_r;
 494     }
 495 }
 496
 497
 498 // end of steam.cxx