sync with JSB JSBSim CVS

[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
index 7df31c67c466b71ea60a72b79528507f78e195eb..c541a5c5579e9a4f37b4bc3e04fe49b99411429b 100755 (executable)
--- a/src/FDM/JSBSim/models/flight_control/FGPID.cpp
+++ b/src/FDM/JSBSim/models/flight_control/FGPID.cpp
@@ -36,10 +36,15 @@ INCLUDES
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 
 #include "FGPID.h"
+#include "input_output/FGXMLElement.h"
+#include <string>
+#include <iostream>
+
+using namespace std;
 
 namespace JSBSim {
 
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGPID.cpp,v 1.20 2012/05/10 12:10:48 jberndt Exp $";
 static const char *IdHdr = ID_PID;
 
 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@@ -60,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");
@@ -76,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;
@@ -100,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"));
   }
@@ -121,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];
 
@@ -129,26 +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
-    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;
+  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
   
   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;
 
@@ -184,9 +239,9 @@ void FGPID::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: -" << InputNodes[0]->GetName() << endl;
       else
-        cout << "      INPUT: " << InputNodes[0]->getName() << endl;
+        cout << "      INPUT: " << InputNodes[0]->GetName() << endl;
 
       if (IsOutput) {
         for (unsigned int i=0; i<OutputNodes.size(); i++)