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)
28 #include "LaRCsim.hxx"
30 #include <Debug/logstream.hxx>
31 #include <Flight/External/external.hxx>
32 #include <Flight/LaRCsim/ls_interface.h>
33 #include <Include/fg_constants.h>
34 #include <Math/fg_geodesy.hxx>
37 // base_fdm_state is the internal state that is updated in integer
38 // multiples of "dt". This leads to "jitter" with respect to the real
39 // world time, so we introduce cur_fdm_state which is extrapolated by
40 // the difference between sim time and real world time
42 FGState cur_fdm_state;
43 FGState base_fdm_state;
46 // Initialize the flight model parameters
47 int fgFlightModelInit(int model, FGState& f, double dt) {
48 double save_alt = 0.0;
50 FG_LOG( FG_FLIGHT ,FG_INFO, "Initializing flight model" );
54 if ( model == FGState::FG_SLEW ) {
56 } else if ( model == FGState::FG_LARCSIM ) {
58 // lets try to avoid really screwing up the LaRCsim model
59 if ( base_fdm_state.get_Altitude() < -9000.0 ) {
60 save_alt = base_fdm_state.get_Altitude();
61 base_fdm_state.set_Altitude( 0.0 );
64 // translate FG to LaRCsim structure
65 FGState_2_LaRCsim(base_fdm_state);
70 FG_LOG( FG_FLIGHT, FG_INFO, "FG pos = " <<
71 base_fdm_state.get_Latitude() );
73 // translate LaRCsim back to FG structure
74 fgLaRCsim_2_FGState(base_fdm_state);
76 // but lets restore our original bogus altitude when we are done
77 if ( save_alt < -9000.0 ) {
78 base_fdm_state.set_Altitude( save_alt );
80 } else if ( model == FGState::FG_EXTERNAL ) {
81 fgExternalInit(base_fdm_state, dt);
83 FG_LOG( FG_FLIGHT, FG_WARN,
84 "Unimplemented flight model == " << model );
93 // Extrapolate fdm based on jitter time (in milliseconds)
94 static FGState extrapolate_fdm( FGState &base, int jitter ) {
97 double dt = jitter / 1000.0;
98 // cout << "dt = " << dt << endl;
100 // start by making a straight up copy
103 double lon = base.get_Longitude() + base.get_Longitude_dot() * dt;
104 double lon_geoc = base.get_Lon_geocentric() + base.get_Longitude_dot() * dt;
106 double lat = base.get_Latitude() + base.get_Latitude_dot() * dt;
107 double lat_geoc = base.get_Lat_geocentric() + base.get_Latitude_dot() * dt;
110 cout << "( " << base.get_Longitude() << ", " <<
111 base.get_Latitude() << " )" << endl;
112 cout << "( " << lon << ", " << lat << " )" << endl;
113 cout << "( " << base.get_Longitude_dot() * dt << ", " <<
114 base.get_Latitude_dot() * dt << ", " <<
115 base.get_Radius_dot() * dt << " )" << endl;
118 double alt = base.get_Altitude() + base.get_Radius_dot() * dt;
119 double radius = base.get_Radius_to_vehicle() + base.get_Radius_dot() * dt;
121 result.set_Longitude( lon );
122 result.set_Latitude( lat );
123 result.set_Altitude( alt );
124 // result.set_Geocentric_Position( lon_geoc, lat_geoc, radius );
130 // Run multiloop iterations of the flight model
131 int fgFlightModelUpdate(int model, FGState& f, int multiloop, int jitter) {
132 double time_step, start_elev, end_elev;
134 // printf("Altitude = %.2f\n", FG_Altitude * 0.3048);
136 // base_fdm_state = f;
138 time_step = (1.0 / DEFAULT_MODEL_HZ) * multiloop;
139 start_elev = base_fdm_state.get_Altitude();
141 if ( model == FGState::FG_SLEW ) {
142 // fgSlewUpdate(f, multiloop);
143 } else if ( model == FGState::FG_LARCSIM ) {
144 fgLaRCsimUpdate(base_fdm_state, multiloop);
145 } else if ( model == FGState::FG_EXTERNAL ) {
146 // fgExternalUpdate(f, multiloop);
148 FG_LOG( FG_FLIGHT, FG_WARN,
149 "Unimplemented flight model == " << model );
152 end_elev = base_fdm_state.get_Altitude();
154 if ( time_step > 0.0 ) {
156 base_fdm_state.set_Climb_Rate( (end_elev - start_elev) / time_step );
159 f = extrapolate_fdm( base_fdm_state, jitter );
165 // Set the altitude (force)
166 void fgFlightModelSetAltitude(int model, double alt_meters) {
167 double sea_level_radius_meters;
170 // Set the FG variables first
171 fgGeodToGeoc( base_fdm_state.get_Latitude(), alt_meters,
172 &sea_level_radius_meters, &lat_geoc);
174 base_fdm_state.set_Altitude( alt_meters * METER_TO_FEET );
175 base_fdm_state.set_Radius_to_vehicle( base_fdm_state.get_Altitude() +
176 (sea_level_radius_meters *
179 // additional work needed for some flight models
180 if ( model == FGState::FG_LARCSIM ) {
181 ls_ForceAltitude( base_fdm_state.get_Altitude() );
187 // Revision 1.9 1999/01/08 19:27:37 curt
188 // Fixed AOA reading on HUD.
189 // Continued work on time jitter compensation.
191 // Revision 1.8 1999/01/08 03:23:51 curt
192 // Beginning work on compensating for sim time vs. real world time "jitter".
194 // Revision 1.7 1998/12/18 23:37:07 curt
195 // Collapsed out the FGState variables not currently needed. They are just
196 // commented out and can be readded easily at any time. The point of this
197 // exersize is to determine which variables were or were not currently being
200 // Revision 1.6 1998/12/05 15:54:11 curt
201 // Renamed class fgFLIGHT to class FGState as per request by JSB.
203 // Revision 1.5 1998/12/04 01:29:39 curt
204 // Stubbed in a new flight model called "External" which is expected to be driven
205 // from some external source.
207 // Revision 1.4 1998/12/03 01:16:40 curt
208 // Converted fgFLIGHT to a class.
210 // Revision 1.3 1998/11/06 21:18:03 curt
211 // Converted to new logstream debugging facility. This allows release
212 // builds with no messages at all (and no performance impact) by using
213 // the -DFG_NDEBUGNDEBUG flag.
215 // Revision 1.2 1998/10/16 23:27:40 curt
218 // Revision 1.1 1998/10/16 20:16:41 curt
219 // Renamed flight.[ch] to flight.[ch]xx
221 // Revision 1.19 1998/09/29 14:57:38 curt
222 // c++-ified comments.
224 // Revision 1.18 1998/09/29 02:02:40 curt
225 // Added a rate of climb calculation.
227 // Revision 1.17 1998/08/24 20:09:07 curt
230 // Revision 1.16 1998/08/22 14:49:55 curt
231 // Attempting to iron out seg faults and crashes.
232 // Did some shuffling to fix a initialization order problem between view
233 // position, scenery elevation.
235 // Revision 1.15 1998/07/30 23:44:36 curt
236 // Beginning to add support for multiple flight models.
238 // Revision 1.14 1998/07/12 03:08:27 curt
239 // Added fgFlightModelSetAltitude() to force the altitude to something
240 // other than the current altitude. LaRCsim doesn't let you do this by just
241 // changing FG_Altitude.
243 // Revision 1.13 1998/04/25 22:06:28 curt
244 // Edited cvs log messages in source files ... bad bad bad!
246 // Revision 1.12 1998/04/21 16:59:33 curt
247 // Integrated autopilot.
248 // Prepairing for C++ integration.
250 // Revision 1.11 1998/04/18 04:14:04 curt
251 // Moved fg_debug.c to it's own library.
253 // Revision 1.10 1998/02/07 15:29:37 curt
254 // Incorporated HUD changes and struct/typedef changes from Charlie Hotchkiss
255 // <chotchkiss@namg.us.anritsu.com>
257 // Revision 1.9 1998/01/27 00:47:53 curt
258 // Incorporated Paul Bleisch's <pbleisch@acm.org> new debug message
259 // system and commandline/config file processing code.
261 // Revision 1.8 1998/01/19 19:27:03 curt
262 // Merged in make system changes from Bob Kuehne <rpk@sgi.com>
263 // This should simplify things tremendously.
265 // Revision 1.7 1998/01/19 18:40:23 curt
266 // Tons of little changes to clean up the code and to remove fatal errors
267 // when building with the c++ compiler.
269 // Revision 1.6 1998/01/19 18:35:43 curt
270 // Minor tweaks and fixes for cygwin32.
272 // Revision 1.5 1997/12/30 20:47:37 curt
273 // Integrated new event manager with subsystem initializations.
275 // Revision 1.4 1997/12/10 22:37:42 curt
276 // Prepended "fg" on the name of all global structures that didn't have it yet.
277 // i.e. "struct WEATHER {}" became "struct fgWEATHER {}"
279 // Revision 1.3 1997/08/27 03:30:04 curt
280 // Changed naming scheme of basic shared structures.
282 // Revision 1.2 1997/05/29 22:39:57 curt
283 // Working on incorporating the LaRCsim flight model.
285 // Revision 1.1 1997/05/29 02:35:04 curt