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 // Extrapolate fdm based on time_offset (in usec)
  49 void FGInterface::extrapolate( int time_offset ) {
  50     double dt = time_offset / 1000000.0;
  51
  52     // -dw- metrowerks complains about ambiguous access, not critical
  53     // to keep this ;)
  54 #ifndef __MWERKS__
  55     cout << "extrapolating FDM by dt = " << dt << endl;
  56 #endif
  57
  58     double lat = geodetic_position_v[0] + geocentric_rates_v[0] * dt;
  59     double lat_geoc = geocentric_position_v[0] + geocentric_rates_v[0] * dt;
  60
  61     double lon = geodetic_position_v[1] + geocentric_rates_v[1] * dt;
  62     double lon_geoc = geocentric_position_v[1] + geocentric_rates_v[1] * dt;
  63
  64     double alt = geodetic_position_v[2] + geocentric_rates_v[2] * dt;
  65     double radius = geocentric_position_v[2] + geocentric_rates_v[2] * dt;
  66
  67     geodetic_position_v[0] = lat;
  68     geocentric_position_v[0] = lat_geoc;
  69
  70     geodetic_position_v[1] = lon;
  71     geocentric_position_v[1] = lon_geoc;
  72
  73     geodetic_position_v[2] = alt;
  74     geocentric_position_v[2] = radius;
  75 }
  76
  77
  78 // Initialize the flight model parameters
  79 int fgFDMInit(int model, FGInterface& f, double dt) {
  80     double save_alt = 0.0;
  81
  82     FG_LOG( FG_FLIGHT ,FG_INFO, "Initializing flight model" );
  83
  84     base_fdm_state = f;
  85
  86     if ( model == FGInterface::FG_SLEW ) {
  87         // fgSlewInit(dt);
  88 #ifndef __MWERKS__   // -dw- 04/22/99 JSB sim not ported yet
  89     } else if ( model == FGInterface::FG_JSBSIM ) {
  90         fgJSBsimInit(dt, f);
  91         fgJSBsim_2_FGInterface(base_fdm_state);
  92 #endif
  93     } else if ( model == FGInterface::FG_LARCSIM ) {
  94         // lets try to avoid really screwing up the LaRCsim model
  95         if ( base_fdm_state.get_Altitude() < -9000.0 ) {
  96             save_alt = base_fdm_state.get_Altitude();
  97             base_fdm_state.set_Altitude( 0.0 );
  98         }
  99
 100         // translate FG to LaRCsim structure
 101         FGInterface_2_LaRCsim(base_fdm_state);
 102
 103         // initialize LaRCsim
 104         fgLaRCsimInit(dt);
 105
 106         FG_LOG( FG_FLIGHT, FG_INFO, "FG pos = " <<
 107                 base_fdm_state.get_Latitude() );
 108
 109         // translate LaRCsim back to FG structure
 110         fgLaRCsim_2_FGInterface(base_fdm_state);
 111
 112         // but lets restore our original bogus altitude when we are done
 113         if ( save_alt < -9000.0 ) {
 114             base_fdm_state.set_Altitude( save_alt );
 115         }
 116     } else if ( model == FGInterface::FG_EXTERNAL ) {
 117         fgExternalInit(base_fdm_state);
 118     } else {
 119         FG_LOG( FG_FLIGHT, FG_WARN,
 120                 "Unimplemented flight model == " << model );
 121     }
 122
 123     // set valid time for this record
 124     base_fdm_state.stamp_time();
 125
 126     f = base_fdm_state;
 127
 128     return 1;
 129 }
 130
 131
 132 // Run multiloop iterations of the flight model
 133 int fgFDMUpdate(int model, FGInterface& f, int multiloop, int time_offset) {
 134     double time_step, start_elev, end_elev;
 135
 136     // printf("Altitude = %.2f\n", FG_Altitude * 0.3048);
 137
 138     // set valid time for this record
 139     base_fdm_state.stamp_time();
 140
 141     time_step = (1.0 / current_options.get_model_hz()) * multiloop;
 142     start_elev = base_fdm_state.get_Altitude();
 143
 144     if ( model == FGInterface::FG_SLEW ) {
 145         // fgSlewUpdate(f, multiloop);
 146 #ifndef __MWERKS__   // -dw- 04/22/99 JSB sim not ported yet
 147     } else if ( model == FGInterface::FG_JSBSIM ) {
 148         fgJSBsimUpdate(base_fdm_state, multiloop);
 149         f = base_fdm_state;
 150 #endif
 151     } else if ( model == FGInterface::FG_LARCSIM ) {
 152         fgLaRCsimUpdate(base_fdm_state, multiloop);
 153         // extrapolate position based on actual time
 154         // f = extrapolate_fdm( base_fdm_state, time_offset );
 155         f = base_fdm_state;
 156     } else if ( model == FGInterface::FG_EXTERNAL ) {
 157         // fgExternalUpdate(f, multiloop);
 158         FGTimeStamp current;
 159         current.stamp();
 160         f = base_fdm_state;
 161         f.extrapolate( current - base_fdm_state.get_time_stamp() );
 162     } else {
 163         FG_LOG( FG_FLIGHT, FG_WARN,
 164                 "Unimplemented flight model == " <<  model );
 165     }
 166
 167     end_elev = base_fdm_state.get_Altitude();
 168
 169     if ( time_step > 0.0 ) {
 170         // feet per second
 171         base_fdm_state.set_Climb_Rate( (end_elev - start_elev) / time_step );
 172     }
 173
 174     return 1;
 175 }
 176
 177
 178 // Set the altitude (force)
 179 void fgFDMForceAltitude(int model, double alt_meters) {
 180     double sea_level_radius_meters;
 181     double lat_geoc;
 182
 183     // Set the FG variables first
 184     fgGeodToGeoc( base_fdm_state.get_Latitude(), alt_meters,
 185                   &sea_level_radius_meters, &lat_geoc);
 186
 187     base_fdm_state.set_Altitude( alt_meters * METER_TO_FEET );
 188     base_fdm_state.set_Radius_to_vehicle( base_fdm_state.get_Altitude() +
 189                                           (sea_level_radius_meters *
 190                                            METER_TO_FEET) );
 191
 192     // additional work needed for some flight models
 193     if ( model == FGInterface::FG_LARCSIM ) {
 194         ls_ForceAltitude( base_fdm_state.get_Altitude() );
 195     }
 196 }
 197
 198
 199 // Set the local ground elevation
 200 void fgFDMSetGroundElevation(int model, double ground_meters) {
 201     base_fdm_state.set_Runway_altitude( ground_meters * METER_TO_FEET );
 202     cur_fdm_state.set_Runway_altitude( ground_meters * METER_TO_FEET );
 203 }
 204
 205