#include <stdio.h>
#include "flight.hxx"
+#include "JSBsim.hxx"
#include "LaRCsim.hxx"
#include <Debug/logstream.hxx>
-#include <Flight/External/external.hxx>
-#include <Flight/LaRCsim/ls_interface.h>
+#include <FDM/External/external.hxx>
+#include <FDM/LaRCsim/ls_interface.h>
#include <Include/fg_constants.h>
#include <Math/fg_geodesy.hxx>
+#include <Time/timestamp.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
+
+FGInterface cur_fdm_state;
+FGInterface base_fdm_state;
+
+
+// Extrapolate fdm based on time_offset (in usec)
+void FGInterface::extrapolate( int time_offset ) {
+ double dt = time_offset / 1000000.0;
+ cout << "extrapolating FDM by dt = " << dt << endl;
+
+ double lat = geodetic_position_v[0] + geocentric_rates_v[0] * dt;
+ double lat_geoc = geocentric_position_v[0] + geocentric_rates_v[0] * dt;
+
+ double lon = geodetic_position_v[1] + geocentric_rates_v[1] * dt;
+ double lon_geoc = geocentric_position_v[1] + geocentric_rates_v[1] * dt;
+
+ double alt = geodetic_position_v[2] + geocentric_rates_v[2] * dt;
+ double radius = geocentric_position_v[2] + geocentric_rates_v[2] * dt;
+
+ geodetic_position_v[0] = lat;
+ geocentric_position_v[0] = lat_geoc;
+
+ geodetic_position_v[1] = lon;
+ geocentric_position_v[1] = lon_geoc;
+
+ geodetic_position_v[2] = alt;
+ geocentric_position_v[2] = radius;
+}
// Initialize the flight model parameters
-int fgFlightModelInit(int model, fgFLIGHT& f, double dt) {
+int fgFDMInit(int model, FGInterface& f, double dt) {
double save_alt = 0.0;
- int result;
FG_LOG( FG_FLIGHT ,FG_INFO, "Initializing flight model" );
- if ( model == fgFLIGHT::FG_SLEW ) {
+ base_fdm_state = f;
+
+ if ( model == FGInterface::FG_SLEW ) {
// fgSlewInit(dt);
- } else if ( model == fgFLIGHT::FG_LARCSIM ) {
+ } else if ( model == FGInterface::FG_JSBSIM ) {
+ fgJSBsimInit(dt);
+ fgJSBsim_2_FGInterface(base_fdm_state);
+ } else if ( model == FGInterface::FG_LARCSIM ) {
// lets try to avoid really screwing up the LaRCsim model
- if ( f.get_Altitude() < -9000.0 ) {
- save_alt = f.get_Altitude();
- f.set_Altitude( 0.0 );
+ 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
+ FGInterface_2_LaRCsim(base_fdm_state);
+
+ // initialize LaRCsim
fgLaRCsimInit(dt);
- FG_LOG( FG_FLIGHT, FG_INFO, "FG pos = " << f.get_Latitude() );
- fgLaRCsim_2_Flight(f); // translate LaRCsim back to FG structure
+
+ FG_LOG( FG_FLIGHT, FG_INFO, "FG pos = " <<
+ base_fdm_state.get_Latitude() );
+
+ // translate LaRCsim back to FG structure
+ fgLaRCsim_2_FGInterface(base_fdm_state);
// but lets restore our original bogus altitude when we are done
if ( save_alt < -9000.0 ) {
- f.set_Altitude( save_alt );
+ base_fdm_state.set_Altitude( save_alt );
}
- } else if ( model == fgFLIGHT::FG_EXTERNAL ) {
- fgExternalInit(f, dt);
+ } else if ( model == FGInterface::FG_EXTERNAL ) {
+ fgExternalInit(base_fdm_state);
} else {
FG_LOG( FG_FLIGHT, FG_WARN,
"Unimplemented flight model == " << model );
}
- result = 1;
+ // set valid time for this record
+ base_fdm_state.stamp_time();
+
+ f = base_fdm_state;
- return(result);
+ return 1;
}
// Run multiloop iterations of the flight model
-int fgFlightModelUpdate(int model, fgFLIGHT& f, int multiloop) {
+int fgFDMUpdate(int model, FGInterface& f, int multiloop, int time_offset) {
double time_step, start_elev, end_elev;
- int result;
+
// printf("Altitude = %.2f\n", FG_Altitude * 0.3048);
+ // set valid time for this record
+ base_fdm_state.stamp_time();
+
time_step = (1.0 / DEFAULT_MODEL_HZ) * multiloop;
- start_elev = f.get_Altitude();
+ start_elev = base_fdm_state.get_Altitude();
- if ( model == fgFLIGHT::FG_SLEW ) {
+ if ( model == FGInterface::FG_SLEW ) {
// fgSlewUpdate(f, multiloop);
- } else if ( model == fgFLIGHT::FG_LARCSIM ) {
- fgLaRCsimUpdate(f, multiloop);
- } else if ( model == fgFLIGHT::FG_EXTERNAL ) {
+ } else if ( model == FGInterface::FG_JSBSIM ) {
+ fgJSBsimUpdate(base_fdm_state, multiloop);
+ f = base_fdm_state;
+ } else if ( model == FGInterface::FG_LARCSIM ) {
+ fgLaRCsimUpdate(base_fdm_state, multiloop);
+ // extrapolate position based on actual time
+ // f = extrapolate_fdm( base_fdm_state, time_offset );
+ f = base_fdm_state;
+ } else if ( model == FGInterface::FG_EXTERNAL ) {
// fgExternalUpdate(f, multiloop);
+ FGTimeStamp current;
+ current.stamp();
+ f = base_fdm_state;
+ f.extrapolate( current - base_fdm_state.get_time_stamp() );
} else {
FG_LOG( FG_FLIGHT, FG_WARN,
"Unimplemented flight model == " << model );
}
- end_elev = f.get_Altitude();
+ end_elev = base_fdm_state.get_Altitude();
- // feet per second
- f.set_Climb_Rate( (end_elev - start_elev) / time_step );
-
- result = 1;
+ if ( time_step > 0.0 ) {
+ // feet per second
+ base_fdm_state.set_Climb_Rate( (end_elev - start_elev) / time_step );
+ }
- return(result);
+ return 1;
}
// Set the altitude (force)
-void fgFlightModelSetAltitude(int model, fgFLIGHT& f, double alt_meters) {
+void fgFDMForceAltitude(int model, double alt_meters) {
double sea_level_radius_meters;
double lat_geoc;
+
// Set the FG variables first
- fgGeodToGeoc( f.get_Latitude(), alt_meters,
+ fgGeodToGeoc( base_fdm_state.get_Latitude(), alt_meters,
&sea_level_radius_meters, &lat_geoc);
- f.set_Altitude( alt_meters * METER_TO_FEET );
- f.set_Radius_to_vehicle( f.get_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 == fgFLIGHT::FG_LARCSIM ) {
- ls_ForceAltitude( f.get_Altitude() );
+ if ( model == FGInterface::FG_LARCSIM ) {
+ ls_ForceAltitude( base_fdm_state.get_Altitude() );
}
+}
+
+// Set the local ground elevation
+void fgFDMSetGroundElevation(int model, double ground_meters) {
+ base_fdm_state.set_Runway_altitude( ground_meters * METER_TO_FEET );
+ cur_fdm_state.set_Runway_altitude( ground_meters * METER_TO_FEET );
}
// $Log$
+// Revision 1.15 1999/02/05 21:29:01 curt
+// Modifications to incorporate Jon S. Berndts flight model code.
+//
+// Revision 1.14 1999/02/01 21:33:31 curt
+// Renamed FlightGear/Simulator/Flight to FlightGear/Simulator/FDM since
+// Jon accepted my offer to do this and thought it was a good idea.
+//
+// Revision 1.13 1999/01/27 04:48:39 curt
+// Set the runway height in cur_fdm_state as well as base_fdm_state.
+//
+// Revision 1.12 1999/01/20 13:42:22 curt
+// Tweaked FDM interface.
+// Testing check sum support for NMEA serial output.
+//
+// Revision 1.11 1999/01/19 17:52:06 curt
+// Working on being able to extrapolate a new position and orientation
+// based on a position, orientation, and time offset.
+//
+// Revision 1.10 1999/01/09 13:37:32 curt
+// Convert fgTIMESTAMP to FGTimeStamp which holds usec instead of ms.
+//
+// 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.
projects / flightgear.git / blobdiff