#include "flight.hxx"
#include "LaRCsim.hxx"
-#include <Debug/fg_debug.h>
+#include <Debug/logstream.hxx>
+#include <Flight/External/external.hxx>
#include <Flight/LaRCsim/ls_interface.h>
#include <Include/fg_constants.h>
#include <Math/fg_geodesy.hxx>
-fgFLIGHT cur_flight_params;
+// base_fdm_state is the internal state that is updated in integer
+// multiples of "dt". This leads to "jitter" with respect to the real
+// world time, so we introduce cur_fdm_state which is extrapolated by
+// the difference between sim time and real world time
+FGState cur_fdm_state;
+FGState base_fdm_state;
-/* Initialize the flight model parameters */
-int fgFlightModelInit(int model, fgFLIGHT *f, double dt) {
+
+// Initialize the flight model parameters
+int fgFlightModelInit(int model, FGState& f, double dt) {
double save_alt = 0.0;
- int result;
- fgPrintf(FG_FLIGHT,FG_INFO,"Initializing flight model\n");
+ FG_LOG( FG_FLIGHT ,FG_INFO, "Initializing flight model" );
+
+ base_fdm_state = f;
- if ( model == FG_SLEW ) {
+ if ( model == FGState::FG_SLEW ) {
// fgSlewInit(dt);
- } else if ( model == FG_LARCSIM ) {
- /* lets try to avoid really screwing up the LaRCsim model */
- if ( FG_Altitude < -9000 ) {
- save_alt = FG_Altitude;
- FG_Altitude = 0;
+ } else if ( model == FGState::FG_LARCSIM ) {
+
+ // lets try to avoid really screwing up the LaRCsim model
+ if ( base_fdm_state.get_Altitude() < -9000.0 ) {
+ save_alt = base_fdm_state.get_Altitude();
+ base_fdm_state.set_Altitude( 0.0 );
}
- fgFlight_2_LaRCsim(f); /* translate FG to LaRCsim structure */
+ // translate FG to LaRCsim structure
+ FGState_2_LaRCsim(base_fdm_state);
+
+ // initialize LaRCsim
fgLaRCsimInit(dt);
- fgPrintf( FG_FLIGHT, FG_INFO, "FG pos = %.2f\n", FG_Latitude );
- fgLaRCsim_2_Flight(f); /* translate LaRCsim back to FG structure */
- /* but lets restore our original bogus altitude when we are done */
- if ( save_alt < -9000 ) {
- FG_Altitude = save_alt;
+ FG_LOG( FG_FLIGHT, FG_INFO, "FG pos = " <<
+ base_fdm_state.get_Latitude() );
+
+ // translate LaRCsim back to FG structure
+ fgLaRCsim_2_FGState(base_fdm_state);
+
+ // but lets restore our original bogus altitude when we are done
+ if ( save_alt < -9000.0 ) {
+ base_fdm_state.set_Altitude( save_alt );
}
+ } else if ( model == FGState::FG_EXTERNAL ) {
+ fgExternalInit(base_fdm_state, dt);
} else {
- fgPrintf( FG_FLIGHT, FG_WARN,
- "Unimplemented flight model == %d\n", model );
+ FG_LOG( FG_FLIGHT, FG_WARN,
+ "Unimplemented flight model == " << model );
}
- result = 1;
+ f = base_fdm_state;
- return(result);
+ return 1;
}
-/* Run multiloop iterations of the flight model */
-int fgFlightModelUpdate(int model, fgFLIGHT *f, int multiloop) {
+// Extrapolate fdm based on jitter time (in milliseconds)
+static FGState extrapolate_fdm( FGState &base, int jitter ) {
+ FGState result;
+
+ double dt = jitter / 1000.0;
+ // cout << "dt = " << dt << endl;
+
+ // start by making a straight up copy
+ result = base;
+
+ double lon = base.get_Longitude() + base.get_Longitude_dot() * dt;
+ double lon_geoc = base.get_Lon_geocentric() + base.get_Longitude_dot() * dt;
+
+ double lat = base.get_Latitude() + base.get_Latitude_dot() * dt;
+ double lat_geoc = base.get_Lat_geocentric() + base.get_Latitude_dot() * dt;
+
+ /*
+ cout << "( " << base.get_Longitude() << ", " <<
+ base.get_Latitude() << " )" << endl;
+ cout << "( " << lon << ", " << lat << " )" << endl;
+ cout << "( " << base.get_Longitude_dot() * dt << ", " <<
+ base.get_Latitude_dot() * dt << ", " <<
+ base.get_Radius_dot() * dt << " )" << endl;
+ */
+
+ double alt = base.get_Altitude() + base.get_Radius_dot() * dt;
+ double radius = base.get_Radius_to_vehicle() + base.get_Radius_dot() * dt;
+
+ result.set_Longitude( lon );
+ result.set_Latitude( lat );
+ result.set_Altitude( alt );
+ // result.set_Geocentric_Position( lon_geoc, lat_geoc, radius );
+
+ return result;
+}
+
+
+// Run multiloop iterations of the flight model
+int fgFlightModelUpdate(int model, FGState& f, int multiloop, int jitter) {
double time_step, start_elev, end_elev;
- int result;
+
// printf("Altitude = %.2f\n", FG_Altitude * 0.3048);
+ // base_fdm_state = f;
+
time_step = (1.0 / DEFAULT_MODEL_HZ) * multiloop;
- start_elev = FG_Altitude;
+ start_elev = base_fdm_state.get_Altitude();
- if ( model == FG_SLEW ) {
+ if ( model == FGState::FG_SLEW ) {
// fgSlewUpdate(f, multiloop);
- } else if ( model == FG_LARCSIM ) {
- fgLaRCsimUpdate(f, multiloop);
+ } else if ( model == FGState::FG_LARCSIM ) {
+ fgLaRCsimUpdate(base_fdm_state, multiloop);
+ } else if ( model == FGState::FG_EXTERNAL ) {
+ // fgExternalUpdate(f, multiloop);
} else {
- fgPrintf( FG_FLIGHT, FG_WARN,
- "Unimplemented flight model == %d\n", model );
+ FG_LOG( FG_FLIGHT, FG_WARN,
+ "Unimplemented flight model == " << model );
}
- end_elev = FG_Altitude;
+ end_elev = base_fdm_state.get_Altitude();
- FG_Climb_Rate = (end_elev - start_elev) / time_step; /* feet per second */
+ if ( time_step > 0.0 ) {
+ // feet per second
+ base_fdm_state.set_Climb_Rate( (end_elev - start_elev) / time_step );
+ }
- result = 1;
+ f = extrapolate_fdm( base_fdm_state, jitter );
- return(result);
+ return 1;
}
-/* Set the altitude (force) */
-void fgFlightModelSetAltitude(int model, fgFLIGHT *f, double alt_meters) {
+// Set the altitude (force)
+void fgFlightModelSetAltitude(int model, double alt_meters) {
double sea_level_radius_meters;
double lat_geoc;
+
// Set the FG variables first
- fgGeodToGeoc( FG_Latitude, alt_meters,
+ fgGeodToGeoc( base_fdm_state.get_Latitude(), alt_meters,
&sea_level_radius_meters, &lat_geoc);
- FG_Altitude = alt_meters * METER_TO_FEET;
- FG_Radius_to_vehicle = FG_Altitude +
- (sea_level_radius_meters * METER_TO_FEET);
+ base_fdm_state.set_Altitude( alt_meters * METER_TO_FEET );
+ base_fdm_state.set_Radius_to_vehicle( base_fdm_state.get_Altitude() +
+ (sea_level_radius_meters *
+ METER_TO_FEET) );
- /* additional work needed for some flight models */
- if ( model == FG_LARCSIM ) {
- ls_ForceAltitude(FG_Altitude);
+ // additional work needed for some flight models
+ if ( model == FGState::FG_LARCSIM ) {
+ ls_ForceAltitude( base_fdm_state.get_Altitude() );
}
-
}
// $Log$
+// Revision 1.9 1999/01/08 19:27:37 curt
+// Fixed AOA reading on HUD.
+// Continued work on time jitter compensation.
+//
+// Revision 1.8 1999/01/08 03:23:51 curt
+// Beginning work on compensating for sim time vs. real world time "jitter".
+//
+// Revision 1.7 1998/12/18 23:37:07 curt
+// Collapsed out the FGState variables not currently needed. They are just
+// commented out and can be readded easily at any time. The point of this
+// exersize is to determine which variables were or were not currently being
+// used.
+//
+// Revision 1.6 1998/12/05 15:54:11 curt
+// Renamed class fgFLIGHT to class FGState as per request by JSB.
+//
+// Revision 1.5 1998/12/04 01:29:39 curt
+// Stubbed in a new flight model called "External" which is expected to be driven
+// from some external source.
+//
+// Revision 1.4 1998/12/03 01:16:40 curt
+// Converted fgFLIGHT to a class.
+//
+// Revision 1.3 1998/11/06 21:18:03 curt
+// Converted to new logstream debugging facility. This allows release
+// builds with no messages at all (and no performance impact) by using
+// the -DFG_NDEBUGNDEBUG flag.
+//
// Revision 1.2 1998/10/16 23:27:40 curt
// C++-ifying.
//
projects / flightgear.git / blobdiff