FlightPlan activation, delegate hook.

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

diff --git a/src/Autopilot/pidcontroller.cxx b/src/Autopilot/pidcontroller.cxx
index 7438d2707460db03f938d5a5d99ef73b5403ed43..4f71fd574f2d4d3614e05d558f3e8aecfc6f6418 100644 (file)
--- a/src/Autopilot/pidcontroller.cxx
+++ b/src/Autopilot/pidcontroller.cxx
@@ -95,8 +95,13 @@ PIDController::PIDController():
 
 void PIDController::update( bool firstTime, double dt ) 
 {
-    double edf_n = 0.0;
-    double u_n = 0.0;       // absolute output
+    if( firstTime ) {
+      ep_n_1 = 0.0;
+      edf_n_2 = edf_n_1 = 0.0;
+
+      // first time being enabled, seed with current property tree value
+      u_n_1 = get_output_value();
+    }
 
     double u_min = _minInput.get_value();
     double u_max = _maxInput.get_value();
@@ -110,15 +115,6 @@ void PIDController::update( bool firstTime, double dt )
     double Ts = elapsedTime; // sampling interval (sec)
     elapsedTime = 0.0;
 
-    if( firstTime ) {
-      // first time being enabled, seed u_n with current
-      // property tree value
-      ep_n_1 = 0.0;
-      edf_n_2 = edf_n_1 = edf_n = 0.0;
-      u_n = get_output_value();
-      u_n_1 = u_n;
-    }
-
     if( Ts > SGLimitsd::min()) {
         if( _debug ) cout <<  "Updating " << get_name()
                           << " Ts " << Ts << endl;
@@ -137,16 +133,7 @@ void PIDController::update( bool firstTime, double dt )
         double e_n = r_n - y_n;
         if ( _debug ) cout << " e_n = " << e_n;
 
-        if( _debug_node )
-        {
-          _debug_node->setDoubleValue("Ts", Ts);
-          _debug_node->setDoubleValue("y_n", y_n);
-          _debug_node->setDoubleValue("r_n", r_n);
-          _debug_node->setDoubleValue("ep_n", ep_n);
-          _debug_node->setDoubleValue("ep_n_1", ep_n_1);
-          _debug_node->setDoubleValue("e_n", e_n);
-        }
-
+        double edf_n = 0.0;
         double td = Td.get_value();
         if ( td > 0.0 ) { // do we need to calcluate derivative error?
 
@@ -162,13 +149,6 @@ void PIDController::update( bool firstTime, double dt )
             edf_n = edf_n_1 / (Ts/Tf + 1)
                 + ed_n * (Ts/Tf) / (Ts/Tf + 1);
             if ( _debug ) cout << " edf_n = " << edf_n;
-
-            if( _debug_node )
-            {
-              _debug_node->setDoubleValue("ed_n", ed_n);
-              _debug_node->setDoubleValue("Tf", Tf);
-              _debug_node->setDoubleValue("edf_n", edf_n);
-            }
         } else {
             edf_n_2 = edf_n_1 = edf_n = 0.0;
         }
@@ -181,20 +161,13 @@ void PIDController::update( bool firstTime, double dt )
                                + ((Ts/ti) * e_n)
                                + ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
 
-          double Pn = Kp.get_value() * (ep_n - ep_n_1),
-                 In = Kp.get_value() * ((Ts/ti) * e_n),
-                 Dn = Kp.get_value() * ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2));
-          if ( _debug )
-          {
+          if ( _debug ) {
               cout << " delta_u_n = " << delta_u_n << endl;
-              cout << "P:" << Pn << " I:" << In << " D:" << Dn << endl;
-          }
-          if( _debug_node )
-          {
-            _debug_node->setDoubleValue("Pn", Pn);
-            _debug_node->setDoubleValue("In", In);
-            _debug_node->setDoubleValue("Dn", Dn);
-          }
+              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:
@@ -207,12 +180,9 @@ void PIDController::update( bool firstTime, double dt )
         }
 
         // Calculates absolute output:
-        u_n = u_n_1 + delta_u_n;
+        double u_n = u_n_1 + delta_u_n;
         if ( _debug ) cout << "  output = " << u_n << endl;
 
-        if( _debug_node )
-          _debug_node->setDoubleValue("dU", delta_u_n);
-
         // Updates indexed values;
         u_n_1   = u_n;
         ep_n_1  = ep_n;
@@ -223,54 +193,50 @@ void PIDController::update( bool firstTime, double dt )
     }
 }
 
-bool PIDController::configure( const std::string & nodeName, SGPropertyNode_ptr configNode )
+//------------------------------------------------------------------------------
+bool PIDController::configure( SGPropertyNode& cfg_node,
+                               const std::string& cfg_name,
+                               SGPropertyNode& prop_root )
 {
-  SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ")" << endl );
-
-  if( AnalogComponent::configure( nodeName, configNode ) )
-    return true;
-
-  if( nodeName == "config" ) {
-    Component::configure( configNode );
+  if( cfg_name == "config" ) {
+    Component::configure(prop_root, cfg_node);
     return true;
   }
 
-  if (nodeName == "Ts") {
-    desiredTs = configNode->getDoubleValue();
+  if (cfg_name == "Ts") {
+    desiredTs = cfg_node.getDoubleValue();
     return true;
   } 
 
-  if (nodeName == "Kp") {
-    Kp.push_back( new InputValue(configNode) );
+  if (cfg_name == "Kp") {
+    Kp.push_back( new InputValue(prop_root, cfg_node) );
     return true;
   } 
 
-  if (nodeName == "Ti") {
-    Ti.push_back( new InputValue(configNode) );
+  if (cfg_name == "Ti") {
+    Ti.push_back( new InputValue(prop_root, cfg_node) );
     return true;
   } 
 
-  if (nodeName == "Td") {
-    Td.push_back( new InputValue(configNode) );
+  if (cfg_name == "Td") {
+    Td.push_back( new InputValue(prop_root, cfg_node) );
     return true;
   } 
 
-  if (nodeName == "beta") {
-    beta = configNode->getDoubleValue();
+  if (cfg_name == "beta") {
+    beta = cfg_node.getDoubleValue();
     return true;
   } 
 
-  if (nodeName == "alpha") {
-    alpha = configNode->getDoubleValue();
+  if (cfg_name == "alpha") {
+    alpha = cfg_node.getDoubleValue();
     return true;
   } 
 
-  if (nodeName == "gamma") {
-    gamma = configNode->getDoubleValue();
+  if (cfg_name == "gamma") {
+    gamma = cfg_node.getDoubleValue();
     return true;
-  } 
+  }
 
-  SG_LOG( SG_AUTOPILOT, SG_BULK, "PIDController::configure(" << nodeName << ") [unhandled]" << endl );
-  return false;
+  return AnalogComponent::configure(cfg_node, cfg_name, prop_root);
 }
-