Update MSVC 7.1 project file : annunciator removed

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

diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx
index 3fef7eaa123e07683aa50ebc8849e5cad1b56b7f..9594d5f07583dc23ed1856662f5d252d97eadf6b 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 ) {
@@ -73,6 +79,10 @@ FGPIDController::FGPIDController( SGPropertyNode *node ):
             if ( val != NULL ) {
                 enable_value = val->getStringValue();
             }
+            SGPropertyNode *pass = child->getChild( "honor-passive" );
+            if ( pass != NULL ) {
+                honor_passive = pass->getBoolValue();
+            }
         } else if ( cname == "input" ) {
             SGPropertyNode *prop = child->getChild( "prop" );
             if ( prop != NULL ) {
@@ -82,6 +92,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 +110,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 +125,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 +238,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 +265,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 +313,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 +325,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;
         }
 
@@ -318,9 +342,18 @@ 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 );
+        // passive_ignore == true means that we go through all the
+        // motions, but drive the outputs.  This is analogous to
+        // running the autopilot with the "servos" off.  This is
+        // helpful for things like flight directors which position
+        // their vbars from the autopilot computations.
+        if ( passive_mode->getBoolValue() && honor_passive ) {
+            // skip output step
+        } else {
+            unsigned int i;
+            for ( i = 0; i < output_list.size(); ++i ) {
+                output_list[i]->setDoubleValue( u_n );
+            }
         }
     } else if ( !enabled ) {
         ep_n  = 0.0;
@@ -607,7 +640,15 @@ FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node)
         } else if ( cname == "debug" ) {
             debug = child->getBoolValue();
         } else if ( cname == "type" ) {
-            filterType = cval;
+            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 ( cname == "input" ) {
             input_prop = fgGetNode( child->getStringValue(), true );
         } else if ( cname == "filter-time" ) {
@@ -623,14 +664,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 {
@@ -645,7 +686,7 @@ void FGDigitalFilter::update(double dt)
          *
          */
 
-        if (filterType == "exponential")
+        if (filterType == exponential)
         {
             double alpha = 1 / ((Tf/dt) + 1);
             output.push_front(alpha * input[0] + 
@@ -656,7 +697,7 @@ void FGDigitalFilter::update(double dt)
             }
             output.resize(1);
         } 
-        else if (filterType == "double-exponential")
+        else if (filterType == doubleExponential)
         {
             double alpha = 1 / ((Tf/dt) + 1);
             output.push_front(alpha * alpha * input[0] + 
@@ -668,7 +709,7 @@ void FGDigitalFilter::update(double dt)
             }
             output.resize(2);
         }
-        else if (filterType == "moving-average")
+        else if (filterType == movingAverage)
         {
             output.push_front(output[0] + 
                               (input[0] - input.back()) / samples);
@@ -678,7 +719,7 @@ void FGDigitalFilter::update(double dt)
             }
             output.resize(1);
         }
-        else if (filterType == "noise-spike")
+        else if (filterType == noiseSpike)
         {
             double maxChange = rateOfChange * dt;
 
@@ -734,7 +775,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,
@@ -873,7 +914,7 @@ static void update_helper( double dt ) {
 
     // Calculate nav1 target heading error normalized to +/- 180.0
     static SGPropertyNode *target_nav1
-        = fgGetNode( "/radios/nav[0]/radials/target-auto-hdg-deg", true );
+        = fgGetNode( "/instrumentation/nav[0]/radials/target-auto-hdg-deg", true );
     static SGPropertyNode *true_nav1
         = fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true );
     static SGPropertyNode *true_track_nav1
@@ -894,7 +935,7 @@ static void update_helper( double dt ) {
     static SGPropertyNode *nav1_course_error
         = fgGetNode( "/autopilot/internal/nav1-course-error", true );
     static SGPropertyNode *nav1_selected_course
-        = fgGetNode( "/radios/nav[0]/radials/selected-deg", true );
+        = fgGetNode( "/instrumentation/nav[0]/radials/selected-deg", true );
 
     diff = nav1_selected_course->getDoubleValue() - ind_hdg->getDoubleValue();
 //    if ( diff < -180.0 ) { diff += 360.0; }