From f526dcdf3ab3a503133b5d183d5cf41672f8a8b0 Mon Sep 17 00:00:00 2001
From: Bertrand Coconnier <bcoconni@users.sourceforge.net>
Date: Sat, 16 Jan 2016 19:32:50 +0100
Subject: [PATCH] Fixed the initialization process for JSBSim models: the error
 message "FGTrim::calcRotation DistPlane^2 larger than sqrRadius" should no
 longer be issued

---
 src/FDM/JSBSim/initialization/FGTrim.cpp     |  7 ++++---
 src/FDM/JSBSim/initialization/FGTrimAxis.cpp | 14 +++++++-------
 2 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/src/FDM/JSBSim/initialization/FGTrim.cpp b/src/FDM/JSBSim/initialization/FGTrim.cpp
index 526b81f22..9472400c9 100644
--- a/src/FDM/JSBSim/initialization/FGTrim.cpp
+++ b/src/FDM/JSBSim/initialization/FGTrim.cpp
@@ -57,7 +57,7 @@ using namespace std;
 
 namespace JSBSim {
 
-IDENT(IdSrc,"$Id: FGTrim.cpp,v 1.29 2015/12/13 08:19:57 bcoconni Exp $");
+IDENT(IdSrc,"$Id: FGTrim.cpp,v 1.30 2015/12/29 13:44:39 ehofman Exp $");
 IDENT(IdHdr,ID_TRIM);
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -74,6 +74,7 @@ FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt)
 
   Debug=0;DebugLevel=0;
   fdmex=FDMExec;
+  fgic = *fdmex->GetIC();
   total_its=0;
   gamma_fallback=false;
   mode=tt;
@@ -196,7 +197,6 @@ bool FGTrim::DoTrim(void) {
   double aileron0 = FCS->GetDaCmd();
   double rudder0 = FCS->GetDrCmd();
   double PitchTrim0 = FCS->GetPitchTrimCmd();
-  fgic = *fdmex->GetIC();
 
   for(int i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
     fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
@@ -210,6 +210,8 @@ bool FGTrim::DoTrim(void) {
   fgic.SetRRadpsIC(0.0);
 
   if (mode == tGround) {
+    fdmex->Initialize(&fgic);
+    fdmex->Run();
     trimOnGround();
     double theta = fgic.GetThetaRadIC();
     double phi = fgic.GetPhiRadIC();
@@ -790,7 +792,6 @@ void FGTrim::setDebug(FGTrimAxis& axis) {
 
 void FGTrim::SetMode(TrimMode tt) {
     ClearStates();
-    fdmex->GetPropagate()->SetInitialState(&fgic);
     mode=tt;
     switch(tt) {
       case tFull:
diff --git a/src/FDM/JSBSim/initialization/FGTrimAxis.cpp b/src/FDM/JSBSim/initialization/FGTrimAxis.cpp
index 69a03ab4a..d78e53d99 100644
--- a/src/FDM/JSBSim/initialization/FGTrimAxis.cpp
+++ b/src/FDM/JSBSim/initialization/FGTrimAxis.cpp
@@ -126,17 +126,17 @@ FGTrimAxis::FGTrimAxis(FGFDMExec* fdex, FGInitialCondition* ic, State st,
   case tAltAGL:
     control_min=0;
     control_max=30;
-    control_value=fdmex->GetPropagate()->GetDistanceAGL();
+    control_value=ic->GetAltitudeAGLFtIC();
     solver_eps=tolerance/100;
     break;
   case tTheta:
-    control_min=fdmex->GetPropagate()->GetEuler(eTht) - 5*degtorad;
-    control_max=fdmex->GetPropagate()->GetEuler(eTht) + 5*degtorad;
+    control_min=ic->GetThetaRadIC() - 5*degtorad;
+    control_max=ic->GetThetaRadIC() + 5*degtorad;
     state_convert=radtodeg;
     break;
   case tPhi:
-    control_min=fdmex->GetPropagate()->GetEuler(ePhi) - 30*degtorad;
-    control_max=fdmex->GetPropagate()->GetEuler(ePhi) + 30*degtorad;
+    control_min=ic->GetPhiRadIC() - 30*degtorad;
+    control_max=ic->GetPhiRadIC() + 30*degtorad;
     state_convert=radtodeg;
     control_convert=radtodeg;
     break;
@@ -147,8 +147,8 @@ FGTrimAxis::FGTrimAxis(FGFDMExec* fdex, FGInitialCondition* ic, State st,
     control_convert=radtodeg;
     break;
   case tHeading:
-    control_min=fdmex->GetPropagate()->GetEuler(ePsi) - 30*degtorad;
-    control_max=fdmex->GetPropagate()->GetEuler(ePsi) + 30*degtorad;
+    control_min=ic->GetPsiRadIC() - 30*degtorad;
+    control_max=ic->GetPsiRadIC() + 30*degtorad;
     state_convert=radtodeg;
     break;
   }
-- 
2.39.5