From ec50bcfb5a27fa72cff98eb1a24943cf48910db5 Mon Sep 17 00:00:00 2001
From: curt <curt>
Date: Tue, 1 Feb 2005 21:00:24 +0000
Subject: [PATCH] Roy Ovesen:

I've added some features to the PID controller:

Ability to set desired sampling interval in seconds. Use <Ts> under <config>
to set the desired sampling interval of the PID controller.

Example:
<config>
     <Ts>0.1</Ts>        <!-- desired sampling interval -->
     <Kp>-0.05</Kp>        <!-- proportional gain -->
     <beta>1.0</beta>    <!-- input value weighing factor -->
     ...
     ...
</config>

Ts defaults to 0.0, so if you don't set it it samples at the highest possible
frequency.


Add an offset to the input variables (input and reference).
Example:
    <reference>
      <prop>/controls/flight/elevator</prop>
      <scale>-1.5</scale>
      <offset>1.0</offset>
    </reference>

Note that <scale> has higher precedence than <offset>, regardless of the order
that they appear in the config file.
---
 src/Autopilot/xmlauto.cxx | 34 +++++++++++++++++++++++++++-------
 src/Autopilot/xmlauto.hxx |  4 ++++
 2 files changed, 31 insertions(+), 7 deletions(-)

diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx
index c8b7b9c61..ac0953b6d 100644
--- a/src/Autopilot/xmlauto.cxx
+++ b/src/Autopilot/xmlauto.cxx
@@ -38,6 +38,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 +51,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 +85,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 +103,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 +118,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 +231,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 +258,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;
         }
diff --git a/src/Autopilot/xmlauto.hxx b/src/Autopilot/xmlauto.hxx
index 7d87dc7cf..ded921924 100644
--- a/src/Autopilot/xmlauto.hxx
+++ b/src/Autopilot/xmlauto.hxx
@@ -100,6 +100,8 @@ private:
     double r_n;                 // reference (set point) value
     double y_scale;             // scale process input from property system
     double r_scale;             // scale reference input from property system
+    double y_offset;
+    double r_offset;
 
     // Configuration values
     double Kp;                  // proportional gain
@@ -123,6 +125,8 @@ private:
     double edf_n_1;             // edf[n-1] (derivative error)
     double edf_n_2;             // edf[n-2] (derivative error)
     double u_n_1;               // u[n-1]   (output)
+    double desiredTs;            // desired sampling interval (sec)
+    double elapsedTime;          // elapsed time (sec)
     
     
     
-- 
2.39.5