Merge branch 'next' of gitorious.org:fg/flightgear into next

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

diff --git a/src/FDM/JSBSim/initialization/FGTrim.cpp b/src/FDM/JSBSim/initialization/FGTrim.cpp
index 854519c618ac820374d3779f0fd9cd530deb9583..6b0cd81c05f665e568067d08ed859a6b7c2682bc 100644 (file)
--- a/src/FDM/JSBSim/initialization/FGTrim.cpp
+++ b/src/FDM/JSBSim/initialization/FGTrim.cpp
@@ -23,12 +23,10 @@
  Further information about the GNU Lesser General Public License can also be found on
  the world wide web at http://www.gnu.org.
 
-
  HISTORY
 --------------------------------------------------------------------------------
 9/8/99   TP   Created
 
-
 FUNCTIONAL DESCRIPTION
 --------------------------------------------------------------------------------
 
@@ -43,26 +41,29 @@ scheme. */
 INCLUDES
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-#include <stdlib.h>
-
-#include <FGFDMExec.h>
-#include <models/FGAtmosphere.h>
-#include "FGInitialCondition.h"
+#include <cstdlib>
+#include <iomanip>
 #include "FGTrim.h"
-#include <models/FGAircraft.h>
-#include <models/FGMassBalance.h>
-#include <models/FGGroundReactions.h>
-#include <models/FGInertial.h>
-#include <models/FGAerodynamics.h>
-#include <math/FGColumnVector3.h>
+#include "models/FGAtmosphere.h"
+#include "FGInitialCondition.h"
+#include "models/FGAircraft.h"
+#include "models/FGMassBalance.h"
+#include "models/FGGroundReactions.h"
+#include "models/FGInertial.h"
+#include "models/FGAerodynamics.h"
+#include "models/FGPropulsion.h"
+#include "models/propulsion/FGEngine.h"
+#include "math/FGColumnVector3.h"
 
 #if _MSC_VER
 #pragma warning (disable : 4786 4788)
 #endif
 
+using namespace std;
+
 namespace JSBSim {
 
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGTrim.cpp,v 1.13 2010/04/23 17:23:40 dpculp Exp $";
 static const char *IdHdr = ID_TRIM;
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -80,7 +81,7 @@ FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
   fgic=fdmex->GetIC();
   total_its=0;
   trimudot=true;
-  gamma_fallback=true;
+  gamma_fallback=false;
   axis_count=0;
   mode=tt;
   xlo=xhi=alo=ahi=0.0;
@@ -105,21 +106,19 @@ FGTrim::~FGTrim(void) {
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 void FGTrim::TrimStats() {
-  char out[80];
   int run_sum=0;
   cout << endl << "  Trim Statistics: " << endl;
   cout << "    Total Iterations: " << total_its << endl;
-  if(total_its > 0) {
+  if( total_its > 0) {
     cout << "    Sub-iterations:" << endl;
-    for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
-      run_sum+=TrimAxes[current_axis]->GetRunCount();
-      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]/double(total_its),
-                  successful[current_axis],
-                  TrimAxes[current_axis]->GetAvgStability() );
-      cout << out;
+    for (current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
+      run_sum += TrimAxes[current_axis]->GetRunCount();
+      cout << "   " << setw(5) << TrimAxes[current_axis]->GetStateName().c_str()
+           << ": " << setprecision(3) << sub_iterations[current_axis]
+           << " average: " << setprecision(5) << sub_iterations[current_axis]/double(total_its)
+           << "  successful:  " << setprecision(3) << successful[current_axis]
+           << "  stability: " << setprecision(5) << TrimAxes[current_axis]->GetAvgStability()
+           << endl;
     }
     cout << "    Run Count: " << run_sum << endl;
   }
@@ -189,7 +188,7 @@ bool FGTrim::RemoveState( State state ) {
       ta=*iAxes;
       if( ta->GetStateType() == state ) {
         delete ta;
-        TrimAxes.erase(iAxes);
+        iAxes = TrimAxes.erase(iAxes);
         result=true;
         continue;
       }
@@ -242,6 +241,8 @@ bool FGTrim::DoTrim(void) {
 
   fdmex->DisableOutput();
 
+  setEngineTrimMode(true);
+
   fgic->SetPRadpsIC(0.0);
   fgic->SetQRadpsIC(0.0);
   fgic->SetRRadpsIC(0.0);
@@ -357,6 +358,7 @@ bool FGTrim::DoTrim(void) {
   for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
     fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
   }
+  setEngineTrimMode(false);
   fdmex->EnableOutput();
   return !trim_failed;
 }
@@ -623,6 +625,15 @@ void FGTrim::setDebug(void) {
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
+void FGTrim::setEngineTrimMode(bool mode) {
+  FGPropulsion* prop = fdmex->GetPropulsion();
+  for (unsigned int i=0; i<prop->GetNumEngines(); i++) {
+    prop->GetEngine(i)->SetTrimMode(mode);
+  }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
 void FGTrim::SetMode(TrimMode tt) {
     ClearStates();
     mode=tt;
@@ -633,7 +644,7 @@ void FGTrim::SetMode(TrimMode tt) {
         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,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 ));