]> git.mxchange.org Git - flightgear.git/blob - src/FDM/flight.cxx
ccc3f908c4f44f3b0aba492f96cb3b27120d2c0f
[flightgear.git] / 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