JSBSim updates. This update changes the file format, so an update of the base

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

diff --git a/src/FDM/JSBSim/FGTrim.cpp b/src/FDM/JSBSim/FGTrim.cpp
index c3adc62c28004030f98374dccacb1dd5ee3bf69e..21dae465da7fc1f369c27f980858bca47d040159 100644 (file)
--- a/src/FDM/JSBSim/FGTrim.cpp
+++ b/src/FDM/JSBSim/FGTrim.cpp
@@ -63,7 +63,7 @@ static const char *IdHdr = ID_TRIM;
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
+FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
 
   N=Nsub=0;
   max_iterations=60;
@@ -73,65 +73,16 @@ FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
   
   Debug=0;DebugLevel=0;
   fdmex=FDMExec;
-  fgic=FGIC;
+  fgic=fdmex->GetIC();
   total_its=0;
   trimudot=true;
   gamma_fallback=true;
   axis_count=0;
   mode=tt;
-  xlo=xhi=alo=ahi;
+  xlo=xhi=alo=ahi=0.0;
   targetNlf=1.0;
   debug_axis=tAll;
-  switch(mode) {
-  case tFull:
-    cout << "  Full Trim" << endl;
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
-    break;
-  case tLongitudinal:
-    cout << "  Longitudinal Trim" << endl;
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
-    break;
-  case tGround:
-    cout << "  Ground Trim" << endl;
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
-    //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
-    break;
-  case tPullup:
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
-    break;
-  case tTurn:
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
-    break;
-  case tCustom:
-  case tNone:
-    break;
-}
-  //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
-  sub_iterations=new double[TrimAxes.size()];
-  successful=new double[TrimAxes.size()];
-  solution=new bool[TrimAxes.size()];
-  current_axis=0;
-  
+  SetMode(tt);
   if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
 }
 
@@ -193,7 +144,7 @@ void FGTrim::ClearStates(void) {
       iAxes++;
     }
     TrimAxes.clear();
-    cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
+    //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
 }
     
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -588,6 +539,8 @@ bool FGTrim::checkLimits(void) {
   return solutionExists;
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGTrim::setupPullup() {
   float g,q,cgamma;
   FGColumnVector3 vPQR;
@@ -601,7 +554,9 @@ void FGTrim::setupPullup() {
   cout << "setPitchRateInPullup() complete" << endl;
   
 }  
-  
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGTrim::setupTurn(void){
   double g,phi;
   phi = fgic->GetRollAngleRadIC();
@@ -610,24 +565,40 @@ void FGTrim::setupTurn(void){
     g = fdmex->GetInertial()->gravity(); 
     psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
     cout << targetNlf << ", " << psidot << endl;
-  }  
+  }
+   
 }  
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGTrim::updateRates(void){
-  double phi = fgic->GetRollAngleRadIC();
-  double g = fdmex->GetInertial()->gravity(); 
-  if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
-    double p,q,r,theta,phi;
-    theta=fgic->GetPitchAngleRadIC();
-    phi=fgic->GetRollAngleRadIC();
-    psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
-    p=-psidot*sin(theta);
-    q=psidot*cos(theta)*sin(phi);
-    r=psidot*cos(theta)*cos(phi);
+  if( mode == tTurn ) {
+    double phi = fgic->GetRollAngleRadIC();
+    double g = fdmex->GetInertial()->gravity(); 
+    double p,q,r,theta;
+    if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
+      theta=fgic->GetPitchAngleRadIC();
+      phi=fgic->GetRollAngleRadIC();
+      psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
+      p=-psidot*sin(theta);
+      q=psidot*cos(theta)*sin(phi);
+      r=psidot*cos(theta)*cos(phi);
+    } else {
+      p=q=r=0;
+    }      
     fdmex->GetRotation()->SetPQR(p,q,r);
-  }
+  } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
+      float g,q,cgamma;
+      FGColumnVector3 vPQR;
+      g=fdmex->GetInertial()->gravity();
+      cgamma=cos(fgic->GetFlightPathAngleRadIC());
+      q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
+      fdmex->GetRotation()->SetPQR(0,q,0);
+  }  
 }  
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGTrim::setDebug(void) {
   if(debug_axis == tAll ||
       TrimAxes[current_axis]->GetStateType() == debug_axis ) {
@@ -638,6 +609,60 @@ void FGTrim::setDebug(void) {
     return;
   }
 }      
-    
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGTrim::SetMode(TrimMode tt) {
+    ClearStates();
+    switch(tt) {
+      case tFull:
+        cout << "  Full Trim" << endl;
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
+        break;
+      case tLongitudinal:
+        cout << "  Longitudinal Trim" << endl;
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
+        break;
+      case tGround:
+        cout << "  Ground Trim" << endl;
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
+        //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
+        break;
+      case tPullup:
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
+        break;
+      case tTurn:
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
+        TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
+        break;
+      case tCustom:
+      case tNone:
+        break;
+    }
+    //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
+    sub_iterations=new double[TrimAxes.size()];
+    successful=new double[TrimAxes.size()];
+    solution=new bool[TrimAxes.size()];
+    current_axis=0;
+}    
 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.