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

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

diff --git a/src/FDM/JSBSim/models/flight_control/FGPID.cpp b/src/FDM/JSBSim/models/flight_control/FGPID.cpp
old mode 100755 (executable)
new mode 100644 (file)
index da6e6c1..c541a5c
--- a/src/FDM/JSBSim/models/flight_control/FGPID.cpp
+++ b/src/FDM/JSBSim/models/flight_control/FGPID.cpp
@@ -44,7 +44,7 @@ using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id: FGPID.cpp,v 1.19 2011/05/05 11:44:11 jberndt Exp $";
+static const char *IdSrc = "$Id: FGPID.cpp,v 1.20 2012/05/10 12:10:48 jberndt Exp $";
 static const char *IdHdr = ID_PID;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -65,6 +65,13 @@ FGPID::FGPID(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
   I_out_total = 0.0;
   Input_prev = Input_prev2 = 0.0;
   Trigger = 0;
+  ProcessVariableDot = 0;
+  IsStandard = false;
+  IntType = eNone;       // No integrator initially defined.
+
+  string pid_type = element->GetAttributeValue("type");
+
+  if (pid_type == "standard") IsStandard = true;
 
   if ( element->FindElement("kp") ) {
     kp_string = element->FindElementValue("kp");
@@ -81,6 +88,20 @@ FGPID::FGPID(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
 
   if ( element->FindElement("ki") ) {
     ki_string = element->FindElementValue("ki");
+
+    string integ_type = element->FindElement("ki")->GetAttributeValue("type");
+    if (integ_type == "rect") {            // Use rectangular integration
+      IntType = eRectEuler;
+    } else if (integ_type == "trap") {     // Use trapezoidal integration
+      IntType = eTrapezoidal;
+    } else if (integ_type == "ab2") {      // Use Adams Bashforth 2nd order integration
+      IntType = eAdamsBashforth2;
+    } else if (integ_type == "ab3") {      // Use Adams Bashforth 3rd order integration
+      IntType = eAdamsBashforth3;
+    } else {                               // Use default Adams Bashforth 2nd order integration
+      IntType = eAdamsBashforth2;
+    }
+
     if (!is_number(ki_string)) { // property
       if (ki_string[0] == '-') {
        KiPropertySign = -1.0;
@@ -105,6 +126,10 @@ FGPID::FGPID(FGFCS* fcs, Element* element) : FGFCSComponent(fcs, element)
     }
   }
 
+  if (element->FindElement("pvdot")) {
+    ProcessVariableDot =  PropertyManager->GetNode(element->FindElementValue("pvdot"));
+  }
+
   if (element->FindElement("trigger")) {
     Trigger =  PropertyManager->GetNode(element->FindElementValue("trigger"));
   }
@@ -126,7 +151,7 @@ FGPID::~FGPID()
 bool FGPID::Run(void )
 {
   double I_out_delta = 0.0;
-  double P_out, D_out;
+  double Dval = 0;
 
   Input = InputNodes[0]->getDoubleValue() * InputSigns[0];
 
@@ -134,30 +159,51 @@ bool FGPID::Run(void )
   if (KiPropertyNode != 0) Ki = KiPropertyNode->getDoubleValue() * KiPropertySign;
   if (KdPropertyNode != 0) Kd = KdPropertyNode->getDoubleValue() * KdPropertySign;
 
-  P_out = Kp * Input;
-  D_out = (Kd / dt) * (Input - Input_prev);
+  if (ProcessVariableDot) {
+    Dval = ProcessVariableDot->getDoubleValue();
+  } else {
+    Dval = (Input - Input_prev)/dt;
+  }
 
   // Do not continue to integrate the input to the integrator if a wind-up
   // condition is sensed - that is, if the property pointed to by the trigger
   // element is non-zero. Reset the integrator to 0.0 if the Trigger value
   // is negative.
 
-  if (Trigger != 0) {
-    double test = Trigger->getDoubleValue();
-    if (fabs(test) < 0.000001) {
-      // I_out_delta = Ki * dt * Input;                         // Normal rectangular integrator
+  double test = 0.0;
+  if (Trigger != 0) test = Trigger->getDoubleValue();
+
+  if (fabs(test) < 0.000001) {
+    switch(IntType) {
+    case eRectEuler:
+      I_out_delta = Ki * dt * Input;                         // Normal rectangular integrator
+      break;
+    case eTrapezoidal:
+      I_out_delta = (Ki/2.0) * dt * (Input + Input_prev);    // Trapezoidal integrator
+      break;
+    case eAdamsBashforth2:
       I_out_delta = Ki * dt * (1.5*Input - 0.5*Input_prev);  // 2nd order Adams Bashforth integrator
+      break;
+    case eAdamsBashforth3:                                   // 3rd order Adams Bashforth integrator
+      I_out_delta = (Ki/12.0) * dt * (23.0*Input - 16.0*Input_prev + 5.0*Input_prev2);
+      break;
+    case eNone:
+      // No integator is defined or used.
+      I_out_delta = 0.0;
+      break;
     }
-    if (test < 0.0)            I_out_total = 0.0;  // Reset integrator to 0.0
-  } else { // no anti-wind-up trigger defined
-    // I_out_delta = Ki * dt * Input;
-    I_out_delta = Ki * dt * (1.5*Input - 0.5*Input_prev);  // 2nd order Adams Bashforth integrator
   }
+
+  if (test < 0.0) I_out_total = 0.0;  // Reset integrator to 0.0
   
   I_out_total += I_out_delta;
 
-  Output = P_out + I_out_total + D_out;
-  
+  if (IsStandard) {
+    Output = Kp * (Input + I_out_total + Kd*Dval);
+  } else {
+    Output = Kp*Input + I_out_total + Kd*Dval;
+  }
+
   Input_prev = Input;
   Input_prev2 = Input_prev;