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.
22 // (Log is kept at end of this file)
27 #include <Debug/logstream.hxx>
28 #include <FDM/External/external.hxx>
29 #include <FDM/LaRCsim/ls_interface.h>
30 #include <Include/fg_constants.h>
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 // Extrapolate fdm based on time_offset (in usec)
49 void FGInterface::extrapolate( int time_offset ) {
50 double dt = time_offset / 1000000.0;
51 cout << "extrapolating FDM by dt = " << dt << endl;
53 double lat = geodetic_position_v[0] + geocentric_rates_v[0] * dt;
54 double lat_geoc = geocentric_position_v[0] + geocentric_rates_v[0] * dt;
56 double lon = geodetic_position_v[1] + geocentric_rates_v[1] * dt;
57 double lon_geoc = geocentric_position_v[1] + geocentric_rates_v[1] * dt;
59 double alt = geodetic_position_v[2] + geocentric_rates_v[2] * dt;
60 double radius = geocentric_position_v[2] + geocentric_rates_v[2] * dt;
62 geodetic_position_v[0] = lat;
63 geocentric_position_v[0] = lat_geoc;
65 geodetic_position_v[1] = lon;
66 geocentric_position_v[1] = lon_geoc;
68 geodetic_position_v[2] = alt;
69 geocentric_position_v[2] = radius;
73 // Initialize the flight model parameters
74 int fgFDMInit(int model, FGInterface& f, double dt) {
75 double save_alt = 0.0;
77 FG_LOG( FG_FLIGHT ,FG_INFO, "Initializing flight model" );
81 if ( model == FGInterface::FG_SLEW ) {
83 } else if ( model == FGInterface::FG_JSBSIM ) {
85 fgJSBsim_2_FGInterface(base_fdm_state);
86 } else if ( model == FGInterface::FG_LARCSIM ) {
87 // lets try to avoid really screwing up the LaRCsim model
88 if ( base_fdm_state.get_Altitude() < -9000.0 ) {
89 save_alt = base_fdm_state.get_Altitude();
90 base_fdm_state.set_Altitude( 0.0 );
93 // translate FG to LaRCsim structure
94 FGInterface_2_LaRCsim(base_fdm_state);
99 FG_LOG( FG_FLIGHT, FG_INFO, "FG pos = " <<
100 base_fdm_state.get_Latitude() );
102 // translate LaRCsim back to FG structure
103 fgLaRCsim_2_FGInterface(base_fdm_state);
105 // but lets restore our original bogus altitude when we are done
106 if ( save_alt < -9000.0 ) {
107 base_fdm_state.set_Altitude( save_alt );
109 } else if ( model == FGInterface::FG_EXTERNAL ) {
110 fgExternalInit(base_fdm_state);
112 FG_LOG( FG_FLIGHT, FG_WARN,
113 "Unimplemented flight model == " << model );
116 // set valid time for this record
117 base_fdm_state.stamp_time();
125 // Run multiloop iterations of the flight model
126 int fgFDMUpdate(int model, FGInterface& f, int multiloop, int time_offset) {
127 double time_step, start_elev, end_elev;
129 // printf("Altitude = %.2f\n", FG_Altitude * 0.3048);
131 // set valid time for this record
132 base_fdm_state.stamp_time();
134 time_step = (1.0 / DEFAULT_MODEL_HZ) * multiloop;
135 start_elev = base_fdm_state.get_Altitude();
137 if ( model == FGInterface::FG_SLEW ) {
138 // fgSlewUpdate(f, multiloop);
139 } else if ( model == FGInterface::FG_JSBSIM ) {
140 fgJSBsimUpdate(base_fdm_state, multiloop);
142 } else if ( model == FGInterface::FG_LARCSIM ) {
143 fgLaRCsimUpdate(base_fdm_state, multiloop);
144 // extrapolate position based on actual time
145 // f = extrapolate_fdm( base_fdm_state, time_offset );
147 } else if ( model == FGInterface::FG_EXTERNAL ) {
148 // fgExternalUpdate(f, multiloop);
152 f.extrapolate( current - base_fdm_state.get_time_stamp() );
154 FG_LOG( FG_FLIGHT, FG_WARN,
155 "Unimplemented flight model == " << model );
158 end_elev = base_fdm_state.get_Altitude();
160 if ( time_step > 0.0 ) {
162 base_fdm_state.set_Climb_Rate( (end_elev - start_elev) / time_step );
169 // Set the altitude (force)
170 void fgFDMForceAltitude(int model, double alt_meters) {
171 double sea_level_radius_meters;
174 // Set the FG variables first
175 fgGeodToGeoc( base_fdm_state.get_Latitude(), alt_meters,
176 &sea_level_radius_meters, &lat_geoc);
178 base_fdm_state.set_Altitude( alt_meters * METER_TO_FEET );
179 base_fdm_state.set_Radius_to_vehicle( base_fdm_state.get_Altitude() +
180 (sea_level_radius_meters *
183 // additional work needed for some flight models
184 if ( model == FGInterface::FG_LARCSIM ) {
185 ls_ForceAltitude( base_fdm_state.get_Altitude() );
190 // Set the local ground elevation
191 void fgFDMSetGroundElevation(int model, double ground_meters) {
192 base_fdm_state.set_Runway_altitude( ground_meters * METER_TO_FEET );
193 cur_fdm_state.set_Runway_altitude( ground_meters * METER_TO_FEET );
198 // Revision 1.17 1999/04/03 04:20:03 curt
199 // Optimizations (tm) by Norman Vine.
201 // Revision 1.16 1999/02/26 22:09:12 curt
202 // Added initial support for native SGI compilers.
203 // Integrated Jon's next version of JSBsim.
205 // Revision 1.15 1999/02/05 21:29:01 curt
206 // Modifications to incorporate Jon S. Berndts flight model code.
208 // Revision 1.14 1999/02/01 21:33:31 curt
209 // Renamed FlightGear/Simulator/Flight to FlightGear/Simulator/FDM since
210 // Jon accepted my offer to do this and thought it was a good idea.
212 // Revision 1.13 1999/01/27 04:48:39 curt
213 // Set the runway height in cur_fdm_state as well as base_fdm_state.
215 // Revision 1.12 1999/01/20 13:42:22 curt
216 // Tweaked FDM interface.
217 // Testing check sum support for NMEA serial output.
219 // Revision 1.11 1999/01/19 17:52:06 curt
220 // Working on being able to extrapolate a new position and orientation
221 // based on a position, orientation, and time offset.
223 // Revision 1.10 1999/01/09 13:37:32 curt
224 // Convert fgTIMESTAMP to FGTimeStamp which holds usec instead of ms.
226 // Revision 1.9 1999/01/08 19:27:37 curt
227 // Fixed AOA reading on HUD.
228 // Continued work on time jitter compensation.
230 // Revision 1.8 1999/01/08 03:23:51 curt
231 // Beginning work on compensating for sim time vs. real world time "jitter".
233 // Revision 1.7 1998/12/18 23:37:07 curt
234 // Collapsed out the FGState variables not currently needed. They are just
235 // commented out and can be readded easily at any time. The point of this
236 // exersize is to determine which variables were or were not currently being
239 // Revision 1.6 1998/12/05 15:54:11 curt
240 // Renamed class fgFLIGHT to class FGState as per request by JSB.
242 // Revision 1.5 1998/12/04 01:29:39 curt
243 // Stubbed in a new flight model called "External" which is expected to be driven
244 // from some external source.
246 // Revision 1.4 1998/12/03 01:16:40 curt
247 // Converted fgFLIGHT to a class.
249 // Revision 1.3 1998/11/06 21:18:03 curt
250 // Converted to new logstream debugging facility. This allows release
251 // builds with no messages at all (and no performance impact) by using
252 // the -DFG_NDEBUGNDEBUG flag.
254 // Revision 1.2 1998/10/16 23:27:40 curt
257 // Revision 1.1 1998/10/16 20:16:41 curt
258 // Renamed flight.[ch] to flight.[ch]xx
260 // Revision 1.19 1998/09/29 14:57:38 curt
261 // c++-ified comments.
263 // Revision 1.18 1998/09/29 02:02:40 curt
264 // Added a rate of climb calculation.
266 // Revision 1.17 1998/08/24 20:09:07 curt
269 // Revision 1.16 1998/08/22 14:49:55 curt
270 // Attempting to iron out seg faults and crashes.
271 // Did some shuffling to fix a initialization order problem between view
272 // position, scenery elevation.
274 // Revision 1.15 1998/07/30 23:44:36 curt
275 // Beginning to add support for multiple flight models.
277 // Revision 1.14 1998/07/12 03:08:27 curt
278 // Added fgFlightModelSetAltitude() to force the altitude to something
279 // other than the current altitude. LaRCsim doesn't let you do this by just
280 // changing FG_Altitude.
282 // Revision 1.13 1998/04/25 22:06:28 curt
283 // Edited cvs log messages in source files ... bad bad bad!
285 // Revision 1.12 1998/04/21 16:59:33 curt
286 // Integrated autopilot.
287 // Prepairing for C++ integration.
289 // Revision 1.11 1998/04/18 04:14:04 curt
290 // Moved fg_debug.c to it's own library.
292 // Revision 1.10 1998/02/07 15:29:37 curt
293 // Incorporated HUD changes and struct/typedef changes from Charlie Hotchkiss
294 // <chotchkiss@namg.us.anritsu.com>
296 // Revision 1.9 1998/01/27 00:47:53 curt
297 // Incorporated Paul Bleisch's <pbleisch@acm.org> new debug message
298 // system and commandline/config file processing code.
300 // Revision 1.8 1998/01/19 19:27:03 curt
301 // Merged in make system changes from Bob Kuehne <rpk@sgi.com>
302 // This should simplify things tremendously.
304 // Revision 1.7 1998/01/19 18:40:23 curt
305 // Tons of little changes to clean up the code and to remove fatal errors
306 // when building with the c++ compiler.
308 // Revision 1.6 1998/01/19 18:35:43 curt
309 // Minor tweaks and fixes for cygwin32.
311 // Revision 1.5 1997/12/30 20:47:37 curt
312 // Integrated new event manager with subsystem initializations.
314 // Revision 1.4 1997/12/10 22:37:42 curt
315 // Prepended "fg" on the name of all global structures that didn't have it yet.
316 // i.e. "struct WEATHER {}" became "struct fgWEATHER {}"
318 // Revision 1.3 1997/08/27 03:30:04 curt
319 // Changed naming scheme of basic shared structures.
321 // Revision 1.2 1997/05/29 22:39:57 curt
322 // Working on incorporating the LaRCsim flight model.
324 // Revision 1.1 1997/05/29 02:35:04 curt