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 f478ab6df125fe7cbed152250ca66409804db67c..e4a5c2ea658f412a824859a11780eec0637b8157 100644 (file)
--- a/src/FDM/JSBSim/FGTrim.cpp
+++ b/src/FDM/JSBSim/FGTrim.cpp
@@ -115,15 +115,17 @@ FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
     break;
   case tTurn:
-    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
+    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()];
@@ -312,14 +314,14 @@ bool FGTrim::DoTrim(void) {
     cout << "nlf done" << endl;
   } else if (mode == tTurn) {
     setupTurn();
-    TrimAxes[0]->SetStateTarget(targetNlf);
+    //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()) { 
@@ -595,30 +597,48 @@ void FGTrim::setupPullup() {
        << fgic->GetVtrueFpsIC() << endl;
   q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
   cout << targetNlf << ", " << q << endl;
-  vPQR.InitMatrix();
-  vPQR(2)=q;
-  cout << vPQR << endl;
-  fdmex->GetRotation()->SetPQR(vPQR);
+  fdmex->GetRotation()->SetPQR(0,q,0);
   cout << "setPitchRateInPullup() complete" << endl;
   
 }  
   
 void FGTrim::setupTurn(void){
-  FGColumnVector3 vPQR;
-  float g,q,r,phi, psidot;
+  double g,phi;
   phi = fgic->GetRollAngleRadIC();
-  if( fabs(phi) > 0.01 && fabs(phi) < 1.56 ) {
+  if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
     targetNlf = 1 / cos(phi);
     g = fdmex->GetInertial()->gravity(); 
-    psidot = g*tan(phi) / fgic->GetVtrueFpsIC();
-    q = psidot*sin(phi);
-    r = psidot*cos(phi);
-    vPQR(1)=0;vPQR(2)=q;vPQR(3)=r;
-    fdmex->GetRotation()->SetPQR(vPQR);
-    cout << targetNlf << ", " << vPQR*57.29577 << endl;
-  }  
+    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 ||