]> git.mxchange.org Git - flightgear.git/blobdiff - src/FDM/JSBSim/FGTrim.cpp
projects / flightgear.git / blobdiff
? search:
summary | shortlog | log | commit | commitdiff | tree
raw | inline | side by side
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 a5fc75d9eceb876419d4b5e2671292a3a6bc5d70..a4921e277a72fa07260f1a20de4656a0e44782d1 100644 (file)
--- a/src/FDM/JSBSim/FGTrim.cpp
+++ b/src/FDM/JSBSim/FGTrim.cpp
@@ -52,18 +52,23 @@ 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;
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
-FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
+FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
 
   N=Nsub=0;
   max_iterations=60;
@@ -73,7 +78,7 @@ FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
   
   Debug=0;DebugLevel=0;
   fdmex=FDMExec;
-  fgic=FGIC;
+  fgic=fdmex->GetIC();
   total_its=0;
   trimudot=true;
   gamma_fallback=true;
@@ -82,56 +87,7 @@ FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
   xlo=xhi=alo=ahi=0.0;
   targetNlf=1.0;
   debug_axis=tAll;
-  switch(mode) {
-  case tFull:
-    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 ));
-    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 tLongitudinal:
-    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;
-    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 ));
-    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()];
-  solution=new bool[TrimAxes.size()];
-  current_axis=0;
-  
+  SetMode(tt);
   if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
 }
 
@@ -193,7 +149,7 @@ void FGTrim::ClearStates(void) {
       iAxes++;
     }
     TrimAxes.clear();
-    cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
+    //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
 }
     
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -286,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();
@@ -387,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);
@@ -588,20 +551,23 @@ bool FGTrim::checkLimits(void) {
   return solutionExists;
 }
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 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;
   
 }  
-  
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGTrim::setupTurn(void){
   double g,phi;
   phi = fgic->GetRollAngleRadIC();
@@ -614,6 +580,8 @@ void FGTrim::setupTurn(void){
    
 }  
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGTrim::updateRates(void){
   if( mode == tTurn ) {
     double phi = fgic->GetRollAngleRadIC();
@@ -629,17 +597,20 @@ 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);
   }  
 }  
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGTrim::setDebug(void) {
   if(debug_axis == tAll ||
       TrimAxes[current_axis]->GetStateType() == debug_axis ) {
@@ -650,6 +621,64 @@ void FGTrim::setDebug(void) {
     return;
   }
 }      
-    
-//YOU WERE WARNED, BUT YOU DID IT ANYWAY.
 
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGTrim::SetMode(TrimMode tt) {
+    ClearStates();
+    mode=tt;
+    switch(tt) {
+      case tFull:
+        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 ));
+        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 tLongitudinal:
+        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:
+        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 ));
+        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()];
+    solution=new bool[TrimAxes.size()];
+    current_axis=0;
+}    
+//YOU WERE WARNED, BUT YOU DID IT ANYWAY.
+}
Personal clone of the FlightGear source code, the free-libre OpenSource flight simulator.