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