Fix stall widths for the "auxilliary" (reverse flow) stalls so they

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

diff --git a/src/FDM/JSBSim/FGTrim.cpp b/src/FDM/JSBSim/FGTrim.cpp
index 21dae465da7fc1f369c27f980858bca47d040159..a4921e277a72fa07260f1a20de4656a0e44782d1 100644 (file)
--- a/src/FDM/JSBSim/FGTrim.cpp
+++ b/src/FDM/JSBSim/FGTrim.cpp
@@ -52,12 +52,17 @@ INCLUDES
 #include "FGTrim.h"
 #include "FGAircraft.h"
 #include "FGMassBalance.h"
+#include "FGGroundReactions.h"
+#include "FGInertial.h"
 #include "FGAerodynamics.h"
 #include "FGColumnVector3.h"
+
 #if _MSC_VER
 #pragma warning (disable : 4786 4788)
 #endif
 
+namespace JSBSim {
+
 static const char *IdSrc = "$Id$";
 static const char *IdHdr = ID_TRIM;
 
@@ -237,14 +242,19 @@ bool FGTrim::DoTrim(void) {
   }
 
   fdmex->GetOutput()->Disable();
+  
+  fgic->SetPRadpsIC(0.0);
+  fgic->SetQRadpsIC(0.0);
+  fgic->SetRRadpsIC(0.0);
 
   //clear the sub iterations counts & zero out the controls
   for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
     //cout << current_axis << "  " << TrimAxes[current_axis]->GetStateName()
     //<< "  " << TrimAxes[current_axis]->GetControlName()<< endl;
     if(TrimAxes[current_axis]->GetStateType() == tQdot) {
-      if(mode == tGround) 
+      if(mode == tGround) {
          TrimAxes[current_axis]->initTheta();
+      }  
     }  
     xlo=TrimAxes[current_axis]->GetControlMin();
     xhi=TrimAxes[current_axis]->GetControlMax();
@@ -338,10 +348,12 @@ bool FGTrim::DoTrim(void) {
   } while((axis_count < TrimAxes.size()) && (!trim_failed));
   if((!trim_failed) && (axis_count >= TrimAxes.size())) {
     total_its=N;
-    cout << endl << "  Trim successful" << endl;
+    if (debug_lvl > 0)
+        cout << endl << "  Trim successful" << endl;
   } else {
     total_its=N;
-    cout << endl << "  Trim failed" << endl;
+    if (debug_lvl > 0)
+        cout << endl << "  Trim failed" << endl;
   }
   for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
     fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
@@ -542,15 +554,14 @@ bool FGTrim::checkLimits(void) {
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGTrim::setupPullup() {
-  float g,q,cgamma;
-  FGColumnVector3 vPQR;
+  double g,q,cgamma;
   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);
+  fgic->SetQRadpsIC(q);
   cout << "setPitchRateInPullup() complete" << endl;
   
 }  
@@ -586,14 +597,15 @@ void FGTrim::updateRates(void){
     } else {
       p=q=r=0;
     }      
-    fdmex->GetRotation()->SetPQR(p,q,r);
+    fgic->SetPRadpsIC(p);
+    fgic->SetQRadpsIC(q);
+    fgic->SetRRadpsIC(r);
   } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
-      float g,q,cgamma;
-      FGColumnVector3 vPQR;
+      double g,q,cgamma;
       g=fdmex->GetInertial()->gravity();
       cgamma=cos(fgic->GetFlightPathAngleRadIC());
       q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
-      fdmex->GetRotation()->SetPQR(0,q,0);
+      fgic->SetQRadpsIC(q);
   }  
 }  
 
@@ -614,9 +626,11 @@ void FGTrim::setDebug(void) {
 
 void FGTrim::SetMode(TrimMode tt) {
     ClearStates();
+    mode=tt;
     switch(tt) {
       case tFull:
-        cout << "  Full Trim" << endl;
+        if (debug_lvl > 0)          
+          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 ));
@@ -626,13 +640,15 @@ void FGTrim::SetMode(TrimMode tt) {
         TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
         break;
       case tLongitudinal:
-        cout << "  Longitudinal Trim" << endl;
+        if (debug_lvl > 0)          
+          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;
+        if (debug_lvl > 0)          
+          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 ));
@@ -665,4 +681,4 @@ void FGTrim::SetMode(TrimMode tt) {
     current_axis=0;
 }    
 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.
-
+}