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