sync with JSB JSBSim CVS

[flightgear.git] / src / FDM / JSBSim / models / flight_control / FGActuator.cpp

diff --git a/src/FDM/JSBSim/models/flight_control/FGActuator.cpp b/src/FDM/JSBSim/models/flight_control/FGActuator.cpp
index 4c58bebfd56603420861464450465fa87fc6891b..8f65597a1f0c3a926da14f4d568df5ab3fce0882 100644 (file)
--- a/src/FDM/JSBSim/models/flight_control/FGActuator.cpp
+++ b/src/FDM/JSBSim/models/flight_control/FGActuator.cpp
@@ -43,7 +43,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGActuator.cpp,v 1.14 2009/10/24 22:59:30 jberndt Exp $";
+static const char *IdSrc = "$Id: FGActuator.cpp,v 1.23 2012/04/08 15:04:41 jberndt Exp $";
 static const char *IdHdr = ID_ACTUATOR;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -65,6 +65,8 @@ FGActuator::FGActuator(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, eleme
   rate_limit = 0.0; // no limit
   fail_zero = fail_hardover = fail_stuck = false;
   ca = cb = 0.0;
+  initialized = 0;
+  saturated = false;
 
   if ( element->FindElement("deadband_width") ) {
     deadband_width = element->FindElementValueAsNumber("deadband_width");
@@ -73,7 +75,7 @@ FGActuator::FGActuator(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, eleme
     hysteresis_width = element->FindElementValueAsNumber("hysteresis_width");
   }
   if ( element->FindElement("rate_limit") ) {
-    rate_limit = element->FindElementValueAsNumber("rate_limit");
+    rate_limit = fabs(element->FindElementValueAsNumber("rate_limit"));
   }
   if ( element->FindElement("bias") ) {
     bias = element->FindElementValueAsNumber("bias");
@@ -104,6 +106,8 @@ bool FGActuator::Run(void )
 {
   Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
 
+  if( fcs->GetTrimStatus() ) initialized = 0;
+
   if (fail_zero) Input = 0;
   if (fail_hardover) Input =  clipmax*sign(Input);
 
@@ -114,16 +118,28 @@ bool FGActuator::Run(void )
                   // the Input will be further processed and the eventual Output
                   // will be overwritten from this perfect value.
 
-  if (lag != 0.0)              Lag();        // models actuator lag
-  if (rate_limit != 0)         RateLimit();  // limit the actuator rate
-  if (deadband_width != 0.0)   Deadband();
-  if (hysteresis_width != 0.0) Hysteresis();
-  if (bias != 0.0)             Bias();       // models a finite bias
+  if (fail_stuck) {
+    Output = PreviousOutput;
+  } else {
+    if (lag != 0.0)              Lag();        // models actuator lag
+    if (rate_limit != 0)         RateLimit();  // limit the actuator rate
+    if (deadband_width != 0.0)   Deadband();
+    if (hysteresis_width != 0.0) Hysteresis();
+    if (bias != 0.0)             Bias();       // models a finite bias
+  }
 
-  if (fail_stuck) Output = PreviousOutput;
   PreviousOutput = Output; // previous value needed for "stuck" malfunction
+  
+  initialized = 1;
 
   Clip();
+
+  if (clip) {
+    saturated = false;
+    if (Output >= clipmax) saturated = true;
+    else if (Output <= clipmin) saturated = true;
+  }
+
   if (IsOutput) SetOutput();
 
   return true;
@@ -143,7 +159,10 @@ void FGActuator::Lag(void)
   // "Output" on the right side of the "=" is the current frame input
   // for this Lag filter
   double input = Output;
-  Output = ca * (input + PreviousLagInput) + PreviousLagOutput * cb;
+
+  if ( initialized )
+    Output = ca * (input + PreviousLagInput) + PreviousLagOutput * cb;
+
   PreviousLagInput = input;
   PreviousLagOutput = Output;
 }
@@ -157,10 +176,11 @@ void FGActuator::Hysteresis(void)
   // method.
   double input = Output;
   
-  if (input > PreviousHystOutput) {
-    Output = max(PreviousHystOutput, input-0.5*hysteresis_width);
-  } else if (input < PreviousHystOutput) {
-    Output = min(PreviousHystOutput, input+0.5*hysteresis_width);
+  if ( initialized ) {
+    if (input > PreviousHystOutput)
+      Output = max(PreviousHystOutput, input-0.5*hysteresis_width);
+    else if (input < PreviousHystOutput)
+      Output = min(PreviousHystOutput, input+0.5*hysteresis_width);
   }
 
   PreviousHystOutput = Output;
@@ -174,10 +194,11 @@ void FGActuator::RateLimit(void)
   // is - for the purposes of this RateLimit method - really the input to the
   // method.
   double input = Output;
-  if (dt > 0.0) {
-    double rate = (input - PreviousRateLimOutput)/dt;
-    if (fabs(rate) > rate_limit) {
-      Output = PreviousRateLimOutput + (rate_limit*fabs(rate)/rate)*dt;
+  if ( initialized ) {
+    double delta = input - PreviousRateLimOutput;
+    if (fabs(delta) > dt * rate_limit) {
+      double signed_rate_limit = delta > 0.0 ? rate_limit : -rate_limit;
+      Output = PreviousRateLimOutput + signed_rate_limit * dt;
     }
   }
   PreviousRateLimOutput = Output;
@@ -187,6 +208,18 @@ void FGActuator::RateLimit(void)
 
 void FGActuator::Deadband(void)
 {
+  // Note: this function acts cumulatively on the "Output" parameter. So, "Output"
+  // is - for the purposes of this Deadband method - really the input to the
+  // method.
+  double input = Output;
+
+  if (input < -deadband_width/2.0) {
+    Output = (input + deadband_width/2.0);
+  } else if (input > deadband_width/2.0) {
+    Output = (input - deadband_width/2.0);
+  } else {
+    Output = 0.0;
+  }
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -200,10 +233,12 @@ void FGActuator::bind(void)
   const string tmp_zero = tmp + "/malfunction/fail_zero";
   const string tmp_hardover = tmp + "/malfunction/fail_hardover";
   const string tmp_stuck = tmp + "/malfunction/fail_stuck";
+  const string tmp_sat = tmp + "/saturated";
 
   PropertyManager->Tie( tmp_zero, this, &FGActuator::GetFailZero, &FGActuator::SetFailZero);
   PropertyManager->Tie( tmp_hardover, this, &FGActuator::GetFailHardover, &FGActuator::SetFailHardover);
   PropertyManager->Tie( tmp_stuck, this, &FGActuator::GetFailStuck, &FGActuator::SetFailStuck);
+  PropertyManager->Tie( tmp_sat, this, &FGActuator::IsSaturated);
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -232,9 +267,9 @@ void FGActuator::Debug(int from)
   if (debug_lvl & 1) { // Standard console startup message output
     if (from == 0) { // Constructor
       if (InputSigns[0] < 0)
-        cout << "      INPUT: -" << InputNodes[0]->getName() << endl;
+        cout << "      INPUT: -" << InputNames[0] << endl;
       else
-        cout << "      INPUT: " << InputNodes[0]->getName() << endl;
+        cout << "      INPUT: " << InputNames[0] << endl;
 
       if (IsOutput) {
         for (unsigned int i=0; i<OutputNodes.size(); i++)