From 3e18bab646c35b7c3e1d89e33f61c71a9186ff0e Mon Sep 17 00:00:00 2001
From: curt <curt>
Date: Tue, 19 Jan 1999 17:52:06 +0000
Subject: [PATCH] Working on being able to extrapolate a new position and
 orientation based on a position, orientation, and time offset.

---
 External/external.cxx | 13 ++++---
 External/external.hxx | 42 ++++++++-------------
 FDM/flight.cxx        | 86 ++++++++++++++++++++++---------------------
 FDM/flight.hxx        | 25 ++++++++++---
 4 files changed, 87 insertions(+), 79 deletions(-)

diff --git a/External/external.cxx b/External/external.cxx
index 10bc0553e..92930394d 100644
--- a/External/external.cxx
+++ b/External/external.cxx
@@ -1,8 +1,8 @@
-// slew.cxx -- the "slew" flight model
+// external.cxx -- externally driven flight model
 //
-// Written by Curtis Olson, started May 1997.
+// Written by Curtis Olson, started January 1998.
 //
-// Copyright (C) 1997  Curtis L. Olson  - curt@infoplane.com
+// Copyright (C) 1998  Curtis L. Olson  - curt@flightgear.org
 //
 // This program is free software; you can redistribute it and/or
 // modify it under the terms of the GNU General Public License as
@@ -31,8 +31,7 @@
 
 
 // reset flight params to a specific position
-void fgExternalInit( FGState &f, double dt ) {
-
+void fgExternalInit( FGState &f ) {
 }
 
 
@@ -43,6 +42,10 @@ void fgExternalUpdate( FGState& f, int multiloop ) {
 
 
 // $Log$
+// Revision 1.3  1999/01/19 17:52:11  curt
+// Working on being able to extrapolate a new position and orientation
+// based on a position, orientation, and time offset.
+//
 // Revision 1.2  1998/12/05 15:54:13  curt
 // Renamed class fgFLIGHT to class FGState as per request by JSB.
 //
diff --git a/External/external.hxx b/External/external.hxx
index 12b59b23d..323420ec5 100644
--- a/External/external.hxx
+++ b/External/external.hxx
@@ -36,6 +36,7 @@
 #include <Time/timestamp.hxx>
 
 
+/*
 class fgFDM_EXTERNAL {
 
 public:
@@ -61,42 +62,31 @@ public:
 
     // Velocities
 
-    // velocity relative to the local aircraft's coordinate system
-    double V_north;
-    double V_east;
-    double V_down;
-
-    // rotational rates relative to the coordinate system the body lives in
-    double P_body;
-    double Q_body;
-    double R_body;
-
-    // Accelerations
-    
-    // acceleration relative to the local aircraft's coordinate system
-    double V_dot_north;
-    double V_dot_east;
-    double V_dot_down;
-
-    // rotational acceleration relative to the coordinate system the
-    // body lives in
-    double P_dot_body;
-    double Q_dot_body;
-    double R_dot_body;
+    // velocities in geodetic coordinates
+    double Latitude_dot;   // rad/sec
+    double Longitude_dot;  // rad/sec
+    double Altitude_dot;   // feet/sec
+
+    // rotational rates
+    double Phi_dot;
+    double Theta_dot;
+    double Psi_dot;
 };
+*/
 
 
 // reset flight params to a specific position 
-void fgExternalInit( FGState& f, double dt );
-
-// update position based on inputs, positions, velocities, etc.
-void fgExternalUpdate( FGState& f, int multiloop );
+void fgExternalInit( FGState& f );
 
 
 #endif // _EXTERNAL_HXX
 
 
 // $Log$
+// Revision 1.5  1999/01/19 17:52:12  curt
+// Working on being able to extrapolate a new position and orientation
+// based on a position, orientation, and time offset.
+//
 // Revision 1.4  1999/01/09 13:37:37  curt
 // Convert fgTIMESTAMP to FGTimeStamp which holds usec instead of ms.
 //
diff --git a/FDM/flight.cxx b/FDM/flight.cxx
index ea70188a9..fbe1a048a 100644
--- a/FDM/flight.cxx
+++ b/FDM/flight.cxx
@@ -32,6 +32,7 @@
 #include <Flight/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
@@ -43,6 +44,31 @@ FGState cur_fdm_state;
 FGState base_fdm_state;
 
 
+// Extrapolate fdm based on time_offset (in usec)
+void FGState::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) {
     double save_alt = 0.0;
@@ -78,62 +104,29 @@ int fgFlightModelInit(int model, FGState& f, double dt) {
 	    base_fdm_state.set_Altitude( save_alt );
 	}
     } else if ( model == FGState::FG_EXTERNAL ) {
-	fgExternalInit(base_fdm_state, dt);
+	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 fgFlightModelUpdate(int model, FGState& 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();
@@ -142,8 +135,15 @@ int fgFlightModelUpdate(int model, FGState& f, int multiloop, int jitter) {
 	// fgSlewUpdate(f, multiloop);
     } else if ( model == FGState::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 == FGState::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 );
@@ -156,8 +156,6 @@ int fgFlightModelUpdate(int model, FGState& f, int multiloop, int jitter) {
 	base_fdm_state.set_Climb_Rate( (end_elev - start_elev) / time_step );
     }
 
-    f = extrapolate_fdm( base_fdm_state, jitter );
-
     return 1;
 }
 
@@ -184,6 +182,10 @@ void fgFlightModelSetAltitude(int model, double alt_meters) {
 
 
 // $Log$
+// 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.
 //
diff --git a/FDM/flight.hxx b/FDM/flight.hxx
index 331c96ec6..6a41775c8 100644
--- a/FDM/flight.hxx
+++ b/FDM/flight.hxx
@@ -26,6 +26,11 @@
 #define _FLIGHT_HXX
 
 
+#ifndef __cplusplus                                                          
+# error This library requires C++
+#endif                                   
+
+
 /* Required get_()
 
    `FGState::get_Longitude ()'
@@ -76,13 +81,8 @@
 
  */
 
-#include <Flight/Slew/slew.hxx>
-
-
-#ifndef __cplusplus                                                          
-# error This library requires C++
-#endif                                   
 
+#include <Time/timestamp.hxx>
 
 typedef double FG_VECTOR_3[3];
 
@@ -775,6 +775,15 @@ public:
     double        climb_rate;           // in feet per second
     inline double get_Climb_Rate() const { return climb_rate; }
     inline void set_Climb_Rate(double rate) { climb_rate = rate; }
+
+    FGTimeStamp valid_stamp;       // time this record is valid
+    FGTimeStamp next_stamp;       // time this record is valid
+    inline FGTimeStamp get_time_stamp() const { return valid_stamp; }
+    inline void stamp_time() { valid_stamp = next_stamp; next_stamp.stamp(); }
+
+    // Extrapolate FDM based on time_offset (in usec)
+    void extrapolate( int time_offset );
+
 };
 
 
@@ -797,6 +806,10 @@ void fgFlightModelSetAltitude(int model, double alt_meters);
 
 
 // $Log$
+// Revision 1.11  1999/01/19 17:52:07  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:33  curt
 // Convert fgTIMESTAMP to FGTimeStamp which holds usec instead of ms.
 //
-- 
2.39.2