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 #if defined( FG_HAVE_NATIVE_SGI_COMPILERS )
  27 #  include <iostream.h>
  28 #else
  29 #  include <iostream>
  30 #endif
  31
  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 <NetworkOLK/features.hxx>
  38
  39 FG_USING_NAMESPACE(std);
  40
  41 #include "radiostack.hxx"
  42 #include "steam.hxx"
  43
  44 static bool isTied = false;
  45
  46
  47 \f
  48 ////////////////////////////////////////////////////////////////////////
  49 // Declare the functions that read the variables
  50 ////////////////////////////////////////////////////////////////////////
  51
  52 // Anything that reads the BFI directly is not implemented at all!
  53
  54
  55 double FGSteam::the_STATIC_inhg = 29.92;
  56 double FGSteam::the_ALT_ft = 0.0;  // Indicated altitude
  57 double FGSteam::get_ALT_ft() { _CatchUp(); return the_ALT_ft; }
  58
  59 double FGSteam::the_ALT_datum_mb = 1013.0;
  60 double FGSteam::get_ALT_datum_mb() { return the_ALT_datum_mb; }
  61
  62 void FGSteam::set_ALT_datum_mb ( double datum_mb ) {
  63     the_ALT_datum_mb = datum_mb;
  64 }
  65
  66 double FGSteam::get_ASI_kias() { return FGBFI::getAirspeed(); }
  67
  68 double FGSteam::the_VSI_case = 29.92;
  69 double FGSteam::the_VSI_fps = 0.0;
  70 double FGSteam::get_VSI_fps() { _CatchUp(); return the_VSI_fps; }
  71
  72 double FGSteam::the_VACUUM_inhg = 0.0;
  73 double FGSteam::get_VACUUM_inhg() { _CatchUp(); return the_VACUUM_inhg; }
  74
  75 double FGSteam::the_MH_err   = 0.0;
  76 double FGSteam::the_MH_deg   = 0.0;
  77 double FGSteam::the_MH_degps = 0.0;
  78 double FGSteam::get_MH_deg () { _CatchUp(); return the_MH_deg; }
  79
  80 double FGSteam::the_DG_err   = 0.0;
  81 double FGSteam::the_DG_deg   = 0.0;
  82 double FGSteam::the_DG_degps = 0.0;
  83 double FGSteam::the_DG_inhg  = 0.0;
  84 double FGSteam::get_DG_deg () { _CatchUp(); return the_DG_deg; }
  85 double FGSteam::get_DG_err () { _CatchUp(); return the_DG_err; }
  86
  87 void FGSteam::set_DG_err ( double approx_magvar ) {
  88     the_DG_err = approx_magvar;
  89 }
  90
  91 double FGSteam::the_TC_rad   = 0.0;
  92 double FGSteam::the_TC_std   = 0.0;
  93 double FGSteam::get_TC_rad () { _CatchUp(); return the_TC_rad; }
  94 double FGSteam::get_TC_std () { _CatchUp(); return the_TC_std; }
  95
  96 \f
  97 ////////////////////////////////////////////////////////////////////////
  98 // Recording the current time
  99 ////////////////////////////////////////////////////////////////////////
 100
 101
 102 int FGSteam::_UpdatesPending = 1000000;  /* Forces filter to reset */
 103
 104
 105                                 // FIXME: no need to use static
 106                                 // functions any longer.
 107
 108 void FGSteam::update ( int timesteps )
 109 {
 110         if (!isTied) {
 111           isTied = true;
 112           fgTie("/steam/airspeed", FGSteam::get_ASI_kias);
 113           fgTie("/steam/altitude", FGSteam::get_ALT_ft);
 114           fgTie("/steam/altimeter-datum-mb",
 115                 FGSteam::get_ALT_datum_mb, FGSteam::set_ALT_datum_mb,
 116                 false);  /* don't modify the value */
 117           fgTie("/steam/turn-rate", FGSteam::get_TC_std);
 118           fgTie("/steam/slip-skid", FGSteam::get_TC_rad);
 119           fgTie("/steam/vertical-speed", FGSteam::get_VSI_fps);
 120           fgTie("/steam/gyro-compass", FGSteam::get_DG_deg);
 121           fgTie("/steam/vor1", FGSteam::get_HackVOR1_deg);
 122           fgTie("/steam/vor2", FGSteam::get_HackVOR2_deg);
 123           fgTie("/steam/glidescope1", FGSteam::get_HackGS_deg);
 124           fgTie("/steam/adf", FGSteam::get_HackADF_deg);
 125           fgTie("/steam/gyro-compass-error",
 126                 FGSteam::get_DG_err,
 127                 FGSteam::set_DG_err);
 128           fgTie("/steam/mag-compass", FGSteam::get_MH_deg);
 129         }
 130         _UpdatesPending += timesteps;
 131 }
 132
 133 #undef DF1
 134 #undef DF2
 135
 136
 137 /* tc should be (elapsed_time_between_updates / desired_smoothing_time) */
 138 void FGSteam::set_lowpass ( double *outthe, double inthe, double tc )
 139 {
 140         if ( tc < 0.0 )
 141         {       if ( tc < -1.0 )
 142                 {       /* time went backwards; kill the filter */
 143                         (*outthe) = inthe;
 144                 } else
 145                 {       /* ignore mildly negative time */
 146                 }
 147         } else
 148         if ( tc < 0.2 )
 149         {       /* Normal mode of operation; fast approximation to exp(-tc) */
 150                 (*outthe) = (*outthe) * ( 1.0 - tc )
 151                           +    inthe  * tc;
 152         } else
 153         if ( tc > 5.0 )
 154         {       /* Huge time step; assume filter has settled */
 155                 (*outthe) = inthe;
 156         } else
 157         {       /* Moderate time step; non linear response */
 158                 double keep = exp ( -tc );
 159                 // printf ( "ARP: Keep is %f\n", keep );
 160                 (*outthe) = (*outthe) * keep
 161                           +    inthe  * ( 1.0 - keep );
 162         }
 163 }
 164
 165
 166 #define INHG_TO_MB 33.86388  /* Inches_of_mercury * INHG_TO_MB == millibars. */
 167
 168 // Convert air pressure to altitude by ICAO Standard Atmosphere
 169 double pressInHgToAltFt(double p_inhg)
 170 {
 171     // Ref. Aviation Formulary, Ed Williams, www.best.com/~williams/avform.htm
 172     const double P_0 = 29.92126;  // Std. MSL pressure, inHg. (=10135.25 mb)
 173     const double p_Tr = 0.2233609 * P_0;  // Pressure at tropopause, same units.
 174     const double h_Tr = 36089.24;  // Alt of tropopause, ft. (=11.0 km)
 175
 176     // return (P_0 - p_inhg) * 1000.0;  // ### crude approx. for low alt's
 177     if (p_inhg > p_Tr)  // 0.0 to 11.0 km
 178         return (1.0 - pow((p_inhg / P_0), 1.0 / 5.2558797)) / 6.8755856e-6;
 179     return h_Tr + log10(p_inhg / p_Tr) / -4.806346e-5;  // 11.0 to 20.0 km
 180     // We could put more code for higher altitudes here.
 181 }
 182
 183
 184 \f
 185 ////////////////////////////////////////////////////////////////////////
 186 // Here the fun really begins
 187 ////////////////////////////////////////////////////////////////////////
 188
 189
 190 void FGSteam::_CatchUp()
 191 { if ( _UpdatesPending != 0 )
 192   {     double dt = _UpdatesPending * 1.0 /
 193             fgGetInt("/sim/model-hz"); // FIXME: inefficient
 194         double AccN, AccE, AccU;
 195         int i /*,j*/;
 196         double d, the_ENGINE_rpm;
 197
 198 #if 0
 199         /**************************
 200         There is the possibility that this is the first call.
 201         If this is the case, we will emit the feature registrations
 202         just to be on the safe side.  Doing it more than once will
 203         waste CPU time but doesn't hurt anything really.
 204         */
 205         if ( _UpdatesPending > 999999 )
 206         { FGFeature::register_int (    "Avionics/NAV1/Localizer", &NAV1_LOC );
 207           FGFeature::register_double ( "Avionics/NAV1/Latitude",  &NAV1_Lat );
 208           FGFeature::register_double ( "Avionics/NAV1/Longitude", &NAV1_Lon );
 209           FGFeature::register_double ( "Avionics/NAV1/Radial",    &NAV1_Rad );
 210           FGFeature::register_double ( "Avionics/NAV1/Altitude",  &NAV1_Alt );
 211           FGFeature::register_int (    "Avionics/NAV2/Localizer", &NAV2_LOC );
 212           FGFeature::register_double ( "Avionics/NAV2/Latitude",  &NAV2_Lat );
 213           FGFeature::register_double ( "Avionics/NAV2/Longitude", &NAV2_Lon );
 214           FGFeature::register_double ( "Avionics/NAV2/Radial",    &NAV2_Rad );
 215           FGFeature::register_double ( "Avionics/NAV2/Altitude",  &NAV2_Alt );
 216           FGFeature::register_double ( "Avionics/ADF/Latitude",   &ADF_Lat );
 217           FGFeature::register_double ( "Avionics/ADF/Longitude",  &ADF_Lon );
 218         }
 219 #endif
 220
 221         /**************************
 222         Someone has called our update function and
 223         it turns out that we are running somewhat behind.
 224         Here, we recalculate everything for a 'dt' second step.
 225         */
 226
 227         /**************************
 228         The ball responds to the acceleration vector in the body
 229         frame, only the components perpendicular to the longitudinal
 230         axis of the aircraft.  This is only related to actual
 231         side slip for a symmetrical aircraft which is not touching
 232         the ground and not changing its attitude.  Math simplifies
 233         by assuming (for small angles) arctan(x)=x in radians.
 234         Obvious failure mode is the absence of liquid in the
 235         tube, which is there to damp the motion, so that instead
 236         the ball will bounce around, hitting the tube ends.
 237         More subtle flaw is having it not move or a travel limit
 238         occasionally due to some dirt in the tube or on the ball.
 239         */
 240         // the_TC_rad = - ( FGBFI::getSideSlip () ); /* incorrect */
 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                         * RAD_TO_DEG / 20.0 +
 257                 current_aircraft.fdm_state->get_Psi_dot ()
 258                         * RAD_TO_DEG / 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   = FGBFI::getHeading () - 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 = FGBFI::getMagVar() / RAD_TO_DEG;
 279                 MagDip = FGBFI::getMagDip() / RAD_TO_DEG;
 280                 CosDip = cos ( MagDip );
 281                 FrcN = CosDip * cos ( MagVar );
 282                 FrcE = CosDip * sin ( MagVar );
 283                 FrcU = sin ( MagDip );
 284                 /* Rotation occurs around acceleration axis,
 285                    but axis magnitude is irrelevant.  So compute it. */
 286                 AccTot = AccN*AccN + AccE*AccE + AccU*AccU;
 287                 if ( AccTot > 1.0 )  AccTot = sqrt ( AccTot );
 288                                 else AccTot = 1.0;
 289                 /* Force applies to north marking on compass card */
 290                 EdgN = cos ( the_MH_err / RAD_TO_DEG );
 291                 EdgE = sin ( the_MH_err / RAD_TO_DEG );
 292                 EdgU = 0.0;
 293                 /* Apply the force to the edge to get torques */
 294                 TrqN = EdgE * FrcU - EdgU * FrcE;
 295                 TrqE = EdgU * FrcN - EdgN * FrcU;
 296                 TrqU = EdgN * FrcE - EdgE * FrcN;
 297                 /* Select the component parallel to the axis */
 298                 Torque = ( TrqN * AccN +
 299                            TrqE * AccE +
 300                            TrqU * AccU ) * 5.0 / AccTot;
 301                 /* The magnetic compass has angular momentum,
 302                    so we apply a torque to it and wait */
 303                 if ( dt < 1.0 )
 304                 {       the_MH_degps= the_MH_degps * (1.0 - dt) - Torque;
 305                         the_MH_err += dt * the_MH_degps;
 306                 }
 307                 if ( the_MH_err >  180.0 ) the_MH_err -= 360.0; else
 308                 if ( the_MH_err < -180.0 ) the_MH_err += 360.0;
 309                 the_MH_deg  = FGBFI::getHeading () - the_MH_err;
 310         }
 311
 312         /**************************
 313         This is not actually correct, but provides a
 314         scaling capability for the vacuum pump later on.
 315         When we have a real engine model, we can ask it.
 316         */
 317         the_ENGINE_rpm = controls.get_throttle(0) * 26.0;
 318
 319         /**************************
 320         First, we need to know what the static line is reporting,
 321         which is a whole simulation area in itself.  For now, we cheat.
 322         We filter the actual value by one second to
 323         account for the line impedance of the plumbing.
 324         */
 325         double static_inhg = 29.92;
 326         i = (int) FGBFI::getAltitude();
 327         while ( i > 9000 )
 328         {       static_inhg *= 0.707;
 329                 i -= 9000;
 330         }
 331         static_inhg *= ( 1.0 - 0.293 * i / 9000.0 );
 332         set_lowpass ( & the_STATIC_inhg, static_inhg, dt );
 333
 334         /*
 335         NO alternate static source error (student feature),
 336         NO possibility of blockage (instructor feature),
 337         NO slip-induced error, important for C172 for example.
 338         */
 339
 340         /**************************
 341         Altimeter.
 342         ICAO standard atmosphere MSL pressure is 1013.25 mb, and pressure
 343         gradient is about 28 ft per mb at MSL increasing to about 32 at
 344         5000 and 38 at 10000 ft.
 345         Standard altimeters apply the subscale offset to the output altitude,
 346         not to the input pressure; I don't know exactly what pressure gradient
 347         they assume for this.  I choose to make it accurate at low altitudes.
 348         Remember, we are trying to simulate a real altimeter, not an ideal one.
 349         */
 350         set_lowpass ( & the_ALT_ft,
 351             pressInHgToAltFt(the_STATIC_inhg) +
 352             (the_ALT_datum_mb - 1013.25) * 28.0, /* accurate at low alt. */
 353             dt * 10 ); /* smoothing time 0.1 s */
 354
 355         /**************************
 356         The VSI case is a low-pass filter of the static line pressure.
 357         The instrument reports the difference, scaled to approx ft.
 358         NO option for student to break glass when static source fails.
 359         NO capability for a fixed non-zero reading when level.
 360         NO capability to have a scaling error of maybe a factor of two.
 361         */
 362         the_VSI_fps = ( the_VSI_case - the_STATIC_inhg )
 363                     * 10000.0; /* manual scaling factor */
 364         set_lowpass ( & the_VSI_case, the_STATIC_inhg, dt/6.0 );
 365
 366         /**************************
 367         The engine driven vacuum pump is directly attached
 368         to the engine shaft, so each engine rotation pumps
 369         a fixed volume.  The amount of air in that volume
 370         is determined by the vacuum line's internal pressure.
 371         The instruments are essentially leaking air like
 372         a fixed source impedance from atmospheric pressure.
 373         The regulator provides a digital limit setting,
 374         which is open circuit unless the pressure drop is big.
 375         Thus, we can compute the vacuum line pressure directly.
 376         We assume that there is negligible reservoir space.
 377         NO failure of the pump supported (yet)
 378         */
 379         the_VACUUM_inhg = the_STATIC_inhg *
 380                 the_ENGINE_rpm / ( the_ENGINE_rpm + 10000.0 );
 381         if ( the_VACUUM_inhg > 5.0 )
 382              the_VACUUM_inhg = 5.0;
 383
 384 /*
 385 > I was merely going to do the engine rpm driven vacuum pump for both
 386 > the AI and DG, have the gyros spin down down in power off descents,
 387 > have it tumble when you exceed the usual pitch or bank limits,
 388 > put in those insidious turning errors ... for now anyway.
 389 */
 390         if ( _UpdatesPending > 999999 )
 391             the_DG_err = FGBFI::getMagVar();
 392         the_DG_degps = 0.01; /* HACK! */
 393         if (dt<1.0) the_DG_err += dt * the_DG_degps;
 394         the_DG_deg = FGBFI::getHeading () - the_DG_err;
 395
 396         /**************************
 397         Finished updates, now clear the timer
 398         */
 399         _UpdatesPending = 0;
 400   } else {
 401       // cout << "0 Updates pending" << endl;
 402   }
 403 }
 404
 405 \f
 406 ////////////////////////////////////////////////////////////////////////
 407 // Everything below is a transient hack; expect it to disappear
 408 ////////////////////////////////////////////////////////////////////////
 409
 410
 411 double FGSteam::get_HackGS_deg () {
 412     if ( current_radiostack->get_nav1_inrange() &&
 413          current_radiostack->get_nav1_has_gs() )
 414     {
 415         double x = current_radiostack->get_nav1_gs_dist();
 416         double y = (FGBFI::getAltitude() - current_radiostack->get_nav1_elev())
 417             * FEET_TO_METER;
 418         double angle = atan2( y, x ) * RAD_TO_DEG;
 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 #if 0
 431         // depricated
 432         if ( current_radiostack->get_nav1_loc() ) {
 433             // localizer doesn't need magvar offset
 434             r = current_radiostack->get_nav1_heading()
 435                 - current_radiostack->get_nav1_radial();
 436         } else {
 437             r = current_radiostack->get_nav1_heading() - FGBFI::getMagVar()
 438                 - current_radiostack->get_nav1_radial();
 439         }
 440 #endif
 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 #if 0
 466         // Depricated
 467         if ( current_radiostack->get_nav2_loc() ) {
 468             // localizer doesn't need magvar offset
 469             r = current_radiostack->get_nav2_heading()
 470                 - current_radiostack->get_nav2_radial();
 471         } else {
 472             r = current_radiostack->get_nav2_heading() - FGBFI::getMagVar()
 473                 - current_radiostack->get_nav2_radial();
 474         }
 475 #endif
 476         r = current_radiostack->get_nav2_heading()
 477             - current_radiostack->get_nav2_radial();
 478         // cout << "Radial = " << current_radiostack->get_nav1_radial()
 479         // << "  Bearing = " << current_radiostack->get_nav1_heading() << endl;
 480
 481         if (r> 180.0) r-=360.0; else
 482             if (r<-180.0) r+=360.0;
 483         if ( fabs(r) > 90.0 )
 484             r = ( r<0.0 ? -r-180.0 : -r+180.0 );
 485     } else {
 486         r = 0.0;
 487     }
 488
 489     return r;
 490 }
 491
 492
 493 double FGSteam::get_HackOBS1_deg () {
 494     return current_radiostack->get_nav1_radial();
 495 }
 496
 497
 498 double FGSteam::get_HackOBS2_deg () {
 499     return current_radiostack->get_nav2_radial();
 500 }
 501
 502
 503 double FGSteam::get_HackADF_deg () {
 504     double r;
 505
 506     if ( current_radiostack->get_adf_inrange() ) {
 507         r = current_radiostack->get_adf_heading() - FGBFI::getHeading();
 508
 509         // cout << "Radial = " << current_radiostack->get_adf_heading()
 510         //      << "  Heading = " << FGBFI::getHeading() << endl;
 511     } else {
 512         r = 0.0;
 513     }
 514
 515     return r;
 516 }
 517
 518
 519 // end of steam.cxx