Encapsulate the interpolstion version of FGEnvironment and fix some bugs

[flightgear.git] / src / FDM / JSBSim / FGTrimAxis.cpp

diff --git a/src/FDM/JSBSim/FGTrimAxis.cpp b/src/FDM/JSBSim/FGTrimAxis.cpp
index 2fe3213f168c2aee9fa0ae01502f7e495472627e..e95d1c54bfc0e564f268172178d6e592be3d2643 100644 (file)
--- a/src/FDM/JSBSim/FGTrimAxis.cpp
+++ b/src/FDM/JSBSim/FGTrimAxis.cpp
@@ -32,6 +32,10 @@
 INCLUDES
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
+#ifdef _MSC_VER
+#  pragma warning (disable : 4786)
+#endif
+
 #include <string>
 #include <stdlib.h>
 
@@ -41,6 +45,10 @@ INCLUDES
 #include "FGTrimAxis.h"
 #include "FGAircraft.h"
 #include "FGPropulsion.h"
+#include "FGAerodynamics.h"
+
+
+namespace JSBSim {
 
 static const char *IdSrc = "$Id$";
 static const char *IdHdr = ID_TRIMAXIS;
@@ -72,6 +80,7 @@ FGTrimAxis::FGTrimAxis(FGFDMExec* fdex, FGInitialCondition* ic, State st,
     case tRdot: tolerance = DEFAULT_TOLERANCE / 10; break;
     case tHmgt: tolerance = 0.01; break;
     case  tNlf: state_target=1.0; tolerance = 1E-5; break;
+    case tAll: break;
   }  
   
   solver_eps=tolerance;
@@ -87,8 +96,8 @@ FGTrimAxis::FGTrimAxis(FGFDMExec* fdex, FGInitialCondition* ic, State st,
     control_convert=radtodeg;
     break;
   case tAlpha:
-    control_min=fdmex->GetAircraft()->GetAlphaCLMin();
-    control_max=fdmex->GetAircraft()->GetAlphaCLMax();
+    control_min=fdmex->GetAerodynamics()->GetAlphaCLMin();
+    control_max=fdmex->GetAerodynamics()->GetAlphaCLMax();
     if(control_max <= control_min) {
       control_max=20*degtorad;
       control_min=-5*degtorad;
@@ -153,14 +162,15 @@ FGTrimAxis::~FGTrimAxis(void)
 
 void FGTrimAxis::getState(void) {
   switch(state) {
-  case tUdot: state_value=fdmex->GetTranslation()->GetUVWdot()(1)-state_target; break;
-  case tVdot: state_value=fdmex->GetTranslation()->GetUVWdot()(2)-state_target; break;
-  case tWdot: state_value=fdmex->GetTranslation()->GetUVWdot()(3)-state_target; break;
+  case tUdot: state_value=fdmex->GetTranslation()->GetUVWdot(1)-state_target; break;
+  case tVdot: state_value=fdmex->GetTranslation()->GetUVWdot(2)-state_target; break;
+  case tWdot: state_value=fdmex->GetTranslation()->GetUVWdot(3)-state_target; break;
   case tQdot: state_value=fdmex->GetRotation()->GetPQRdot(2)-state_target;break;
   case tPdot: state_value=fdmex->GetRotation()->GetPQRdot(1)-state_target; break;
   case tRdot: state_value=fdmex->GetRotation()->GetPQRdot(3)-state_target; break;
   case tHmgt: state_value=computeHmgt()-state_target; break;
   case tNlf:  state_value=fdmex->GetAircraft()->GetNlf()-state_target; break;
+  case tAll: break;
   }
 }
 
@@ -313,22 +323,24 @@ bool FGTrimAxis::initTheta(void) {
   level=false;
   theta=fgic->GetPitchAngleDegIC(); 
   while(!level && (i < 100)) {
-       theta+=2.0*zDiff;
-       fgic->SetPitchAngleDegIC(theta);   
-       fdmex->RunIC(fgic);
-       zAft=fdmex->GetGroundReactions()->GetGearUnit(1)->GetLocalGear(3);
-        zForward=fdmex->GetGroundReactions()->GetGearUnit(0)->GetLocalGear(3);
-        zDiff = zForward - zAft;
-       //cout << endl << theta << "  " << zDiff << endl;
-       //cout << "0: " << fdmex->GetGroundReactions()->GetGearUnit(0)->GetLocalGear() << endl;
-       //cout << "1: " << fdmex->GetGroundReactions()->GetGearUnit(1)->GetLocalGear() << endl;
-
-       if(fabs(zDiff ) < 0.1) 
-           level=true;
-       i++;   
+         theta+=2.0*zDiff;
+         fgic->SetPitchAngleDegIC(theta);   
+         fdmex->RunIC();
+         zAft=fdmex->GetGroundReactions()->GetGearUnit(iAft)->GetLocalGear(3);
+    zForward=fdmex->GetGroundReactions()->GetGearUnit(iForward)->GetLocalGear(3);
+    zDiff = zForward - zAft;
+         //cout << endl << theta << "  " << zDiff << endl;
+         //cout << "0: " << fdmex->GetGroundReactions()->GetGearUnit(0)->GetLocalGear() << endl;
+         //cout << "1: " << fdmex->GetGroundReactions()->GetGearUnit(1)->GetLocalGear() << endl;
+         if(fabs(zDiff ) < 0.1) 
+             level=true;
+         i++;   
   }                    
   //cout << i << endl;
-  cout << "    Initial Theta: " << fdmex->GetRotation()->Gettht()*radtodeg << endl;
+  if (debug_lvl > 0) {
+      cout << "    Initial Theta: " << fdmex->GetRotation()->Gettht()*radtodeg << endl;
+      cout << "    Used gear unit " << iAft << " as aft and " << iForward << " as forward" << endl;
+  }
   control_min=(theta+5)*degtorad;
   control_max=(theta-5)*degtorad;
   fgic->SetAltitudeAGLFtIC(saveAlt);
@@ -364,7 +376,7 @@ void FGTrimAxis::SetPhiOnGround(double ff) {
     fgic->SetAltitudeAGLFtIC(hagl);
   }                   
   fgic->SetRollAngleRadIC(ff);
-           
+
 }      
 
 /*****************************************************************************/
@@ -380,7 +392,7 @@ void FGTrimAxis::Run(void) {
   while(!stable) {
     i++;
     last_state_value=state_value;
-    fdmex->RunIC(fgic);
+    fdmex->RunIC();
     getState();
     if(i > 1) {
       if((fabs(last_state_value - state_value) < tolerance) || (i >= 100) )
@@ -403,7 +415,7 @@ void FGTrimAxis::setThrottlesPct(void) {
       //cout << "setThrottlespct: " << i << ", " << control_min << ", " << control_max << ", " << control_value;
       fdmex->GetFCS()->SetThrottleCmd(i,tMin+control_value*(tMax-tMin));
       //cout << "setThrottlespct: " << fdmex->GetFCS()->GetThrottleCmd(i) << endl;
-      fdmex->RunIC(fgic); //apply throttle change
+      fdmex->RunIC(); //apply throttle change
       fdmex->GetPropulsion()->GetSteadyState();
   }
 }
@@ -450,9 +462,9 @@ double FGTrimAxis::GetAvgStability( void ) {
 
 void FGTrimAxis::Debug(int from)
 {
-  if (debug_lvl <= 0) return;
 
-  if (debug_lvl & 1) { // Standard console startup message output
+  if (debug_lvl <= 0) return;
+  if (debug_lvl & 1 ) { // Standard console startup message output
     if (from == 0) { // Constructor
 
     }
@@ -467,5 +479,11 @@ void FGTrimAxis::Debug(int from)
   }
   if (debug_lvl & 16) { // Sanity checking
   }
+  if (debug_lvl & 64) {
+    if (from == 0) { // Constructor
+      cout << IdSrc << endl;
+      cout << IdHdr << endl;
+    }
+  }
+}
 }
-