X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Fpidcontroller.cxx;h=4f71fd574f2d4d3614e05d558f3e8aecfc6f6418;hb=564177933b4225fca89adb4039f3c0bf6e8bc828;hp=1ad027460c1c45a3d7055c87f16e3569357ad9db;hpb=0bce31ae9a80c571b9126427c093b8dc3d3f6820;p=flightgear.git

diff --git a/src/Autopilot/pidcontroller.cxx b/src/Autopilot/pidcontroller.cxx
index 1ad027460..4f71fd574 100644
--- a/src/Autopilot/pidcontroller.cxx
+++ b/src/Autopilot/pidcontroller.cxx
@@ -25,6 +25,9 @@
 
 using namespace FGXMLAutopilot;
 
+using std::endl;
+using std::cout;
+
 PIDController::PIDController():
     AnalogComponent(),
     alpha( 0.1 ),
@@ -92,9 +95,13 @@ PIDController::PIDController():
 
 void PIDController::update( bool firstTime, double dt ) 
 {
-    double edf_n = 0.0;
-    double delta_u_n = 0.0; // incremental output
-    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();
@@ -108,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;
@@ -135,6 +133,7 @@ void PIDController::update( bool firstTime, double dt )
         double e_n = r_n - y_n;
         if ( _debug ) cout << " 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?
 
@@ -156,6 +155,7 @@ void PIDController::update( bool firstTime, double dt )
 
         // Calculates the incremental output:
         double ti = Ti.get_value();
+        double delta_u_n = 0.0; // incremental output
         if ( ti > 0.0 ) {
             delta_u_n = Kp.get_value() * ( (ep_n - ep_n_1)
                                + ((Ts/ti) * e_n)
@@ -180,7 +180,7 @@ 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;
 
         // Updates indexed values;
@@ -193,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);
 }
-