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