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