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 52faac0fb2fc30c8e1b6619e82d4324ae7fb77b9..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.19 2011/06/18 13:30:27 bcoconni 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;
@@ -144,7 +160,7 @@ void FGActuator::Lag(void)
   // for this Lag filter
   double input = Output;
 
-  if (!fcs->GetTrimStatus())
+  if ( initialized )
     Output = ca * (input + PreviousLagInput) + PreviousLagOutput * cb;
 
   PreviousLagInput = input;
@@ -160,7 +176,7 @@ void FGActuator::Hysteresis(void)
   // method.
   double input = Output;
   
-  if (!fcs->GetTrimStatus()) {
+  if ( initialized ) {
     if (input > PreviousHystOutput)
       Output = max(PreviousHystOutput, input-0.5*hysteresis_width);
     else if (input < PreviousHystOutput)
@@ -178,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 (!fcs->GetTrimStatus()) {
-    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;
@@ -216,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);
 }
 
 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%