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