only activate heading & altitude lock when in air (and even then it should

[flightgear.git] / src / Autopilot / xmlauto.cxx

diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx
index c8b7b9c61d5a5f1e2e312d7a71e4f0c986411d38..2de17263ef9a59d46f8878b5f6a5411ac1b5867f 100644 (file)
--- a/src/Autopilot/xmlauto.cxx
+++ b/src/Autopilot/xmlauto.cxx
@@ -16,10 +16,13 @@
 //
 // 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 <simgear/structure/exception.hxx>
 #include <simgear/misc/sg_path.hxx>
@@ -38,6 +41,8 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
     r_n( 0.0 ),
     y_scale( 1.0 ),
     r_scale( 1.0 ),
+    y_offset( 0.0 ),
+    r_offset( 0.0 ),
     Kp( 0.0 ),
     alpha( 0.1 ),
     beta( 1.0 ),
@@ -49,7 +54,8 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
     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 )
 {
     int i;
     for ( i = 0; i < node->nChildren(); ++i ) {
@@ -82,6 +88,10 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
             if ( prop != NULL ) {
                 y_scale = prop->getDoubleValue();
             }
+            prop = child->getChild( "offset" );
+            if ( prop != NULL ) {
+                y_offset = prop->getDoubleValue();
+            }
         } else if ( cname == "reference" ) {
             SGPropertyNode *prop = child->getChild( "prop" );
             if ( prop != NULL ) {
@@ -96,6 +106,10 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
             if ( prop != NULL ) {
                 r_scale = prop->getDoubleValue();
             }
+            prop = child->getChild( "offset" );
+            if ( prop != NULL ) {
+                r_offset = prop->getDoubleValue();
+            }
         } else if ( cname == "output" ) {
             int i = 0;
             SGPropertyNode *prop;
@@ -107,6 +121,11 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
         } else if ( cname == "config" ) {
             SGPropertyNode *prop;
 
+            prop = child->getChild( "Ts" );
+            if ( prop != NULL ) {
+                desiredTs = prop->getDoubleValue();
+            }
+            
             prop = child->getChild( "Kp" );
             if ( prop != NULL ) {
                 Kp = prop->getDoubleValue();
@@ -215,13 +234,16 @@ void FGPIDController::update( double dt ) {
     double Tf;              // filter time
     double delta_u_n = 0.0; // incremental output
     double u_n = 0.0;       // absolute output
-    double Ts = dt;         // Sampling interval (sec)
-
-    if ( Ts <= 0.0 ) {
+    double Ts;              // sampling interval (sec)
+    
+    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 ( !enabled ) {
@@ -239,16 +261,17 @@ void FGPIDController::update( double dt ) {
     }
 
     if ( enabled && Ts > 0.0) {
-        if ( debug ) cout << "Updating " << name << endl;
+        if ( debug ) cout << "Updating " << name
+                          << " Ts " << Ts << endl;
 
         double y_n = 0.0;
         if ( input_prop != NULL ) {
-            y_n = input_prop->getDoubleValue() * y_scale;
+            y_n = input_prop->getDoubleValue() * y_scale + y_offset;
         }
 
         double r_n = 0.0;
         if ( r_n_prop != NULL ) {
-            r_n = r_n_prop->getDoubleValue() * r_scale;
+            r_n = r_n_prop->getDoubleValue() * r_scale + r_offset;
         } else {
             r_n = r_n_value;
         }
@@ -286,9 +309,6 @@ void FGPIDController::update( double dt ) {
             delta_u_n = Kp * ( (ep_n - ep_n_1)
                                + ((Ts/Ti) * e_n)
                                + ((Td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
-        } else if ( Ti <= 0.0 ) {
-            delta_u_n = Kp * ( (ep_n - ep_n_1)
-                               + ((Td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
         }
 
         if ( debug ) {
@@ -301,10 +321,10 @@ void FGPIDController::update( double dt ) {
 
         // 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;
         }
 
@@ -631,14 +651,14 @@ FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node)
     }
 
     output.resize(2, 0.0);
-    input.resize(samples, 0.0);
+    input.resize(samples + 1, 0.0);
 }
 
 void FGDigitalFilter::update(double dt)
 {
     if ( input_prop != NULL ) {
         input.push_front(input_prop->getDoubleValue());
-        input.resize(samples, 0.0);
+        input.resize(samples + 1, 0.0);
         // no sense if there isn't an input :-)
         enabled = true;
     } else {
@@ -742,7 +762,7 @@ 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,