From: curt <curt>
Date: Wed, 4 Feb 2004 17:10:32 +0000 (+0000)
Subject: Cleaned up some left over stuff.  Working towards infrastructure to support
X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=8191e743f0a207309f5e7d67858ce0c5d52c51ef;p=flightgear.git

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 8edc11c60..bf37d442c 100644
--- 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 1e840d824..92735d6a8 100644
--- 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.
  *