ignore resets for now because every z/Z key press would trigger a call to NOAA. We...

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

diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx
index bf37d442c4d3172c1e78b48b83f9396ac3dc6a2c..d682f2b790a24ddcdc40274d2d2d51708c587f2f 100644 (file)
--- a/src/Autopilot/xmlauto.cxx
+++ b/src/Autopilot/xmlauto.cxx
@@ -212,6 +212,15 @@ void FGPIDController::update( double dt ) {
     }
 
     if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) {
+        if ( !enabled ) {
+            // first time being enabled, seed u_n with current
+            // property tree value
+            u_n = output_list[0]->getDoubleValue();
+            // and clip
+            if ( u_n < u_min ) { u_n = u_min; }
+            if ( u_n > u_max ) { u_n = u_max; }
+            u_n_1 = u_n;
+        }
         enabled = true;
     } else {
         enabled = false;
@@ -296,13 +305,13 @@ void FGPIDController::update( double dt ) {
 }
 
 
-FGSimplePIController::FGSimplePIController( SGPropertyNode *node ):
+FGPISimpleController::FGPISimpleController( SGPropertyNode *node ):
     proportional( false ),
-    factor( 0.0 ),
+    Kp( 0.0 ),
     offset_prop( NULL ),
     offset_value( 0.0 ),
     integral( false ),
-    gain( 0.0 ),
+    Ki( 0.0 ),
     int_sum( 0.0 ),
     clamp( false ),
     debug( false ),
@@ -318,6 +327,8 @@ FGSimplePIController::FGSimplePIController( SGPropertyNode *node ):
         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" );
@@ -331,8 +342,6 @@ FGSimplePIController::FGSimplePIController( SGPropertyNode *node ):
             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 ) {
@@ -345,7 +354,7 @@ FGSimplePIController::FGSimplePIController( SGPropertyNode *node ):
             } else {
                 prop = child->getChild( "value" );
                 if ( prop != NULL ) {
-                    r_n_value = prop->getDoubleValue();
+                    r_n = prop->getDoubleValue();
                 }
             }
         } else if ( cname == "output" ) {
@@ -356,64 +365,43 @@ FGSimplePIController::FGSimplePIController( SGPropertyNode *node ):
                 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;
-                }
+        } else if ( cname == "config" ) {
+            SGPropertyNode *prop;
 
-                tmp = prop->getChild( "max" );
-                if ( tmp != NULL ) {
-                    u_max = tmp->getDoubleValue();
-                    // cout << "max = " << u_max << endl;
-                }
+            prop = child->getChild( "Kp" );
+            if ( prop != NULL ) {
+                Kp = prop->getDoubleValue();
+                proportional = true;
             }
-        } else if ( cname == "proportional" ) {
-            proportional = true;
-
-            SGPropertyNode *prop;
 
-            prop = child->getChild( "factor" );
+            prop = child->getChild( "Ki" );
             if ( prop != NULL ) {
-                factor = prop->getDoubleValue();
+                Ki = prop->getDoubleValue();
+                integral = true;
             }
 
-            prop = child->getChild( "offset" );
+            prop = child->getChild( "u_min" );
             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;
-                    }
-                }
+                u_min = prop->getDoubleValue();
+                clamp = true;
             }
-        } else if ( cname == "integral" ) {
-            integral = true;
 
-            SGPropertyNode *prop;
-            prop = child->getChild( "gain" );
+            prop = child->getChild( "u_max" );
             if ( prop != NULL ) {
-                gain = prop->getDoubleValue();
+                u_max = prop->getDoubleValue();
+                clamp = true;
             }
         } else {
             SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" );
+            if ( name.length() ) {
+                SG_LOG( SG_AUTOPILOT, SG_WARN, "Section = " << name );
+            }
         }
     }   
 }
 
 
-void FGSimplePIController::update( double dt ) {
+void FGPISimpleController::update( double dt ) {
     if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) {
         if ( !enabled ) {
             // we have just been enabled, zero out int_sum
@@ -454,11 +442,11 @@ void FGSimplePIController::update( double dt ) {
         }
 
         if ( proportional ) {
-            prop_comp = error * factor + offset;
+            prop_comp = error * Kp + offset;
         }
 
         if ( integral ) {
-            int_sum += error * gain * dt;
+            int_sum += error * Ki * dt;
         } else {
             int_sum = 0.0;
         }
@@ -554,6 +542,9 @@ bool FGXMLAutopilot::build() {
         if ( name == "pid-controller" ) {
             FGXMLAutoComponent *c = new FGPIDController( node );
             components.push_back( c );
+        } else if ( name == "pi-simple-controller" ) {
+            FGXMLAutoComponent *c = new FGPISimpleController( node );
+            components.push_back( c );
         } else {
             SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: " 
                     << name );
@@ -646,6 +637,14 @@ static void update_helper( double dt ) {
     if ( diff < -180.0 ) { diff += 360.0; }
     if ( diff > 180.0 ) { diff -= 360.0; }
     true_nav1->setDoubleValue( diff );
+
+    // Calculate vertical speed in fpm
+    static SGPropertyNode *vs_fps
+        = fgGetNode( "/velocities/vertical-speed-fps", true );
+    static SGPropertyNode *vs_fpm
+        = fgGetNode( "/autopilot/internal/vert-speed-fpm", true );
+
+    vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );  
 }