1 // flight.c -- a general interface to the various flight models
3 // Written by Curtis Olson, started May 1997.
5 // Copyright (C) 1997 Curtis L. Olson - curt@infoplane.com
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.
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.
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.
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>
36 #include "LaRCsim.hxx"
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
44 FGInterface * cur_fdm_state;
45 FGInterface base_fdm_state;
48 int FGInterface::init( double dt ) {
49 cout << "dummy init() ... SHOULDN'T BE CALLED!" << endl;
53 int FGInterface::update( int multi_loop ) {
54 cout << "dummy update() ... SHOULDN'T BE CALLED!" << endl;
58 FGInterface::~FGInterface() {
61 // Extrapolate fdm based on time_offset (in usec)
62 void FGInterface::extrapolate( int time_offset ) {
63 double dt = time_offset / 1000000.0;
65 // -dw- metrowerks complains about ambiguous access, not critical
68 cout << "extrapolating FDM by dt = " << dt << endl;
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;
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;
77 double alt = geodetic_position_v[2] + geocentric_rates_v[2] * dt;
78 double radius = geocentric_position_v[2] + geocentric_rates_v[2] * dt;
80 geodetic_position_v[0] = lat;
81 geocentric_position_v[0] = lat_geoc;
83 geodetic_position_v[1] = lon;
84 geocentric_position_v[1] = lon_geoc;
86 geodetic_position_v[2] = alt;
87 geocentric_position_v[2] = radius;
92 // Initialize the flight model parameters
93 int fgFDMInit(int model, FGInterface& f, double dt) {
94 double save_alt = 0.0;
96 FG_LOG( FG_FLIGHT ,FG_INFO, "Initializing flight model" );
100 if ( model == FGInterface::FG_SLEW ) {
102 #ifndef __MWERKS__ // -dw- 04/22/99 JSB sim not ported yet
103 } else if ( model == FGInterface::FG_JSBSIM ) {
105 fgJSBsim_2_FGInterface(base_fdm_state);
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 );
114 // translate FG to LaRCsim structure
115 FGInterface_2_LaRCsim(base_fdm_state);
117 // initialize LaRCsim
120 FG_LOG( FG_FLIGHT, FG_INFO, "FG pos = " <<
121 base_fdm_state.get_Latitude() );
123 // translate LaRCsim back to FG structure
124 fgLaRCsim_2_FGInterface(base_fdm_state);
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 );
130 } else if ( model == FGInterface::FG_EXTERNAL ) {
131 fgExternalInit(base_fdm_state);
133 FG_LOG( FG_FLIGHT, FG_WARN,
134 "Unimplemented flight model == " << model );
137 // set valid time for this record
138 base_fdm_state.stamp_time();
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;
152 // printf("Altitude = %.2f\n", FG_Altitude * 0.3048);
154 // set valid time for this record
155 base_fdm_state.stamp_time();
157 time_step = (1.0 / current_options.get_model_hz()) * multiloop;
158 start_elev = base_fdm_state.get_Altitude();
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);
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 );
172 } else if ( model == FGInterface::FG_EXTERNAL ) {
173 // fgExternalUpdate(f, multiloop);
177 f.extrapolate( current - base_fdm_state.get_time_stamp() );
179 FG_LOG( FG_FLIGHT, FG_WARN,
180 "Unimplemented flight model == " << model );
183 end_elev = base_fdm_state.get_Altitude();
185 if ( time_step > 0.0 ) {
187 base_fdm_state.set_Climb_Rate( (end_elev - start_elev) / time_step );
195 // Set the altitude (force)
196 void fgFDMForceAltitude(int model, double alt_meters) {
197 double sea_level_radius_meters;
200 // Set the FG variables first
201 fgGeodToGeoc( base_fdm_state.get_Latitude(), alt_meters,
202 &sea_level_radius_meters, &lat_geoc);
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 *
209 // additional work needed for some flight models
210 if ( model == FGInterface::FG_LARCSIM ) {
211 ls_ForceAltitude( base_fdm_state.get_Altitude() );
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 );