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