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 c3adc62c28004030f98374dccacb1dd5ee3bf69e..e4a5c2ea658f412a824859a11780eec0637b8157 100644 (file)
--- a/src/FDM/JSBSim/FGTrim.cpp
+++ b/src/FDM/JSBSim/FGTrim.cpp
@@ -610,22 +610,34 @@ 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) {