X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Fxmlauto.cxx;h=6761cddf7d30a88330064d7e088b9447b089a75d;hb=11d15b451347674fba77648700d23c5aaec3c6c2;hp=2002ceb7622bb3d984c864467c098512be12edde;hpb=da5ea10d5db9675e6a5d75db5b33edfc350566b1;p=flightgear.git

diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx
index 2002ceb76..6761cddf7 100644
--- a/src/Autopilot/xmlauto.cxx
+++ b/src/Autopilot/xmlauto.cxx
@@ -2,7 +2,7 @@
 //
 // Written by Curtis Olson, started January 2004.
 //
-// Copyright (C) 2004  Curtis L. Olson  - curt@flightgear.org
+// Copyright (C) 2004  Curtis L. Olson  - http://www.flightgear.org/~curt
 //
 // This program is free software; you can redistribute it and/or
 // modify it under the terms of the GNU General Public License as
@@ -16,350 +16,328 @@
 //
 // You should have received a copy of the GNU General Public License
 // along with this program; if not, write to the Free Software
-// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 //
 // $Id$
 
+#ifdef HAVE_CONFIG_H
+#  include <config.h>
+#endif
+
+#include <iostream>
 
 #include <simgear/structure/exception.hxx>
 #include <simgear/misc/sg_path.hxx>
+#include <simgear/sg_inlines.h>
+#include <simgear/props/props_io.hxx>
 
 #include <Main/fg_props.hxx>
 #include <Main/globals.hxx>
+#include <Main/util.hxx>
 
 #include "xmlauto.hxx"
 
+using std::cout;
+using std::endl;
 
-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 )
+void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffset, double aScale )
+{
+    value = aValue;
+    property = NULL; 
+    offset = NULL;
+    scale = NULL;
+    min = NULL;
+    max = NULL;
+
+    if( node == NULL )
+        return;
+
+    SGPropertyNode * n;
+
+    if( (n = node->getChild("condition")) != NULL ) {
+        _condition = sgReadCondition(node, n);
+    }
+
+    if( (n = node->getChild( "scale" )) != NULL ) {
+        scale = new FGXMLAutoInput( n, aScale );
+    }
+
+    if( (n = node->getChild( "offset" )) != NULL ) {
+        offset = new FGXMLAutoInput( n, aOffset );
+    }
+
+    if( (n = node->getChild( "max" )) != NULL ) {
+        max = new FGXMLAutoInput( n );
+    }
+
+    if( (n = node->getChild( "min" )) != NULL ) {
+        min = new FGXMLAutoInput( n );
+    }
+
+    if( (n = node->getChild( "abs" )) != NULL ) {
+      abs = n->getBoolValue();
+    }
+
+    SGPropertyNode *valueNode = node->getChild( "value" );
+    if ( valueNode != NULL ) {
+        value = valueNode->getDoubleValue();
+    }
+
+    n = node->getChild( "property" );
+    // if no <property> element, check for <prop> element for backwards
+    // compatibility
+    if(  n == NULL )
+        n = node->getChild( "prop" );
+
+    if (  n != NULL ) {
+        property = fgGetNode(  n->getStringValue(), true );
+        if ( valueNode != NULL ) {
+            // initialize property with given value 
+            // if both <prop> and <value> exist
+            double s = get_scale();
+            if( s != 0 )
+              property->setDoubleValue( (value - get_offset())/s );
+            else
+              property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero
+        }
+    }
+
+    if ( n == NULL && valueNode == NULL ) {
+        // no <value> element and no <prop> element, use text node 
+        const char * textnode = node->getStringValue();
+        char * endp = NULL;
+        // try to convert to a double value. If the textnode does not start with a number
+        // endp will point to the beginning of the string. We assume this should be
+        // a property name
+        value = strtod( textnode, &endp );
+        if( endp == textnode ) {
+          property = fgGetNode( textnode, true );
+        }
+    }
+}
+
+void FGXMLAutoInput::set_value( double aValue ) 
+{
+    double s = get_scale();
+    if( s != 0 )
+        property->setDoubleValue( (aValue - get_offset())/s );
+    else
+        property->setDoubleValue( 0 ); // if scale is zero, value*scale is zero
+}
+
+double FGXMLAutoInput::get_value() 
+{
+    if( property != NULL ) 
+        value = property->getDoubleValue();
+
+    if( scale ) 
+        value *= scale->get_value();
+
+    if( offset ) 
+        value += offset->get_value();
+
+    if( min ) {
+        double m = min->get_value();
+        if( value < m )
+            value = m;
+    }
+
+    if( max ) {
+        double m = max->get_value();
+        if( value > m )
+            value = m;
+    }
+    
+    return abs ? fabs(value) : value;
+}
+
+FGXMLAutoComponent::FGXMLAutoComponent( SGPropertyNode * node ) :
+      _condition( NULL ),
+      enable_prop( NULL ),
+      enable_value( NULL ),
+      passive_mode( fgGetNode("/autopilot/locks/passive-mode", true) ),
+      honor_passive( false ),
+      name(""),
+      feedback_if_disabled( false ),
+      debug(false),
+      enabled( false )
 {
     int i;
+    SGPropertyNode *prop; 
+
     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 == "feedback-if-disabled" ) {
+            feedback_if_disabled = child->getBoolValue();
+
+        } else if ( cname == "debug" ) {
+            debug = child->getBoolValue();
+
         } 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 );
+            if( (prop = child->getChild("condition")) != NULL ) {
+              _condition = sgReadCondition(child, prop);
             } else {
-                cout << "no prop child" << endl;
+               if ( (prop = child->getChild( "prop" )) != NULL ) {
+                   enable_prop = fgGetNode( prop->getStringValue(), true );
+               }
+
+               if ( (prop = child->getChild( "value" )) != NULL ) {
+                   delete enable_value;
+                   enable_value = new string(prop->getStringValue());
+               }
             }
-            SGPropertyNode *val = child->getChild( "value" );
-            if ( val != NULL ) {
-                enable_value = val->getStringValue();
+            if ( (prop = child->getChild( "honor-passive" )) != NULL ) {
+                honor_passive = prop->getBoolValue();
             }
-        } 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 );
-            }
+
+              valueInput.push_back( new FGXMLAutoInput( child ) );
+
         } 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();
-                }
-            }
+
+              referenceInput.push_back( new FGXMLAutoInput( child ) );
+
         } else if ( cname == "output" ) {
-            int i = 0;
-            SGPropertyNode *prop;
-            while ( (prop = child->getChild("prop", i)) != NULL ) {
+            // grab all <prop> and <property> childs
+            int found = 0;
+            // backwards compatibility: allow <prop> elements
+            for( int i = 0; (prop = child->getChild("prop", i)) != NULL; i++ ) { 
                 SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
                 output_list.push_back( tmp );
-                i++;
+                found++;
             }
-            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;
-                }
+            for( int i = 0; (prop = child->getChild("property", i)) != NULL; i++ ) { 
+                SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true );
+                output_list.push_back( tmp );
+                found++;
             }
-        } else if ( cname == "proportional" ) {
-            proportional = true;
 
-            SGPropertyNode *prop;
+            // no <prop> elements, text node of <output> is property name
+            if( found == 0 )
+                output_list.push_back( fgGetNode(child->getStringValue(), true ) );
 
-            prop = child->getChild( "pre" );
-            if ( prop != NULL ) {
-                prop = prop->getChild( "one-eighty" );
-                if ( prop != NULL && prop->getBoolValue() ) {
-                    one_eighty = true;
-                }
+        } else if ( cname == "config" ) {
+            if( (prop = child->getChild("min")) != NULL ) {
+              uminInput.push_back( new FGXMLAutoInput( prop ) );
             }
-
-            prop = child->getChild( "factor" );
-            if ( prop != NULL ) {
-                factor = prop->getDoubleValue();
+            if( (prop = child->getChild("u_min")) != NULL ) {
+              uminInput.push_back( new FGXMLAutoInput( prop ) );
             }
-
-            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;
-                    }
-                }
+            if( (prop = child->getChild("max")) != NULL ) {
+              umaxInput.push_back( new FGXMLAutoInput( prop ) );
             }
-        } else if ( cname == "integral" ) {
-            integral = true;
-
-            SGPropertyNode *prop;
-            prop = child->getChild( "gain" );
-            if ( prop != NULL ) {
-                gain = prop->getDoubleValue();
+            if( (prop = child->getChild("u_max")) != NULL ) {
+              umaxInput.push_back( new FGXMLAutoInput( prop ) );
             }
+        } else if ( cname == "min" ) {
+            uminInput.push_back( new FGXMLAutoInput( child ) );
+        } else if ( cname == "u_min" ) {
+            uminInput.push_back( new FGXMLAutoInput( child ) );
+        } else if ( cname == "max" ) {
+            umaxInput.push_back( new FGXMLAutoInput( child ) );
+        } else if ( cname == "u_max" ) {
+            umaxInput.push_back( new FGXMLAutoInput( child ) );
+        } 
+    }   
+}
+
+FGXMLAutoComponent::~FGXMLAutoComponent() 
+{
+    delete enable_value;
+}
+
+bool FGXMLAutoComponent::isPropertyEnabled()
+{
+    if( _condition )
+        return _condition->test();
+
+    if( enable_prop ) {
+        if( enable_value ) {
+            return *enable_value == enable_prop->getStringValue();
         } else {
-            SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
+            return enable_prop->getBoolValue();
         }
-    }   
+    }
+    return true;
+}
+
+void FGXMLAutoComponent::do_feedback_if_disabled()
+{
+    if( output_list.size() > 0 ) {    
+        FGXMLAutoInput * input = valueInput.get_active();
+        if( input != NULL )
+            input->set_value( output_list[0]->getDoubleValue() );
+    }
 }
 
+double FGXMLAutoComponent::clamp( double value )
+{
+    // clamp, if either min or max is defined
+    if( uminInput.size() + umaxInput.size() > 0 ) {
+        double d = umaxInput.get_value( 0.0 );
+        if( value > d ) value = d;
+        d = uminInput.get_value( 0.0 );
+        if( value < d ) value = d;
+    }
+    return value;
+}
 
 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 ),
-    Kp( 0.0 ),
+    FGXMLAutoComponent( node ),
     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 )
+    u_n_1( 0.0 ),
+    desiredTs( 0.0 ),
+    elapsedTime( 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 == "debug" ) {
-            debug = child->getBoolValue();
-        } 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 == "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 = 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++;
-            }
-        } else if ( cname == "config" ) {
-            SGPropertyNode *prop;
+        if ( cname == "config" ) {
+            SGPropertyNode *config;
 
-            prop = child->getChild( "Kp" );
-            if ( prop != NULL ) {
-                Kp = prop->getDoubleValue();
+            if ( (config = child->getChild( "Ts" )) != NULL ) {
+                desiredTs = config->getDoubleValue();
             }
-
-            prop = child->getChild( "beta" );
-            if ( prop != NULL ) {
-                beta = prop->getDoubleValue();
+           
+            Kp.push_back( new FGXMLAutoInput( child->getChild( "Kp" ) ) );
+            Ti.push_back( new FGXMLAutoInput( child->getChild( "Ti" ) ) );
+            Td.push_back( new FGXMLAutoInput( child->getChild( "Td" ) ) );
+
+            config = child->getChild( "beta" );
+            if ( config != NULL ) {
+                beta = config->getDoubleValue();
             }
 
-            prop = child->getChild( "alpha" );
-            if ( prop != NULL ) {
-                alpha = prop->getDoubleValue();
+            config = child->getChild( "alpha" );
+            if ( config != NULL ) {
+                alpha = config->getDoubleValue();
             }
 
-            prop = child->getChild( "gamma" );
-            if ( prop != NULL ) {
-                gamma = prop->getDoubleValue();
+            config = child->getChild( "gamma" );
+            if ( config != NULL ) {
+                gamma = config->getDoubleValue();
             }
 
-            prop = child->getChild( "Ti" );
-            if ( prop != NULL ) {
-                Ti = prop->getDoubleValue();
-            }
-
-            prop = child->getChild( "Td" );
-            if ( prop != NULL ) {
-                Td = prop->getDoubleValue();
-            }
-
-            prop = child->getChild( "u_min" );
-            if ( prop != NULL ) {
-                u_min = prop->getDoubleValue();
-            }
-
-            prop = child->getChild( "u_max" );
-            if ( prop != NULL ) {
-                u_max = prop->getDoubleValue();
-            }
         } else {
             SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
-        }
-    }   
-}
-
-
-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 ( get_name().length() ) {
+                SG_LOG( SG_AUTOPILOT, SG_WARN, "Section = " << get_name() );
             }
         }
-        if ( debug ) cout << "output = " << output << endl;
-
-        unsigned int i;
-        for ( i = 0; i < output_list.size(); ++i ) {
-            output_list[i]->setDoubleValue( output );
-        }
-    }
+    }   
 }
 
 
@@ -415,41 +393,46 @@ void FGPIDController::update_old( double dt ) {
  */
 
 void FGPIDController::update( double dt ) {
-    double ep_n;         // proportional error with reference weighing
-    double e_n;          // error
-    double ed_n;         // derivative error
-    double edf_n;        // derivative error filter
-    double Tf;           // filter time
-    double delta_u_n;    // incremental output
-    double u_n;          // absolute output
-    double Ts = dt;      // Sampling interval (sec)
-
-    if ( Ts <= 0.0 ) {
+    double ep_n;            // proportional error with reference weighing
+    double e_n;             // error
+    double ed_n;            // derivative error
+    double edf_n;           // derivative error filter
+    double Tf;              // filter time
+    double delta_u_n = 0.0; // incremental output
+    double u_n = 0.0;       // absolute output
+    double Ts;              // sampling interval (sec)
+
+    double u_min = uminInput.get_value();
+    double u_max = umaxInput.get_value();
+
+    elapsedTime += dt;
+    if ( elapsedTime <= desiredTs ) {
         // do nothing if time step is not positive (i.e. no time has
         // elapsed)
         return;
     }
+    Ts = elapsedTime;
+    elapsedTime = 0.0;
 
-    if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) {
+    if ( isPropertyEnabled() ) {
+        if ( !enabled ) {
+            // first time being enabled, seed u_n with current
+            // property tree value
+            u_n = get_output_value();
+            u_n_1 = u_n;
+        }
         enabled = true;
     } else {
         enabled = false;
+        do_feedback();
     }
 
-    if ( enabled ) {
-        if ( debug ) cout << "Updating " << name << endl;
-
-        double y_n = 0.0;
-        if ( input_prop != NULL ) {
-            y_n = input_prop->getDoubleValue();
-        }
+    if ( enabled && Ts > 0.0) {
+        if ( debug ) cout << "Updating " << get_name()
+                          << " Ts " << Ts << endl;
 
-        double r_n = 0.0;
-        if ( r_n_prop != NULL ) {
-            r_n = r_n_prop->getDoubleValue();
-        } else {
-            r_n = r_n_value;
-        }
+        double y_n = valueInput.get_value();
+        double r_n = referenceInput.get_value();
                       
         if ( debug ) cout << "  input = " << y_n << " ref = " << r_n << endl;
 
@@ -466,26 +449,43 @@ void FGPIDController::update( double dt ) {
         ed_n = gamma * r_n - y_n;
         if ( debug ) cout << " ed_n = " << ed_n;
 
-        // Calculates filter time:
-        Tf = alpha * Td;
-        if ( debug ) cout << " Tf = " << Tf;
+        double td = Td.get_value();
+        if ( td > 0.0 ) {
+            // Calculates filter time:
+            Tf = alpha * td;
+            if ( debug ) cout << " Tf = " << Tf;
 
-        // Filters the derivate error:
-        edf_n = edf_n_1 / (Ts/Tf + 1)
-            + ed_n * (Ts/Tf) / (Ts/Tf + 1);
-        if ( debug ) cout << " edf_n = " << edf_n;
+            // Filters the derivate error:
+            edf_n = edf_n_1 / (Ts/Tf + 1)
+                + ed_n * (Ts/Tf) / (Ts/Tf + 1);
+            if ( debug ) cout << " edf_n = " << edf_n;
+        } else {
+            edf_n = ed_n;
+        }
 
         // Calculates the incremental output:
-        delta_u_n = Kp * ( (ep_n - ep_n_1)
-                           + ((Ts/Ti) * e_n)
-                           + ((Td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
-        if ( debug ) cout << " delta_u_n = " << delta_u_n << endl;
+        double ti = Ti.get_value();
+        if ( ti > 0.0 ) {
+            delta_u_n = Kp.get_value() * ( (ep_n - ep_n_1)
+                               + ((Ts/ti) * e_n)
+                               + ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
+        }
+
+        if ( debug ) {
+            cout << " delta_u_n = " << delta_u_n << endl;
+            cout << "P:" << Kp.get_value() * (ep_n - ep_n_1)
+                 << " I:" << Kp.get_value() * ((Ts/ti) * e_n)
+                 << " D:" << Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2))
+                 << endl;
+        }
 
         // Integrator anti-windup logic:
         if ( delta_u_n > (u_max - u_n_1) ) {
-            delta_u_n = 0;
+            delta_u_n = u_max - u_n_1;
+            if ( debug ) cout << " max saturation " << endl;
         } else if ( delta_u_n < (u_min - u_n_1) ) {
-            delta_u_n = 0;
+            delta_u_n = u_min - u_n_1;
+            if ( debug ) cout << " min saturation " << endl;
         }
 
         // Calculates absolute output:
@@ -498,12 +498,8 @@ void FGPIDController::update( double dt ) {
         edf_n_2 = edf_n_1;
         edf_n_1 = edf_n;
 
-        unsigned int i;
-        for ( i = 0; i < output_list.size(); ++i ) {
-            output_list[i]->setDoubleValue( u_n );
-        }
+        set_output_value( u_n );
     } else if ( !enabled ) {
-        u_n   = 0.0;
         ep_n  = 0.0;
         edf_n = 0.0;
         // Updates indexed values;
@@ -515,6 +511,259 @@ void FGPIDController::update( double dt ) {
 }
 
 
+FGPISimpleController::FGPISimpleController( SGPropertyNode *node ):
+    FGXMLAutoComponent( node ),
+    int_sum( 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 == "config" ) {
+            Kp.push_back( new FGXMLAutoInput( child->getChild( "Kp" ) ) );
+            Ki.push_back( new FGXMLAutoInput( child->getChild( "Ki" ) ) );
+        } else {
+            SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
+            if ( get_name().length() ) {
+                SG_LOG( SG_AUTOPILOT, SG_WARN, "Section = " << get_name() );
+            }
+        }
+    }   
+}
+
+
+void FGPISimpleController::update( double dt ) {
+
+    if ( isPropertyEnabled() ) {
+        if ( !enabled ) {
+            // we have just been enabled, zero out int_sum
+            int_sum = 0.0;
+        }
+        enabled = true;
+    } else {
+        enabled = false;
+        do_feedback();
+    }
+
+    if ( enabled ) {
+        if ( debug ) cout << "Updating " << get_name() << endl;
+        double y_n = valueInput.get_value();
+        double r_n = referenceInput.get_value();
+                      
+        double error = r_n - y_n;
+        if ( debug ) cout << "input = " << y_n
+                          << " reference = " << r_n
+                          << " error = " << error
+                          << endl;
+
+        double prop_comp = error * Kp.get_value();
+        int_sum += error * Ki.get_value() * dt;
+
+
+        if ( debug ) cout << "prop_comp = " << prop_comp
+                          << " int_sum = " << int_sum << endl;
+
+        double output = prop_comp + int_sum;
+        output = clamp( output );
+        set_output_value( output );
+        if ( debug ) cout << "output = " << output << endl;
+    }
+}
+
+
+FGPredictor::FGPredictor ( SGPropertyNode *node ):
+    FGXMLAutoComponent( node ),
+    average(0.0)
+{
+    int i;
+    for ( i = 0; i < node->nChildren(); ++i ) {
+        SGPropertyNode *child = node->getChild(i);
+        string cname = child->getName();
+        if ( cname == "seconds" ) {
+            seconds.push_back( new FGXMLAutoInput( child, 0 ) );
+        } else if ( cname == "filter-gain" ) {
+            filter_gain.push_back( new FGXMLAutoInput( child, 0 ) );
+        }
+    }   
+}
+
+void FGPredictor::update( double dt ) {
+    /*
+       Simple moving average filter converts input value to predicted value "seconds".
+
+       Smoothing as described by Curt Olson:
+         gain would be valid in the range of 0 - 1.0
+         1.0 would mean no filtering.
+         0.0 would mean no input.
+         0.5 would mean (1 part past value + 1 part current value) / 2
+         0.1 would mean (9 parts past value + 1 part current value) / 10
+         0.25 would mean (3 parts past value + 1 part current value) / 4
+
+    */
+
+    double ivalue = valueInput.get_value();
+
+    if ( isPropertyEnabled() ) {
+        if ( !enabled ) {
+            // first time being enabled
+            last_value = ivalue;
+        }
+        enabled = true;
+    } else {
+        enabled = false;
+        do_feedback();
+    }
+
+    if ( enabled ) {
+
+        if ( dt > 0.0 ) {
+            double current = (ivalue - last_value)/dt; // calculate current error change (per second)
+            average = dt < 1.0 ? ((1.0 - dt) * average + current * dt) : current;
+
+            // calculate output with filter gain adjustment
+            double output = ivalue + 
+               (1.0 - filter_gain.get_value()) * (average * seconds.get_value()) + 
+                       filter_gain.get_value() * (current * seconds.get_value());
+            output = clamp( output );
+            set_output_value( output );
+        }
+        last_value = ivalue;
+    }
+}
+
+
+FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node):
+    FGXMLAutoComponent( node ),
+    filterType(none)
+{
+    int i;
+    for ( i = 0; i < node->nChildren(); ++i ) {
+        SGPropertyNode *child = node->getChild(i);
+        string cname = child->getName();
+        string cval = child->getStringValue();
+        if ( cname == "type" ) {
+            if ( cval == "exponential" ) {
+                filterType = exponential;
+            } else if (cval == "double-exponential") {
+                filterType = doubleExponential;
+            } else if (cval == "moving-average") {
+                filterType = movingAverage;
+            } else if (cval == "noise-spike") {
+                filterType = noiseSpike;
+            } else if (cval == "gain") {
+                filterType = gain;
+            } else if (cval == "reciprocal") {
+                filterType = reciprocal;
+            }
+        } else if ( cname == "filter-time" ) {
+            TfInput.push_back( new FGXMLAutoInput( child, 1.0 ) );
+            if( filterType == none ) filterType = exponential;
+        } else if ( cname == "samples" ) {
+            samplesInput.push_back( new FGXMLAutoInput( child, 1 ) );
+            if( filterType == none ) filterType = movingAverage;
+        } else if ( cname == "max-rate-of-change" ) {
+            rateOfChangeInput.push_back( new FGXMLAutoInput( child, 1 ) );
+            if( filterType == none ) filterType = noiseSpike;
+        } else if ( cname == "gain" ) {
+            gainInput.push_back( new FGXMLAutoInput( child, 1 ) );
+            if( filterType == none ) filterType = gain;
+        }
+    }
+
+    output.resize(2, 0.0);
+    input.resize(samplesInput.get_value() + 1, 0.0);
+}
+
+void FGDigitalFilter::update(double dt)
+{
+    if ( isPropertyEnabled() ) {
+
+        input.push_front(valueInput.get_value());
+        input.resize(samplesInput.get_value() + 1, 0.0);
+
+        if ( !enabled ) {
+            // first time being enabled, initialize output to the
+            // value of the output property to avoid bumping.
+            output.push_front(get_output_value());
+        }
+
+        enabled = true;
+    } else {
+        enabled = false;
+        do_feedback();
+    }
+
+    if ( enabled && dt > 0.0 ) {
+        /*
+         * Exponential filter
+         *
+         * Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1]
+         *
+         */
+         if( debug ) cout << "Updating " << get_name()
+                          << " dt " << dt << endl;
+
+        if (filterType == exponential)
+        {
+            double alpha = 1 / ((TfInput.get_value()/dt) + 1);
+            output.push_front(alpha * input[0] + 
+                              (1 - alpha) * output[0]);
+        } 
+        else if (filterType == doubleExponential)
+        {
+            double alpha = 1 / ((TfInput.get_value()/dt) + 1);
+            output.push_front(alpha * alpha * input[0] + 
+                              2 * (1 - alpha) * output[0] -
+                              (1 - alpha) * (1 - alpha) * output[1]);
+        }
+        else if (filterType == movingAverage)
+        {
+            output.push_front(output[0] + 
+                              (input[0] - input.back()) / samplesInput.get_value());
+        }
+        else if (filterType == noiseSpike)
+        {
+            double maxChange = rateOfChangeInput.get_value() * dt;
+
+            if ((output[0] - input[0]) > maxChange)
+            {
+                output.push_front(output[0] - maxChange);
+            }
+            else if ((output[0] - input[0]) < -maxChange)
+            {
+                output.push_front(output[0] + maxChange);
+            }
+            else if (fabs(input[0] - output[0]) <= maxChange)
+            {
+                output.push_front(input[0]);
+            }
+        }
+        else if (filterType == gain)
+        {
+            output[0] = gainInput.get_value() * input[0];
+        }
+        else if (filterType == reciprocal)
+        {
+            if (input[0] != 0.0) {
+                output[0] = gainInput.get_value() / input[0];
+            }
+        }
+
+        output[0] = clamp(output[0]) ;
+        set_output_value( output[0] );
+
+        output.resize(2);
+
+        if (debug)
+        {
+            cout << "input:" << input[0] 
+                 << "\toutput:" << output[0] << endl;
+        }
+    }
+}
+
+
 FGXMLAutopilot::FGXMLAutopilot() {
 }
 
@@ -539,14 +788,14 @@ void FGXMLAutopilot::init() {
 
             if ( ! build() ) {
                 SG_LOG( SG_ALL, SG_ALERT,
-                        "Detected an internal inconsistancy in the autopilot");
+                        "Detected an internal inconsistency in the autopilot");
                 SG_LOG( SG_ALL, SG_ALERT,
                         " configuration.  See earlier errors for" );
                 SG_LOG( SG_ALL, SG_ALERT,
                         " details.");
                 exit(-1);
             }        
-        } catch (const sg_exception& exc) {
+        } catch (const sg_exception&) {
             SG_LOG( SG_ALL, SG_ALERT, "Failed to load autopilot configuration: "
                     << config.str() );
         }
@@ -560,7 +809,7 @@ void FGXMLAutopilot::init() {
 
 void FGXMLAutopilot::reinit() {
     components.clear();
-    build();
+    init();
 }
 
 
@@ -580,8 +829,13 @@ bool FGXMLAutopilot::build() {
         string name = node->getName();
         // cout << name << endl;
         if ( name == "pid-controller" ) {
-            FGXMLAutoComponent *c = new FGPIDController( node );
-            components.push_back( c );
+            components.push_back( new FGPIDController( node ) );
+        } else if ( name == "pi-simple-controller" ) {
+            components.push_back( new FGPISimpleController( node ) );
+        } else if ( name == "predict-simple" ) {
+            components.push_back( new FGPredictor( node ) );
+        } else if ( name == "filter" ) {
+            components.push_back( new FGDigitalFilter( node ) );
         } else {
             SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: " 
                     << name );
@@ -598,10 +852,10 @@ bool FGXMLAutopilot::build() {
  */
 static void update_helper( double dt ) {
     // Estimate speed in 5,10 seconds
-    static SGPropertyNode *vel = fgGetNode( "/velocities/airspeed-kt", true );
-    static SGPropertyNode *lookahead5
+    static SGPropertyNode_ptr vel = fgGetNode( "/velocities/airspeed-kt", true );
+    static SGPropertyNode_ptr lookahead5
         = fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true );
-    static SGPropertyNode *lookahead10
+    static SGPropertyNode_ptr lookahead10
         = fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true );
 
     static double average = 0.0; // average/filtered prediction
@@ -623,26 +877,42 @@ static void update_helper( double dt ) {
         v_last = v;
     }
 
-    // Calculate heading bug error normalized to +/- 180.0
-    static SGPropertyNode *bug
+    // Calculate heading bug error normalized to +/- 180.0 (based on
+    // DG indicated heading)
+    static SGPropertyNode_ptr bug
         = fgGetNode( "/autopilot/settings/heading-bug-deg", true );
-    static SGPropertyNode *ind_hdg
+    static SGPropertyNode_ptr ind_hdg
         = fgGetNode( "/instrumentation/heading-indicator/indicated-heading-deg",
                      true );
-    static SGPropertyNode *bug_error
+    static SGPropertyNode_ptr 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_ptr mag_hdg
+        = fgGetNode( "/orientation/heading-magnetic-deg", true );
+    static SGPropertyNode_ptr 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
+    static SGPropertyNode_ptr target_true
         = fgGetNode( "/autopilot/settings/true-heading-deg", true );
-    static SGPropertyNode *true_hdg
+    static SGPropertyNode_ptr true_hdg
         = fgGetNode( "/orientation/heading-deg", true );
-    static SGPropertyNode *true_error
+    static SGPropertyNode_ptr true_track
+        = fgGetNode( "/instrumentation/gps/indicated-track-true-deg", true );
+    static SGPropertyNode_ptr true_error
         = fgGetNode( "/autopilot/internal/true-heading-error-deg", true );
 
     diff = target_true->getDoubleValue() - true_hdg->getDoubleValue();
@@ -651,15 +921,65 @@ static void update_helper( double dt ) {
     true_error->setDoubleValue( diff );
 
     // Calculate nav1 target heading error normalized to +/- 180.0
-    static SGPropertyNode *target_nav1
-        = fgGetNode( "/radios/nav[0]/radials/target-auto-hdg-deg", true );
-    static SGPropertyNode *true_nav1
+    static SGPropertyNode_ptr target_nav1
+        = fgGetNode( "/instrumentation/nav[0]/radials/target-auto-hdg-deg", true );
+    static SGPropertyNode_ptr true_nav1
         = fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true );
+    static SGPropertyNode_ptr true_track_nav1
+        = fgGetNode( "/autopilot/internal/nav1-track-error-deg", true );
 
     diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue();
     if ( diff < -180.0 ) { diff += 360.0; }
     if ( diff > 180.0 ) { diff -= 360.0; }
     true_nav1->setDoubleValue( diff );
+
+    diff = target_nav1->getDoubleValue() - true_track->getDoubleValue();
+    if ( diff < -180.0 ) { diff += 360.0; }
+    if ( diff > 180.0 ) { diff -= 360.0; }
+    true_track_nav1->setDoubleValue( diff );
+
+    // Calculate nav1 selected course error normalized to +/- 180.0
+    // (based on DG indicated heading)
+    static SGPropertyNode_ptr nav1_course_error
+        = fgGetNode( "/autopilot/internal/nav1-course-error", true );
+    static SGPropertyNode_ptr nav1_selected_course
+        = fgGetNode( "/instrumentation/nav[0]/radials/selected-deg", true );
+
+    diff = nav1_selected_course->getDoubleValue() - ind_hdg->getDoubleValue();
+//    if ( diff < -180.0 ) { diff += 360.0; }
+//    if ( diff > 180.0 ) { diff -= 360.0; }
+    SG_NORMALIZE_RANGE( diff, -180.0, 180.0 );
+    nav1_course_error->setDoubleValue( diff );
+
+    // Calculate vertical speed in fpm
+    static SGPropertyNode_ptr vs_fps
+        = fgGetNode( "/velocities/vertical-speed-fps", true );
+    static SGPropertyNode_ptr vs_fpm
+        = fgGetNode( "/autopilot/internal/vert-speed-fpm", true );
+
+    vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );
+
+
+    // Calculate static port pressure rate in [inhg/s].
+    // Used to determine vertical speed.
+    static SGPropertyNode_ptr static_pressure
+	= fgGetNode( "/systems/static[0]/pressure-inhg", true );
+    static SGPropertyNode_ptr pressure_rate
+	= fgGetNode( "/autopilot/internal/pressure-rate", true );
+
+    static double last_static_pressure = 0.0;
+
+    if ( dt > 0.0 ) {
+	double current_static_pressure = static_pressure->getDoubleValue();
+
+	double current_pressure_rate = 
+	    ( current_static_pressure - last_static_pressure ) / dt;
+
+	pressure_rate->setDoubleValue(current_pressure_rate);
+
+	last_static_pressure = current_static_pressure;
+    }
+
 }
 
 
@@ -675,3 +995,4 @@ void FGXMLAutopilot::update( double dt ) {
         components[i]->update( dt );
     }
 }
+