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