#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>
// base_fdm_state is the internal state that is updated in integer
// 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;
+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, FGState& f, double dt) {
+int fgFDMInit(int model, FGInterface& f, double dt) {
double save_alt = 0.0;
FG_LOG( FG_FLIGHT ,FG_INFO, "Initializing flight model" );
base_fdm_state = f;
- if ( model == FGState::FG_SLEW ) {
+ if ( model == FGInterface::FG_SLEW ) {
// fgSlewInit(dt);
- } else if ( model == FGState::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 ( base_fdm_state.get_Altitude() < -9000.0 ) {
save_alt = base_fdm_state.get_Altitude();
}
// translate FG to LaRCsim structure
- FGState_2_LaRCsim(base_fdm_state);
+ FGInterface_2_LaRCsim(base_fdm_state);
// initialize LaRCsim
fgLaRCsimInit(dt);
base_fdm_state.get_Latitude() );
// translate LaRCsim back to FG structure
- fgLaRCsim_2_FGState(base_fdm_state);
+ fgLaRCsim_2_FGInterface(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 if ( model == FGInterface::FG_EXTERNAL ) {
+ fgExternalInit(base_fdm_state);
} else {
FG_LOG( FG_FLIGHT, FG_WARN,
"Unimplemented flight model == " << model );
}
+ // set valid time for this record
+ base_fdm_state.stamp_time();
+
f = base_fdm_state;
return 1;
}
-// Extrapolate fdm based on jitter time (in milliseconds)
-static FGState extrapolate_fdm( FGState &base, int jitter ) {
- FGState result;
-
- double dt = jitter / 1000000.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) {
+int fgFDMUpdate(int model, FGInterface& f, int multiloop, int time_offset) {
double time_step, start_elev, end_elev;
// printf("Altitude = %.2f\n", FG_Altitude * 0.3048);
- // base_fdm_state = f;
+ // set valid time for this record
+ base_fdm_state.stamp_time();
time_step = (1.0 / DEFAULT_MODEL_HZ) * multiloop;
start_elev = base_fdm_state.get_Altitude();
- if ( model == FGState::FG_SLEW ) {
+ if ( model == FGInterface::FG_SLEW ) {
// fgSlewUpdate(f, multiloop);
- } else if ( model == FGState::FG_LARCSIM ) {
+ } 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);
- } else if ( model == FGState::FG_EXTERNAL ) {
+ // 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 );
base_fdm_state.set_Climb_Rate( (end_elev - start_elev) / time_step );
}
- f = extrapolate_fdm( base_fdm_state, jitter );
-
return 1;
}
// Set the altitude (force)
-void fgFlightModelSetAltitude(int model, double alt_meters) {
+void fgFDMForceAltitude(int model, double alt_meters) {
double sea_level_radius_meters;
double lat_geoc;
METER_TO_FEET) );
// additional work needed for some flight models
- if ( model == FGState::FG_LARCSIM ) {
+ 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.
//
projects / flightgear.git / blobdiff