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