Initial revision.

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

diff --git a/src/FDM/JSBSim/FGTrimAxis.cpp b/src/FDM/JSBSim/FGTrimAxis.cpp
index 6bdfd66322596eaa439102e59bc1c7938bf4e1d8..2fe3213f168c2aee9fa0ae01502f7e495472627e 100644 (file)
--- a/src/FDM/JSBSim/FGTrimAxis.cpp
+++ b/src/FDM/JSBSim/FGTrimAxis.cpp
@@ -62,7 +62,8 @@ FGTrimAxis::FGTrimAxis(FGFDMExec* fdex, FGInitialCondition* ic, State st,
   state_convert=1.0;
   control_convert=1.0;
   state_value=0;
-    switch(state) {
+  state_target=0;
+  switch(state) {
     case tUdot: tolerance = DEFAULT_TOLERANCE; break;
     case tVdot: tolerance = DEFAULT_TOLERANCE; break;
     case tWdot: tolerance = DEFAULT_TOLERANCE; break;
@@ -70,6 +71,7 @@ FGTrimAxis::FGTrimAxis(FGFDMExec* fdex, FGInitialCondition* ic, State st,
     case tPdot: tolerance = DEFAULT_TOLERANCE / 10; break;
     case tRdot: tolerance = DEFAULT_TOLERANCE / 10; break;
     case tHmgt: tolerance = 0.01; break;
+    case  tNlf: state_target=1.0; tolerance = 1E-5; break;
   }  
   
   solver_eps=tolerance;
@@ -137,27 +139,28 @@ FGTrimAxis::FGTrimAxis(FGFDMExec* fdex, FGInitialCondition* ic, State st,
   }
   
   
-  if (debug_lvl & 2) cout << "Instantiated: FGTrimAxis" << endl;
+  Debug(0);
 }
 
 /*****************************************************************************/
 
-FGTrimAxis::~FGTrimAxis()
+FGTrimAxis::~FGTrimAxis(void)
 {
-  if (debug_lvl & 2) cout << "Destroyed:    FGTrimAxis" << endl;
+  Debug(1);
 }
 
 /*****************************************************************************/
 
 void FGTrimAxis::getState(void) {
   switch(state) {
-  case tUdot: state_value=fdmex->GetAircraft()->GetBodyAccel()(1); break;
-  case tVdot: state_value=fdmex->GetAircraft()->GetBodyAccel()(2); break;
-  case tWdot: state_value=fdmex->GetAircraft()->GetBodyAccel()(3); break;
-  case tQdot: state_value=fdmex->GetRotation()->GetPQRdot(2);break;
-  case tPdot: state_value=fdmex->GetRotation()->GetPQRdot(1); break;
-  case tRdot: state_value=fdmex->GetRotation()->GetPQRdot(3); break;
-  case tHmgt: state_value=computeHmgt(); break;
+  case tUdot: state_value=fdmex->GetTranslation()->GetUVWdot()(1)-state_target; break;
+  case tVdot: state_value=fdmex->GetTranslation()->GetUVWdot()(2)-state_target; break;
+  case tWdot: state_value=fdmex->GetTranslation()->GetUVWdot()(3)-state_target; break;
+  case tQdot: state_value=fdmex->GetRotation()->GetPQRdot(2)-state_target;break;
+  case tPdot: state_value=fdmex->GetRotation()->GetPQRdot(1)-state_target; break;
+  case tRdot: state_value=fdmex->GetRotation()->GetPQRdot(3)-state_target; break;
+  case tHmgt: state_value=computeHmgt()-state_target; break;
+  case tNlf:  state_value=fdmex->GetAircraft()->GetNlf()-state_target; break;
   }
 }
 
@@ -412,7 +415,7 @@ void FGTrimAxis::AxisReport(void) {
   char out[80];
   sprintf(out,"  %20s: %6.2f %5s: %9.2e Tolerance: %3.0e\n",
            GetControlName().c_str(), GetControl()*control_convert,
-           GetStateName().c_str(), GetState(), GetTolerance()); 
+           GetStateName().c_str(), GetState()+state_target, GetTolerance()); 
   cout << out;
 
 }
@@ -427,8 +430,42 @@ double FGTrimAxis::GetAvgStability( void ) {
 }
 
 /*****************************************************************************/
-
-void FGTrimAxis::Debug(void)
+//    The bitmasked value choices are as follows:
+//    unset: In this case (the default) JSBSim would only print
+//       out the normally expected messages, essentially echoing
+//       the config files as they are read. If the environment
+//       variable is not set, debug_lvl is set to 1 internally
+//    0: This requests JSBSim not to output any messages
+//       whatsoever.
+//    1: This value explicity requests the normal JSBSim
+//       startup messages
+//    2: This value asks for a message to be printed out when
+//       a class is instantiated
+//    4: When this value is set, a message is displayed when a
+//       FGModel object executes its Run() method
+//    8: When this value is set, various runtime state variables
+//       are printed out periodically
+//    16: When set various parameters are sanity checked and
+//       a message is printed out when they go out of bounds
+
+void FGTrimAxis::Debug(int from)
 {
+  if (debug_lvl <= 0) return;
+
+  if (debug_lvl & 1) { // Standard console startup message output
+    if (from == 0) { // Constructor
+
+    }
+  }
+  if (debug_lvl & 2 ) { // Instantiation/Destruction notification
+    if (from == 0) cout << "Instantiated: FGTrimAxis" << endl;
+    if (from == 1) cout << "Destroyed:    FGTrimAxis" << endl;
+  }
+  if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
+  }
+  if (debug_lvl & 8 ) { // Runtime state variables
+  }
+  if (debug_lvl & 16) { // Sanity checking
+  }
 }