Cleaned up some left over stuff. Working towards infrastructure to support

adding additional PID type algorithms to the code.
Add support for calculating heading bug error relative to magnetic heading
for slaved DG's.

diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx
index 8edc11c60d767408e1b1d3c52a1dab78d9429fc7..bf37d442c4d3172c1e78b48b83f9396ac3dc6a2c 100644 (file)
--- a/src/Autopilot/xmlauto.cxx
+++ b/src/Autopilot/xmlauto.cxx
@@ -30,152 +30,7 @@
 #include "xmlauto.hxx"
 
 
-FGPIDController::FGPIDController( SGPropertyNode *node, bool old ):
-    proportional( false ),
-    factor( 0.0 ),
-    offset_prop( NULL ),
-    offset_value( 0.0 ),
-    integral( false ),
-    gain( 0.0 ),
-    int_sum( 0.0 ),
-    one_eighty( false ),
-    clamp( false ),
-    debug( false ),
-    y_n( 0.0 ),
-    r_n( 0.0 ),
-    Kp( 0.0 ),
-    alpha( 0.1 ),
-    beta( 1.0 ),
-    gamma( 0.0 ),
-    Ti( 0.0 ),
-    Td( 0.0 ),
-    u_min( 0.0 ),
-    u_max( 0.0 ),
-    ep_n_1( 0.0 ),
-    edf_n_1( 0.0 ),
-    edf_n_2( 0.0 ),
-    u_n_1( 0.0 )
-{
-    int i;
-    for ( i = 0; i < node->nChildren(); ++i ) {
-        SGPropertyNode *child = node->getChild(i);
-        string cname = child->getName();
-        string cval = child->getStringValue();
-        if ( cname == "name" ) {
-            name = cval;
-        } else if ( cname == "enable" ) {
-            // cout << "parsing enable" << endl;
-            SGPropertyNode *prop = child->getChild( "prop" );
-            if ( prop != NULL ) {
-                // cout << "prop = " << prop->getStringValue() << endl;
-                enable_prop = fgGetNode( prop->getStringValue(), true );
-            } else {
-                // cout << "no prop child" << endl;
-            }
-            SGPropertyNode *val = child->getChild( "value" );
-            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 ) {
-                input_prop = fgGetNode( prop->getStringValue(), true );
-            }
-        } else if ( cname == "reference" ) {
-            SGPropertyNode *prop = child->getChild( "prop" );
-            if ( prop != NULL ) {
-                r_n_prop = fgGetNode( prop->getStringValue(), true );
-            } else {
-                prop = child->getChild( "value" );
-                if ( prop != NULL ) {
-                    r_n_value = prop->getDoubleValue();
-                }
-            }
-        } else if ( cname == "output" ) {
-            int i = 0;
-            SGPropertyNode *prop;
-            while ( (prop = child->getChild("prop", i)) != NULL ) {
-                SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
-                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;
-                }
-
-                tmp = prop->getChild( "max" );
-                if ( tmp != NULL ) {
-                    u_max = tmp->getDoubleValue();
-                    // cout << "max = " << u_max << endl;
-                }
-            }
-        } else if ( cname == "proportional" ) {
-            proportional = true;
-
-            SGPropertyNode *prop;
-
-            prop = child->getChild( "pre" );
-            if ( prop != NULL ) {
-                prop = prop->getChild( "one-eighty" );
-                if ( prop != NULL && prop->getBoolValue() ) {
-                    one_eighty = true;
-                }
-            }
-
-            prop = child->getChild( "factor" );
-            if ( prop != NULL ) {
-                factor = prop->getDoubleValue();
-            }
-
-            prop = child->getChild( "offset" );
-            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;
-                    }
-                }
-            }
-        } else if ( cname == "integral" ) {
-            integral = true;
-
-            SGPropertyNode *prop;
-            prop = child->getChild( "gain" );
-            if ( prop != NULL ) {
-                gain = prop->getDoubleValue();
-            }
-        } else {
-            SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
-        }
-    }   
-}
-
-
 FGPIDController::FGPIDController( SGPropertyNode *node ):
-    proportional( false ),
-    factor( 0.0 ),
-    offset_prop( NULL ),
-    offset_value( 0.0 ),
-    integral( false ),
-    gain( 0.0 ),
-    int_sum( 0.0 ),
-    one_eighty( false ),
-    clamp( false ),
     debug( false ),
     y_n( 0.0 ),
     r_n( 0.0 ),
@@ -289,83 +144,6 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
 }
 
 
-void FGPIDController::update_old( double dt ) {
-    if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) {
-        if ( !enabled ) {
-            // we have just been enabled, zero out int_sum
-            int_sum = 0.0;
-        }
-        enabled = true;
-    } else {
-        enabled = false;
-    }
-
-    if ( enabled ) {
-        if ( debug ) cout << "Updating " << name << endl;
-        double input = 0.0;
-        if ( input_prop != NULL ) {
-            input = input_prop->getDoubleValue();
-        }
-
-        double r_n = 0.0;
-        if ( r_n_prop != NULL ) {
-            r_n = r_n_prop->getDoubleValue();
-        } else {
-            r_n = r_n_value;
-        }
-                      
-        double error = r_n - input;
-        if ( one_eighty ) {
-            while ( error < -180.0 ) { error += 360.0; }
-            while ( error > 180.0 ) { error -= 360.0; }
-        }
-        if ( debug ) cout << "input = " << input
-                          << " reference = " << r_n
-                          << " error = " << error
-                          << endl;
-
-        double prop_comp = 0.0;
-        double offset = 0.0;
-        if ( offset_prop != NULL ) {
-            offset = offset_prop->getDoubleValue();
-            if ( debug ) cout << "offset = " << offset << endl;
-        } else {
-            offset = offset_value;
-        }
-
-        if ( proportional ) {
-            prop_comp = error * factor + offset;
-        }
-
-        if ( integral ) {
-            int_sum += error * gain * dt;
-        } else {
-            int_sum = 0.0;
-        }
-
-        if ( debug ) cout << "prop_comp = " << prop_comp
-                          << " int_sum = " << int_sum << endl;
-
-        double output = prop_comp + int_sum;
-
-        if ( clamp ) {
-            if ( output < u_min ) {
-                output = u_min;
-            }
-            if ( output > u_max ) {
-                output = u_max;
-            }
-        }
-        if ( debug ) cout << "output = " << output << endl;
-
-        unsigned int i;
-        for ( i = 0; i < output_list.size(); ++i ) {
-            output_list[i]->setDoubleValue( output );
-        }
-    }
-}
-
-
 /*
  * Roy Vegard Ovesen:
  *
@@ -518,6 +296,196 @@ void FGPIDController::update( double dt ) {
 }
 
 
+FGSimplePIController::FGSimplePIController( SGPropertyNode *node ):
+    proportional( false ),
+    factor( 0.0 ),
+    offset_prop( NULL ),
+    offset_value( 0.0 ),
+    integral( false ),
+    gain( 0.0 ),
+    int_sum( 0.0 ),
+    clamp( false ),
+    debug( false ),
+    y_n( 0.0 ),
+    r_n( 0.0 ),
+    u_min( 0.0 ),
+    u_max( 0.0 )
+{
+    int i;
+    for ( i = 0; i < node->nChildren(); ++i ) {
+        SGPropertyNode *child = node->getChild(i);
+        string cname = child->getName();
+        string cval = child->getStringValue();
+        if ( cname == "name" ) {
+            name = cval;
+        } else if ( cname == "enable" ) {
+            // cout << "parsing enable" << endl;
+            SGPropertyNode *prop = child->getChild( "prop" );
+            if ( prop != NULL ) {
+                // cout << "prop = " << prop->getStringValue() << endl;
+                enable_prop = fgGetNode( prop->getStringValue(), true );
+            } else {
+                // cout << "no prop child" << endl;
+            }
+            SGPropertyNode *val = child->getChild( "value" );
+            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 ) {
+                input_prop = fgGetNode( prop->getStringValue(), true );
+            }
+        } else if ( cname == "reference" ) {
+            SGPropertyNode *prop = child->getChild( "prop" );
+            if ( prop != NULL ) {
+                r_n_prop = fgGetNode( prop->getStringValue(), true );
+            } else {
+                prop = child->getChild( "value" );
+                if ( prop != NULL ) {
+                    r_n_value = prop->getDoubleValue();
+                }
+            }
+        } else if ( cname == "output" ) {
+            int i = 0;
+            SGPropertyNode *prop;
+            while ( (prop = child->getChild("prop", i)) != NULL ) {
+                SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
+                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;
+                }
+
+                tmp = prop->getChild( "max" );
+                if ( tmp != NULL ) {
+                    u_max = tmp->getDoubleValue();
+                    // cout << "max = " << u_max << endl;
+                }
+            }
+        } else if ( cname == "proportional" ) {
+            proportional = true;
+
+            SGPropertyNode *prop;
+
+            prop = child->getChild( "factor" );
+            if ( prop != NULL ) {
+                factor = prop->getDoubleValue();
+            }
+
+            prop = child->getChild( "offset" );
+            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;
+                    }
+                }
+            }
+        } else if ( cname == "integral" ) {
+            integral = true;
+
+            SGPropertyNode *prop;
+            prop = child->getChild( "gain" );
+            if ( prop != NULL ) {
+                gain = prop->getDoubleValue();
+            }
+        } else {
+            SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
+        }
+    }   
+}
+
+
+void FGSimplePIController::update( double dt ) {
+    if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) {
+        if ( !enabled ) {
+            // we have just been enabled, zero out int_sum
+            int_sum = 0.0;
+        }
+        enabled = true;
+    } else {
+        enabled = false;
+    }
+
+    if ( enabled ) {
+        if ( debug ) cout << "Updating " << name << endl;
+        double input = 0.0;
+        if ( input_prop != NULL ) {
+            input = input_prop->getDoubleValue();
+        }
+
+        double r_n = 0.0;
+        if ( r_n_prop != NULL ) {
+            r_n = r_n_prop->getDoubleValue();
+        } else {
+            r_n = r_n_value;
+        }
+                      
+        double error = r_n - input;
+        if ( debug ) cout << "input = " << input
+                          << " reference = " << r_n
+                          << " error = " << error
+                          << endl;
+
+        double prop_comp = 0.0;
+        double offset = 0.0;
+        if ( offset_prop != NULL ) {
+            offset = offset_prop->getDoubleValue();
+            if ( debug ) cout << "offset = " << offset << endl;
+        } else {
+            offset = offset_value;
+        }
+
+        if ( proportional ) {
+            prop_comp = error * factor + offset;
+        }
+
+        if ( integral ) {
+            int_sum += error * gain * dt;
+        } else {
+            int_sum = 0.0;
+        }
+
+        if ( debug ) cout << "prop_comp = " << prop_comp
+                          << " int_sum = " << int_sum << endl;
+
+        double output = prop_comp + int_sum;
+
+        if ( clamp ) {
+            if ( output < u_min ) {
+                output = u_min;
+            }
+            if ( output > u_max ) {
+                output = u_max;
+            }
+        }
+        if ( debug ) cout << "output = " << output << endl;
+
+        unsigned int i;
+        for ( i = 0; i < output_list.size(); ++i ) {
+            output_list[i]->setDoubleValue( output );
+        }
+    }
+}
+
+
 FGXMLAutopilot::FGXMLAutopilot() {
 }
 
@@ -627,19 +595,33 @@ static void update_helper( double dt ) {
         v_last = v;
     }
 
-    // Calculate heading bug error normalized to +/- 180.0
+    // Calculate heading bug error normalized to +/- 180.0 (based on
+    // DG indicated heading)
     static SGPropertyNode *bug
         = fgGetNode( "/autopilot/settings/heading-bug-deg", true );
     static SGPropertyNode *ind_hdg
         = fgGetNode( "/instrumentation/heading-indicator/indicated-heading-deg",
                      true );
-    static SGPropertyNode *bug_error
+    static SGPropertyNode *ind_bug_error
         = fgGetNode( "/autopilot/internal/heading-bug-error-deg", true );
 
     double diff = bug->getDoubleValue() - ind_hdg->getDoubleValue();
     if ( diff < -180.0 ) { diff += 360.0; }
     if ( diff > 180.0 ) { diff -= 360.0; }
-    bug_error->setDoubleValue( diff );
+    ind_bug_error->setDoubleValue( diff );
+
+    // Calculate heading bug error normalized to +/- 180.0 (based on
+    // actual/nodrift magnetic-heading, i.e. a DG slaved to magnetic
+    // compass.)
+    static SGPropertyNode *mag_hdg
+        = fgGetNode( "/orientation/heading-magnetic-deg", true );
+    static SGPropertyNode *fdm_bug_error
+        = fgGetNode( "/autopilot/internal/fdm-heading-bug-error-deg", true );
+
+    diff = bug->getDoubleValue() - mag_hdg->getDoubleValue();
+    if ( diff < -180.0 ) { diff += 360.0; }
+    if ( diff > 180.0 ) { diff -= 360.0; }
+    fdm_bug_error->setDoubleValue( diff );
 
     // Calculate true heading error normalized to +/- 180.0
     static SGPropertyNode *target_true

diff --git a/src/Autopilot/xmlauto.hxx b/src/Autopilot/xmlauto.hxx
index 1e840d82446d65d8e90d148120d6cb0273e82996..92735d6a8721448955bd6043f9c02360573f0ec0 100644 (file)
--- a/src/Autopilot/xmlauto.hxx
+++ b/src/Autopilot/xmlauto.hxx
@@ -83,30 +83,13 @@ public:
 
 
 /**
- * A simple proportional controler
+ * Roy Ovesen's PID controller
  */
 
 class FGPIDController : public FGXMLAutoComponent {
 
 private:
 
-    // proportional component data
-    bool proportional;
-    double factor;
-    SGPropertyNode *offset_prop;
-    double offset_value;
-
-    // integral component data
-    bool integral;
-    double gain;
-    double int_sum;
-
-    // prep functions for error term
-    bool one_eighty;
-
-    // post functions for output
-    bool clamp;
-
     // debug flag
     bool debug;
 
@@ -150,6 +133,48 @@ public:
 };
 
 
+/**
+ * A simplistic P [ + I ] PID controller
+ */
+
+class FGSimplePIController : public FGXMLAutoComponent {
+
+private:
+
+    // proportional component data
+    bool proportional;
+    double factor;
+    SGPropertyNode *offset_prop;
+    double offset_value;
+
+    // integral component data
+    bool integral;
+    double gain;
+    double int_sum;
+
+    // post functions for output
+    bool clamp;
+
+    // debug flag
+    bool debug;
+
+    // Input values
+    double y_n;                 // measured process value
+    double r_n;                 // reference (set point) value
+
+    double u_min;               // Minimum output clamp
+    double u_max;               // Maximum output clamp
+
+    
+public:
+
+    FGSimplePIController( SGPropertyNode *node );
+    ~FGSimplePIController() {}
+
+    void update( double dt );
+};
+
+
 /**
  * Model an autopilot system.
  *