Andreas Gaeb: fix #222 (JSBSIm reset problems)

[flightgear.git] / src / FDM / JSBSim / models / FGPropagate.cpp

diff --git a/src/FDM/JSBSim/models/FGPropagate.cpp b/src/FDM/JSBSim/models/FGPropagate.cpp
index 5405c863983c2241b03487527ded86bec28e22ff..4d1ef44a858a60d532d336d532d30f10673ced8c 100644 (file)
--- a/src/FDM/JSBSim/models/FGPropagate.cpp
+++ b/src/FDM/JSBSim/models/FGPropagate.cpp
@@ -71,14 +71,15 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.74 2010/11/28 13:02:43 bcoconni Exp $";
+static const char *IdSrc = "$Id: FGPropagate.cpp,v 1.76 2011/01/16 16:10:59 bcoconni Exp $";
 static const char *IdHdr = ID_PROPAGATE;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 CLASS IMPLEMENTATION
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex)
+FGPropagate::FGPropagate(FGFDMExec* fdmex) : FGModel(fdmex),
+LocalTerrainRadius(0), SeaLevelRadius(0), VehicleRadius(0)
 {
   Debug(0);
   Name = "FGPropagate";
@@ -157,8 +158,8 @@ void FGPropagate::SetInitialState(const FGInitialCondition *FGIC)
 
   VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
 
-  Ti2ec = GetTi2ec();         // ECI to ECEF transform
-  Tec2i = Ti2ec.Transposed(); // ECEF to ECI frame transform
+  Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
+  Tec2i = Ti2ec.Transposed();          // ECEF to ECI frame transform
 
   VState.vInertialPosition = Tec2i * VState.vLocation;
 
@@ -260,8 +261,8 @@ bool FGPropagate::Run(void)
   VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
 
   // 2. Update the Ti2ec and Tec2i transforms from the updated EPA
-  Ti2ec = GetTi2ec();          // ECI to ECEF transform
-  Tec2i = Ti2ec.Transposed();  // ECEF to ECI frame transform
+  Ti2ec = VState.vLocation.GetTi2ec(); // ECI to ECEF transform
+  Tec2i = Ti2ec.Transposed();          // ECEF to ECI frame transform
 
   // 3. Update the location from the updated Ti2ec and inertial position
   VState.vLocation = Ti2ec*VState.vInertialPosition;
@@ -561,7 +562,7 @@ void FGPropagate::ResolveFrictionForces(double dt)
 
   // Prepare the linear system for the Gauss-Seidel algorithm :
   // 1. Compute the right hand side member 'rhs'
-  // 2. Divide every line of 'a' and 'lhs' by a[i,i]. This is in order to save
+  // 2. Divide every line of 'a' and 'rhs' by a[i,i]. This is in order to save
   //    a division computation at each iteration of Gauss-Seidel.
   for (int i=0; i < n; i++) {
     double d = 1.0 / a[i*n+i];
@@ -619,22 +620,22 @@ void FGPropagate::ResolveFrictionForces(double dt)
 
 void FGPropagate::UpdateLocationMatrices(void)
 {
-  Tl2ec = GetTl2ec();          // local to ECEF transform
-  Tec2l = Tl2ec.Transposed();  // ECEF to local frame transform
-  Ti2l  = GetTi2l();
-  Tl2i  = Ti2l.Transposed();
+  Tl2ec = VState.vLocation.GetTl2ec(); // local to ECEF transform
+  Tec2l = Tl2ec.Transposed();          // ECEF to local frame transform
+  Ti2l  = VState.vLocation.GetTi2l();  // ECI to local frame transform
+  Tl2i  = Ti2l.Transposed();           // local to ECI transform
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGPropagate::UpdateBodyMatrices(void)
 {
-  Ti2b  = GetTi2b();           // ECI to body frame transform
-  Tb2i  = Ti2b.Transposed();   // body to ECI frame transform
-  Tl2b  = Ti2b*Tl2i;           // local to body frame transform
-  Tb2l  = Tl2b.Transposed();   // body to local frame transform
-  Tec2b = Tl2b * Tec2l;        // ECEF to body frame transform
-  Tb2ec = Tec2b.Transposed();  // body to ECEF frame tranform
+  Ti2b  = VState.qAttitudeECI.GetT(); // ECI to body frame transform
+  Tb2i  = Ti2b.Transposed();          // body to ECI frame transform
+  Tl2b  = Ti2b*Tl2i;                  // local to body frame transform
+  Tb2l  = Tl2b.Transposed();          // body to local frame transform
+  Tec2b = Tl2b * Tec2l;               // ECEF to body frame transform
+  Tb2ec = Tec2b.Transposed();         // body to ECEF frame tranform
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -651,7 +652,7 @@ void FGPropagate::SetInertialOrientation(FGQuaternion Qi) {
 void FGPropagate::SetInertialVelocity(FGColumnVector3 Vi) {
   VState.vInertialVelocity = Vi;
   CalculateUVW();
-  vVel = GetTb2l() * VState.vUVW;
+  vVel = Tb2l * VState.vUVW;
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -715,48 +716,57 @@ double FGPropagate::GetTerrainElevation(void) const
   return FDMExec->GetGroundCallback()->GetTerrainGeoCentRadius()-SeaLevelRadius;
 }
 
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-//Todo: when should this be called - when should the new EPA be used to
-// calculate the transformation matrix, so that the matrix is not a step
-// ahead of the sim and the associated calculations?
-const FGMatrix33& FGPropagate::GetTi2ec(void)
-{
-  return VState.vLocation.GetTi2ec();
-}
-
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-const FGMatrix33& FGPropagate::GetTec2i(void)
+double FGPropagate::GetDistanceAGL(void) const
 {
-  return VState.vLocation.GetTec2i();
+  return VState.vLocation.GetRadius() - LocalTerrainRadius;
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-void FGPropagate::SetAltitudeASL(double altASL)
+void FGPropagate::SetVState(const VehicleState& vstate)
 {
-  VState.vLocation.SetRadius( altASL + SeaLevelRadius );
-}
-
-//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+  VState.vLocation = vstate.vLocation;
+  VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
+  Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
+  Tec2i = Ti2ec.Transposed();
+  UpdateLocationMatrices();
+  SetInertialOrientation(vstate.qAttitudeECI);
+  RecomputeLocalTerrainRadius();
+  VehicleRadius = GetRadius();
+  VState.vUVW = vstate.vUVW;
+  vVel = Tb2l * VState.vUVW;
+  VState.vPQR = vstate.vPQR;
+  VState.vPQRi = VState.vPQR + Ti2b * vOmegaEarth;
+  VState.vPQRi_i = Tb2i * VState.vPQRi;
+  VState.vInertialPosition = vstate.vInertialPosition;
 
-double FGPropagate::GetLocalTerrainRadius(void) const
-{
-  return LocalTerrainRadius;
+  InitializeDerivatives();
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-double FGPropagate::GetDistanceAGL(void) const
+void FGPropagate::UpdateVehicleState(void)
 {
-  return VState.vLocation.GetRadius() - LocalTerrainRadius;
+  RecomputeLocalTerrainRadius();
+  VehicleRadius = GetRadius();
+  VState.vInertialPosition = Tec2i * VState.vLocation;
+  UpdateLocationMatrices();
+  UpdateBodyMatrices();
+  vVel = Tb2l * VState.vUVW;
+  VState.qAttitudeLocal = Tl2b.GetQuaternion();
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-void FGPropagate::SetDistanceAGL(double tt)
+void FGPropagate::SetLocation(const FGLocation& l)
 {
-  VState.vLocation.SetRadius( tt + LocalTerrainRadius );
+  VState.vLocation = l;
+  VState.vLocation.SetEarthPositionAngle(FDMExec->GetInertial()->GetEarthPositionAngle());
+  Ti2ec = VState.vLocation.GetTi2ec(); // useless ?
+  Tec2i = Ti2ec.Transposed();
+  UpdateVehicleState();
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -784,7 +794,7 @@ void FGPropagate::DumpState(void)
   cout << endl << "  " << underon
        <<   "Velocity" << underoff << endl;
   cout << "    ECI:   " << VState.vInertialVelocity.Dump(", ") << " (x,y,z in ft/s)" << endl;
-  cout << "    ECEF:  " << (GetTb2ec() * VState.vUVW).Dump(", ")  << " (x,y,z in ft/s)"  << endl;
+  cout << "    ECEF:  " << (Tb2ec * VState.vUVW).Dump(", ")  << " (x,y,z in ft/s)"  << endl;
   cout << "    Local: " << GetVel() << " (n,e,d in ft/sec)" << endl;
   cout << "    Body:  " << GetUVW() << " (u,v,w in ft/sec)" << endl;