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 ccde8c51983203fb59da132b4c773d69db39805a..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,10 +105,30 @@ 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 float[TrimAxes.size()];
-  successful=new float[TrimAxes.size()];
+  sub_iterations=new double[TrimAxes.size()];
+  successful=new double[TrimAxes.size()];
   solution=new bool[TrimAxes.size()];
   current_axis=0;
   
@@ -138,7 +161,7 @@ void FGTrim::TrimStats() {
       snprintf(out,80,"   %5s: %3.0f average: %5.2f  successful: %3.0f  stability: %5.2f\n",
                   TrimAxes[current_axis]->GetStateName().c_str(),
                   sub_iterations[current_axis],
-                  sub_iterations[current_axis]/float(total_its),
+                  sub_iterations[current_axis]/double(total_its),
                   successful[current_axis],
                   TrimAxes[current_axis]->GetAvgStability() );
       cout << out;
@@ -192,8 +215,8 @@ bool FGTrim::AddState( State state, Control control ) {
     delete[] sub_iterations;
     delete[] successful;
     delete[] solution;
-    sub_iterations=new float[TrimAxes.size()];
-    successful=new float[TrimAxes.size()];
+    sub_iterations=new double[TrimAxes.size()];
+    successful=new double[TrimAxes.size()];
     solution=new bool[TrimAxes.size()];
   }
   return result;
@@ -221,8 +244,8 @@ bool FGTrim::RemoveState( State state ) {
     delete[] sub_iterations;
     delete[] successful;
     delete[] solution;
-    sub_iterations=new float[TrimAxes.size()];
-    successful=new float[TrimAxes.size()];
+    sub_iterations=new double[TrimAxes.size()];
+    successful=new double[TrimAxes.size()];
     solution=new bool[TrimAxes.size()];
   }  
   return result;
@@ -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()) { 
@@ -365,9 +403,9 @@ bool FGTrim::DoTrim(void) {
 
 bool FGTrim::solve(void) {
 
-  float x1,x2,x3,f1,f2,f3,d,d0;
-  const float relax =0.9;
-  float eps=TrimAxes[current_axis]->GetSolverEps();
+  double x1,x2,x3,f1,f2,f3,d,d0;
+  const double relax =0.9;
+  double eps=TrimAxes[current_axis]->GetSolverEps();
 
   x1=x2=x3=0;
   d=1;
@@ -446,12 +484,12 @@ bool FGTrim::solve(void) {
 */
 bool FGTrim::findInterval(void) {
   bool found=false;
-  float step;
-  float current_control=TrimAxes[current_axis]->GetControl();
-  float current_accel=TrimAxes[current_axis]->GetState();;
-  float xmin=TrimAxes[current_axis]->GetControlMin();
-  float xmax=TrimAxes[current_axis]->GetControlMax();
-  float lastxlo,lastxhi,lastalo,lastahi;
+  double step;
+  double current_control=TrimAxes[current_axis]->GetControl();
+  double current_accel=TrimAxes[current_axis]->GetState();;
+  double xmin=TrimAxes[current_axis]->GetControlMin();
+  double xmax=TrimAxes[current_axis]->GetControlMax();
+  double lastxlo,lastxhi,lastalo,lastahi;
   
   step=0.025*fabs(xmax);
   xlo=xhi=current_control;
@@ -516,8 +554,8 @@ bool FGTrim::findInterval(void) {
 
 bool FGTrim::checkLimits(void) {
   bool solutionExists;
-  float current_control=TrimAxes[current_axis]->GetControl();
-  float current_accel=TrimAxes[current_axis]->GetState();
+  double current_control=TrimAxes[current_axis]->GetControl();
+  double current_accel=TrimAxes[current_axis]->GetState();
   xlo=TrimAxes[current_axis]->GetControlMin();
   xhi=TrimAxes[current_axis]->GetControlMax();
 
@@ -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.