src/FDM/flight.cxx

   1 // flight.c -- a general interface to the various flight models
   2 //
   3 // Written by Curtis Olson, started May 1997.
   4 //
   5 // Copyright (C) 1997  Curtis L. Olson  - curt@infoplane.com
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 // $Id$
  22
  23
  24 #include <stdio.h>
  25
  26 #include <Debug/logstream.hxx>
  27 #include <FDM/External/external.hxx>
  28 #include <FDM/LaRCsim/ls_interface.h>
  29 #include <Include/fg_constants.h>
  30 #include <Main/options.hxx>
  31 #include <Math/fg_geodesy.hxx>
  32 #include <Time/timestamp.hxx>
  33
  34 #include "flight.hxx"
  35 #include "JSBsim.hxx"
  36 #include "LaRCsim.hxx"
  37
  38
  39 // base_fdm_state is the internal state that is updated in integer
  40 // multiples of "dt".  This leads to "jitter" with respect to the real
  41 // world time, so we introduce cur_fdm_state which is extrapolated by
  42 // the difference between sim time and real world time
  43
  44 FGInterface * cur_fdm_state;
  45 FGInterface base_fdm_state;
  46
  47
  48 int FGInterface::init( double dt ) {
  49     cout << "dummy init() ... SHOULDN'T BE CALLED!" << endl;
  50     return 0;
  51 }
  52
  53 int FGInterface::update( int multi_loop ) {
  54     cout << "dummy update() ... SHOULDN'T BE CALLED!" << endl;
  55     return 0;
  56 }
  57
  58 FGInterface::~FGInterface() {
  59 }
  60
  61 // Extrapolate fdm based on time_offset (in usec)
  62 void FGInterface::extrapolate( int time_offset ) {
  63     double dt = time_offset / 1000000.0;
  64
  65     // -dw- metrowerks complains about ambiguous access, not critical
  66     // to keep this ;)
  67 #ifndef __MWERKS__
  68     cout << "extrapolating FDM by dt = " << dt << endl;
  69 #endif
  70
  71     double lat = geodetic_position_v[0] + geocentric_rates_v[0] * dt;
  72     double lat_geoc = geocentric_position_v[0] + geocentric_rates_v[0] * dt;
  73
  74     double lon = geodetic_position_v[1] + geocentric_rates_v[1] * dt;
  75     double lon_geoc = geocentric_position_v[1] + geocentric_rates_v[1] * dt;
  76
  77     double alt = geodetic_position_v[2] + geocentric_rates_v[2] * dt;
  78     double radius = geocentric_position_v[2] + geocentric_rates_v[2] * dt;
  79
  80     geodetic_position_v[0] = lat;
  81     geocentric_position_v[0] = lat_geoc;
  82
  83     geodetic_position_v[1] = lon;
  84     geocentric_position_v[1] = lon_geoc;
  85
  86     geodetic_position_v[2] = alt;
  87     geocentric_position_v[2] = radius;
  88 }
  89
  90
  91 #if 0
  92 // Initialize the flight model parameters
  93 int fgFDMInit(int model, FGInterface& f, double dt) {
  94     double save_alt = 0.0;
  95
  96     FG_LOG( FG_FLIGHT ,FG_INFO, "Initializing flight model" );
  97
  98     base_fdm_state = f;
  99
 100     if ( model == FGInterface::FG_SLEW ) {
 101         // fgSlewInit(dt);
 102 #ifndef __MWERKS__   // -dw- 04/22/99 JSB sim not ported yet
 103     } else if ( model == FGInterface::FG_JSBSIM ) {
 104         fgJSBsimInit(dt, f);
 105         fgJSBsim_2_FGInterface(base_fdm_state);
 106 #endif
 107     } else if ( model == FGInterface::FG_LARCSIM ) {
 108         // lets try to avoid really screwing up the LaRCsim model
 109         if ( base_fdm_state.get_Altitude() < -9000.0 ) {
 110             save_alt = base_fdm_state.get_Altitude();
 111             base_fdm_state.set_Altitude( 0.0 );
 112         }
 113
 114         // translate FG to LaRCsim structure
 115         FGInterface_2_LaRCsim(base_fdm_state);
 116
 117         // initialize LaRCsim
 118         fgLaRCsimInit(dt);
 119
 120         FG_LOG( FG_FLIGHT, FG_INFO, "FG pos = " <<
 121                 base_fdm_state.get_Latitude() );
 122
 123         // translate LaRCsim back to FG structure
 124         fgLaRCsim_2_FGInterface(base_fdm_state);
 125
 126         // but lets restore our original bogus altitude when we are done
 127         if ( save_alt < -9000.0 ) {
 128             base_fdm_state.set_Altitude( save_alt );
 129         }
 130     } else if ( model == FGInterface::FG_EXTERNAL ) {
 131         fgExternalInit(base_fdm_state);
 132     } else {
 133         FG_LOG( FG_FLIGHT, FG_WARN,
 134                 "Unimplemented flight model == " << model );
 135     }
 136
 137     // set valid time for this record
 138     base_fdm_state.stamp_time();
 139
 140     f = base_fdm_state;
 141
 142     return 1;
 143 }
 144 #endif
 145
 146
 147 #if 0
 148 // Run multiloop iterations of the flight model
 149 int fgFDMUpdate(int model, FGInterface& f, int multiloop, int time_offset) {
 150     double time_step, start_elev, end_elev;
 151
 152     // printf("Altitude = %.2f\n", FG_Altitude * 0.3048);
 153
 154     // set valid time for this record
 155     base_fdm_state.stamp_time();
 156
 157     time_step = (1.0 / current_options.get_model_hz()) * multiloop;
 158     start_elev = base_fdm_state.get_Altitude();
 159
 160     if ( model == FGInterface::FG_SLEW ) {
 161         // fgSlewUpdate(f, multiloop);
 162 #ifndef __MWERKS__   // -dw- 04/22/99 JSB sim not ported yet
 163     } else if ( model == FGInterface::FG_JSBSIM ) {
 164         fgJSBsimUpdate(base_fdm_state, multiloop);
 165         f = base_fdm_state;
 166 #endif
 167     } else if ( model == FGInterface::FG_LARCSIM ) {
 168         fgLaRCsimUpdate(base_fdm_state, multiloop);
 169         // extrapolate position based on actual time
 170         // f = extrapolate_fdm( base_fdm_state, time_offset );
 171         f = base_fdm_state;
 172     } else if ( model == FGInterface::FG_EXTERNAL ) {
 173         // fgExternalUpdate(f, multiloop);
 174         FGTimeStamp current;
 175         current.stamp();
 176         f = base_fdm_state;
 177         f.extrapolate( current - base_fdm_state.get_time_stamp() );
 178     } else {
 179         FG_LOG( FG_FLIGHT, FG_WARN,
 180                 "Unimplemented flight model == " <<  model );
 181     }
 182
 183     end_elev = base_fdm_state.get_Altitude();
 184
 185     if ( time_step > 0.0 ) {
 186         // feet per second
 187         base_fdm_state.set_Climb_Rate( (end_elev - start_elev) / time_step );
 188     }
 189
 190     return 1;
 191 }
 192 #endif
 193
 194
 195 // Set the altitude (force)
 196 void fgFDMForceAltitude(int model, double alt_meters) {
 197     double sea_level_radius_meters;
 198     double lat_geoc;
 199
 200     // Set the FG variables first
 201     fgGeodToGeoc( base_fdm_state.get_Latitude(), alt_meters,
 202                   &sea_level_radius_meters, &lat_geoc);
 203
 204     base_fdm_state.set_Altitude( alt_meters * METER_TO_FEET );
 205     base_fdm_state.set_Radius_to_vehicle( base_fdm_state.get_Altitude() +
 206                                           (sea_level_radius_meters *
 207                                            METER_TO_FEET) );
 208
 209     // additional work needed for some flight models
 210     if ( model == FGInterface::FG_LARCSIM ) {
 211         ls_ForceAltitude( base_fdm_state.get_Altitude() );
 212     }
 213 }
 214
 215
 216 // Set the local ground elevation
 217 void fgFDMSetGroundElevation(int model, double ground_meters) {
 218     base_fdm_state.set_Runway_altitude( ground_meters * METER_TO_FEET );
 219     cur_fdm_state->set_Runway_altitude( ground_meters * METER_TO_FEET );
 220 }
 221
 222