Sync'ed with JSBSim: avoid the lag in <kinematic> while trimming. This allows the...

diff --git a/src/FDM/JSBSim/models/flight_control/FGKinemat.cpp b/src/FDM/JSBSim/models/flight_control/FGKinemat.cpp
index bd1270e8d0609494fbf63af5090a015c9927deff..05d41ab70463a27b4eab4a8586ef7e131f3eb3f2 100644 (file)
--- a/src/FDM/JSBSim/models/flight_control/FGKinemat.cpp
+++ b/src/FDM/JSBSim/models/flight_control/FGKinemat.cpp
@@ -39,6 +39,7 @@ INCLUDES
 
 #include "FGKinemat.h"
 #include "input_output/FGXMLElement.h"
+#include "models/FGFCS.h"
 #include <iostream>
 #include <cstdlib>
 
@@ -46,7 +47,7 @@ using namespace std;
 
 namespace JSBSim {
 
-IDENT(IdSrc,"$Id: FGKinemat.cpp,v 1.15 2015/04/02 17:39:28 bcoconni Exp $");
+IDENT(IdSrc,"$Id: FGKinemat.cpp,v 1.16 2016/06/12 14:47:46 bcoconni Exp $");
 IDENT(IdHdr,ID_FLAPS);
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -62,7 +63,7 @@ FGKinemat::FGKinemat(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element
   Detents.clear();
   TransitionTimes.clear();
 
-  Output = OutputPct = 0;
+  Output = 0;
   DoScale = true;
 
   if (element->FindElement("noscale")) DoScale = false;
@@ -76,16 +77,14 @@ FGKinemat::FGKinemat(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element
     TransitionTimes.push_back(tmpTime);
     setting_element = traverse_element->FindNextElement("setting");
   }
-  NumDetents = Detents.size();
 
-  if (NumDetents <= 1) {
+  if (Detents.size() <= 1) {
     cerr << "Kinematic component " << Name
          << " must have more than 1 setting element" << endl;
     exit(-1);
   }
 
   FGFCSComponent::bind();
-//  treenode->Tie("output-norm", this, &FGKinemat::GetOutputPct );
 
   Debug(0);
 }
@@ -105,58 +104,56 @@ bool FGKinemat::Run(void )
 
   Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
 
-  if (DoScale) Input *= Detents[NumDetents-1];
+  if (DoScale) Input *= Detents.back();
 
   if (IsOutput) Output = OutputNodes[0]->getDoubleValue();
 
-  if (Input < Detents[0])
-    Input = Detents[0];
-  else if (Detents[NumDetents-1] < Input)
-    Input = Detents[NumDetents-1];
-
-  // Process all detent intervals the movement traverses until either the
-  // final value is reached or the time interval has finished.
-  while ( dt0 > 0.0 && !EqualToRoundoff(Input, Output) ) {
-
-    // Find the area where Output is in
-    unsigned int ind;
-    for (ind = 1; (Input < Output) ? Detents[ind] < Output : Detents[ind] <= Output ; ++ind)
-      if (NumDetents <= ind)
+  Input = Constrain(Detents.front(), Input, Detents.back());
+
+  if (fcs->GetTrimStatus())
+    // When trimming the output must be reached in one step
+    Output = Input;
+  else {
+    // Process all detent intervals the movement traverses until either the
+    // final value is reached or the time interval has finished.
+    while ( dt0 > 0.0 && !EqualToRoundoff(Input, Output) ) {
+
+      // Find the area where Output is in
+      unsigned int ind;
+      for (ind = 1; (Input < Output) ? Detents[ind] < Output : Detents[ind] <= Output ; ++ind)
+        if (ind >= Detents.size())
+          break;
+
+      // A transition time of 0.0 means an infinite rate.
+      // The output is reached in one step
+      if (TransitionTimes[ind] <= 0.0) {
+        Output = Input;
         break;
-
-    // A transition time of 0.0 means an infinite rate.
-    // The output is reached in one step
-    if (TransitionTimes[ind] <= 0.0) {
-      Output = Input;
-      break;
-    } else {
-      // Compute the rate in this area
-      double Rate = (Detents[ind] - Detents[ind-1])/TransitionTimes[ind];
-      // Compute the maximum input value inside this area
-      double ThisInput = Input;
-      if (ThisInput < Detents[ind-1])   ThisInput = Detents[ind-1];
-      if (Detents[ind] < ThisInput)     ThisInput = Detents[ind];
-      // Compute the time to reach the value in ThisInput
-      double ThisDt = fabs((ThisInput-Output)/Rate);
-
-      // and clip to the timestep size
-      if (dt0 < ThisDt) {
-        ThisDt = dt0;
-        if (Output < Input)
-          Output += ThisDt*Rate;
-        else
-          Output -= ThisDt*Rate;
-      } else
-        // Handle this case separate to make shure the termination condition
-        // is met even in inexact arithmetics ...
-        Output = ThisInput;
-
-      dt0 -= ThisDt;
+      } else {
+        // Compute the rate in this area
+        double Rate = (Detents[ind] - Detents[ind-1])/TransitionTimes[ind];
+        // Compute the maximum input value inside this area
+        double ThisInput = Constrain(Detents[ind-1], Input, Detents[ind]);
+        // Compute the time to reach the value in ThisInput
+        double ThisDt = fabs((ThisInput-Output)/Rate);
+
+        // and clip to the timestep size
+        if (dt0 < ThisDt) {
+          ThisDt = dt0;
+          if (Output < Input)
+            Output += ThisDt*Rate;
+          else
+            Output -= ThisDt*Rate;
+        } else
+          // Handle this case separate to make shure the termination condition
+          // is met even in inexact arithmetics ...
+          Output = ThisInput;
+
+        dt0 -= ThisDt;
+      }
     }
   }
 
-  OutputPct = (Output-Detents[0])/(Detents[NumDetents-1]-Detents[0]);
-
   Clip();
   if (IsOutput) SetOutput();
 
@@ -189,8 +186,8 @@ void FGKinemat::Debug(int from)
   if (debug_lvl & 1) { // Standard console startup message output
     if (from == 0) { // Constructor
       cout << "      INPUT: " << InputNodes[0]->GetName() << endl;
-      cout << "      DETENTS: " << NumDetents << endl;
-      for (unsigned int i=0;i<NumDetents;i++) {
+      cout << "      DETENTS: " << Detents.size() << endl;
+      for (unsigned int i=0;i<Detents.size();i++) {
         cout << "        " << Detents[i] << " " << TransitionTimes[i] << endl;
       }
       if (IsOutput) {

diff --git a/src/FDM/JSBSim/models/flight_control/FGKinemat.h b/src/FDM/JSBSim/models/flight_control/FGKinemat.h
index 03b01ab475a3fc274b19ead98fd306299d3463a5..ff06bfbe9df34cdf1cf54d782d7ec5e6fcc4404b 100644 (file)
--- a/src/FDM/JSBSim/models/flight_control/FGKinemat.h
+++ b/src/FDM/JSBSim/models/flight_control/FGKinemat.h
@@ -44,7 +44,7 @@ INCLUDES
 DEFINITIONS
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
-#define ID_FLAPS "$Id: FGKinemat.h,v 1.11 2015/03/28 14:49:02 bcoconni Exp $"
+#define ID_FLAPS "$Id: FGKinemat.h,v 1.12 2016/06/12 14:47:46 bcoconni Exp $"
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 FORWARD DECLARATIONS
@@ -121,7 +121,8 @@ public:
 
   /** Kinematic component output value.
       @return the current output of the kinematic object on the range of [0,1]. */
-  double GetOutputPct() const { return OutputPct; }
+  double GetOutputPct() const
+  { return (Output-Detents[0])/(Detents.back()-Detents[0]); }
 
   /** Run method, overrides FGModel::Run().
       @return false on success, true on failure.
@@ -131,8 +132,6 @@ public:
 private:
   std::vector<double> Detents;
   std::vector<double> TransitionTimes;
-  size_t NumDetents;
-  double OutputPct;
   bool  DoScale;
 
   void Debug(int from);