Updated to match changes in radiostack.[ch]xx

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

diff --git a/src/FDM/JSBSim/FGTrim.cpp b/src/FDM/JSBSim/FGTrim.cpp
index ebd8a10754230edbcbdd6c99e8593177ef1b0091..e4a5c2ea658f412a824859a11780eec0637b8157 100644 (file)
--- a/src/FDM/JSBSim/FGTrim.cpp
+++ b/src/FDM/JSBSim/FGTrim.cpp
@@ -53,6 +53,7 @@ INCLUDES
 #include "FGAircraft.h"
 #include "FGMassBalance.h"
 #include "FGAerodynamics.h"
+#include "FGColumnVector3.h"
 #if _MSC_VER
 #pragma warning (disable : 4786 4788)
 #endif
@@ -70,7 +71,7 @@ FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
   Tolerance=1E-3;
   A_Tolerance = Tolerance / 10;
   
-  Debug=0;
+  Debug=0;DebugLevel=0;
   fdmex=FDMExec;
   fgic=FGIC;
   total_its=0;
@@ -79,6 +80,8 @@ FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
   axis_count=0;
   mode=tt;
   xlo=xhi=alo=ahi;
+  targetNlf=1.0;
+  debug_axis=tAll;
   switch(mode) {
   case tFull:
     cout << "  Full Trim" << endl;
@@ -102,7 +105,27 @@ FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
     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()];
@@ -281,9 +304,24 @@ bool FGTrim::DoTrim(void) {
     successful[current_axis]=0;
     solution[current_axis]=false;
   }
+  
+  
+  if(mode == tPullup ) {
+    cout << "Setting pitch rate and nlf... " << endl;
+    setupPullup();
+    cout << "pitch rate done ... " << endl;
+    TrimAxes[0]->SetStateTarget(targetNlf);
+    cout << "nlf done" << endl;
+  } else if (mode == tTurn) {
+    setupTurn();
+    //TrimAxes[0]->SetStateTarget(targetNlf);
+  }  
+  
   do {
     axis_count=0;
     for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
+      setDebug();
+      updateRates();
       Nsub=0;
       if(!solution[current_axis]) {
         if(checkLimits()) { 
@@ -550,5 +588,68 @@ bool FGTrim::checkLimits(void) {
   return solutionExists;
 }
 
+void FGTrim::setupPullup() {
+  float g,q,cgamma;
+  FGColumnVector3 vPQR;
+  g=fdmex->GetInertial()->gravity();
+  cgamma=cos(fgic->GetFlightPathAngleRadIC());
+  cout << "setPitchRateInPullup():  " << g << ", " << cgamma << ", "
+       << fgic->GetVtrueFpsIC() << endl;
+  q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
+  cout << targetNlf << ", " << q << endl;
+  fdmex->GetRotation()->SetPQR(0,q,0);
+  cout << "setPitchRateInPullup() complete" << endl;
+  
+}  
+  
+void FGTrim::setupTurn(void){
+  double g,phi;
+  phi = fgic->GetRollAngleRadIC();
+  if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
+    targetNlf = 1 / cos(phi);
+    g = fdmex->GetInertial()->gravity(); 
+    psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
+    cout << targetNlf << ", " << psidot << endl;
+  }
+   
+}  
+
+void FGTrim::updateRates(void){
+  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 ) {
+    Debug=DebugLevel; 
+    return;
+  } else {
+    Debug=0;
+    return;
+  }
+}      
+    
 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.