Add my old ultra-simplistic PI controller. The fancy PID controller doesn't

seem to be fully deterministic in P-only mode.  This old simple controller
does what I expect, so it's good for calulating stage #1's of multi-stage
controllers.

diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx
index e329d46b3dca5c624822ca2d0070065650f04ea4..88c0f99849d6bdf98dc1d500c71723c8970c9341 100644 (file)
--- a/src/Autopilot/xmlauto.cxx
+++ b/src/Autopilot/xmlauto.cxx
@@ -296,13 +296,13 @@ void FGPIDController::update( double dt ) {
 }
 
 
-FGSimplePIController::FGSimplePIController( SGPropertyNode *node ):
+FGPISimpleController::FGPISimpleController( SGPropertyNode *node ):
     proportional( false ),
-    factor( 0.0 ),
+    Kp( 0.0 ),
     offset_prop( NULL ),
     offset_value( 0.0 ),
     integral( false ),
-    gain( 0.0 ),
+    Ki( 0.0 ),
     int_sum( 0.0 ),
     clamp( false ),
     debug( false ),
@@ -318,6 +318,8 @@ FGSimplePIController::FGSimplePIController( SGPropertyNode *node ):
         string cval = child->getStringValue();
         if ( cname == "name" ) {
             name = cval;
+        } else if ( cname == "debug" ) {
+            debug = child->getBoolValue();
         } else if ( cname == "enable" ) {
             // cout << "parsing enable" << endl;
             SGPropertyNode *prop = child->getChild( "prop" );
@@ -331,8 +333,6 @@ FGSimplePIController::FGSimplePIController( SGPropertyNode *node ):
             if ( val != NULL ) {
                 enable_value = val->getStringValue();
             }
-        } else if ( cname == "debug" ) {
-            debug = child->getBoolValue();
         } else if ( cname == "input" ) {
             SGPropertyNode *prop = child->getChild( "prop" );
             if ( prop != NULL ) {
@@ -345,7 +345,7 @@ FGSimplePIController::FGSimplePIController( SGPropertyNode *node ):
             } else {
                 prop = child->getChild( "value" );
                 if ( prop != NULL ) {
-                    r_n_value = prop->getDoubleValue();
+                    r_n = prop->getDoubleValue();
                 }
             }
         } else if ( cname == "output" ) {
@@ -356,64 +356,43 @@ FGSimplePIController::FGSimplePIController( SGPropertyNode *node ):
                 output_list.push_back( tmp );
                 i++;
             }
-            prop = child->getChild( "clamp" );
-            if ( prop != NULL ) {
-                clamp = true;
-
-                SGPropertyNode *tmp;
-
-                tmp = prop->getChild( "min" );
-                if ( tmp != NULL ) {
-                    u_min = tmp->getDoubleValue();
-                    // cout << "min = " << u_min << endl;
-                }
+        } else if ( cname == "config" ) {
+            SGPropertyNode *prop;
 
-                tmp = prop->getChild( "max" );
-                if ( tmp != NULL ) {
-                    u_max = tmp->getDoubleValue();
-                    // cout << "max = " << u_max << endl;
-                }
+            prop = child->getChild( "Kp" );
+            if ( prop != NULL ) {
+                Kp = prop->getDoubleValue();
+                proportional = true;
             }
-        } else if ( cname == "proportional" ) {
-            proportional = true;
-
-            SGPropertyNode *prop;
 
-            prop = child->getChild( "factor" );
+            prop = child->getChild( "Ki" );
             if ( prop != NULL ) {
-                factor = prop->getDoubleValue();
+                Ki = prop->getDoubleValue();
+                integral = true;
             }
 
-            prop = child->getChild( "offset" );
+            prop = child->getChild( "u_min" );
             if ( prop != NULL ) {
-                SGPropertyNode *sub = prop->getChild( "prop" );
-                if ( sub != NULL ) {
-                    offset_prop = fgGetNode( sub->getStringValue(), true );
-                    // cout << "offset prop = " << sub->getStringValue() << endl;
-                } else {
-                    sub = prop->getChild( "value" );
-                    if ( sub != NULL ) {
-                        offset_value = sub->getDoubleValue();
-                        // cout << "offset value = " << offset_value << endl;
-                    }
-                }
+                u_min = prop->getDoubleValue();
+                clamp = true;
             }
-        } else if ( cname == "integral" ) {
-            integral = true;
 
-            SGPropertyNode *prop;
-            prop = child->getChild( "gain" );
+            prop = child->getChild( "u_max" );
             if ( prop != NULL ) {
-                gain = prop->getDoubleValue();
+                u_max = prop->getDoubleValue();
+                clamp = true;
             }
         } else {
             SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
+            if ( name.length() ) {
+                SG_LOG( SG_AUTOPILOT, SG_WARN, "Section = " << name );
+            }
         }
     }   
 }
 
 
-void FGSimplePIController::update( double dt ) {
+void FGPISimpleController::update( double dt ) {
     if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) {
         if ( !enabled ) {
             // we have just been enabled, zero out int_sum
@@ -454,11 +433,11 @@ void FGSimplePIController::update( double dt ) {
         }
 
         if ( proportional ) {
-            prop_comp = error * factor + offset;
+            prop_comp = error * Kp + offset;
         }
 
         if ( integral ) {
-            int_sum += error * gain * dt;
+            int_sum += error * Ki * dt;
         } else {
             int_sum = 0.0;
         }
@@ -554,6 +533,9 @@ bool FGXMLAutopilot::build() {
         if ( name == "pid-controller" ) {
             FGXMLAutoComponent *c = new FGPIDController( node );
             components.push_back( c );
+        } else if ( name == "pi-simple-controller" ) {
+            FGXMLAutoComponent *c = new FGPISimpleController( node );
+            components.push_back( c );
         } else {
             SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: " 
                     << name );

diff --git a/src/Autopilot/xmlauto.hxx b/src/Autopilot/xmlauto.hxx
index 92735d6a8721448955bd6043f9c02360573f0ec0..237678eb4b0ceebe9fd992c68c554aaefd1ca1a6 100644 (file)
--- a/src/Autopilot/xmlauto.hxx
+++ b/src/Autopilot/xmlauto.hxx
@@ -137,19 +137,19 @@ public:
  * A simplistic P [ + I ] PID controller
  */
 
-class FGSimplePIController : public FGXMLAutoComponent {
+class FGPISimpleController : public FGXMLAutoComponent {
 
 private:
 
     // proportional component data
     bool proportional;
-    double factor;
+    double Kp;
     SGPropertyNode *offset_prop;
     double offset_value;
 
     // integral component data
     bool integral;
-    double gain;
+    double Ki;
     double int_sum;
 
     // post functions for output
@@ -168,8 +168,8 @@ private:
     
 public:
 
-    FGSimplePIController( SGPropertyNode *node );
-    ~FGSimplePIController() {}
+    FGPISimpleController( SGPropertyNode *node );
+    ~FGPISimpleController() {}
 
     void update( double dt );
 };