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