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