From 91ae41f9ef5139b7bb9595a67f986dbe2b10be4a Mon Sep 17 00:00:00 2001
From: Thomas Geymayer <tomgey@gmail.com>
Date: Mon, 17 Jun 2013 23:19:46 +0200
Subject: [PATCH] Autopilot: Optionally write (internal) state to property tree

This can be useful eg. for plotting autopilot response
with the Canvas.
---
 src/Autopilot/analogcomponent.hxx    |  3 +++
 src/Autopilot/component.cxx          |  5 ++++
 src/Autopilot/component.hxx          |  5 ++++
 src/Autopilot/digitalfilter.cxx      |  3 +++
 src/Autopilot/pidcontroller.cxx      | 39 +++++++++++++++++++++++-----
 src/Autopilot/pisimplecontroller.cxx |  9 +++++++
 6 files changed, 58 insertions(+), 6 deletions(-)

diff --git a/src/Autopilot/analogcomponent.hxx b/src/Autopilot/analogcomponent.hxx
index fc658befe..7563c27b9 100644
--- a/src/Autopilot/analogcomponent.hxx
+++ b/src/Autopilot/analogcomponent.hxx
@@ -132,6 +132,9 @@ protected:
         for( simgear::PropertyList::iterator it = _output_list.begin();
              it != _output_list.end(); ++it)
           (*it)->setDoubleValue( value );
+
+        if( _debug_node )
+          _debug_node->setDoubleValue("Un", value);
     }
 
 public:
diff --git a/src/Autopilot/component.cxx b/src/Autopilot/component.cxx
index f1c289412..d3aa1b661 100644
--- a/src/Autopilot/component.cxx
+++ b/src/Autopilot/component.cxx
@@ -70,6 +70,11 @@ bool Component::configure( const std::string & nodeName, SGPropertyNode_ptr conf
     return true;
   }
 
+  if ( nodeName == "debug-node" ) {
+    _debug_node = fgGetNode( configNode->getStringValue(), true );
+    return true;
+  }
+
   if ( nodeName == "enable" ) {
     SGPropertyNode_ptr prop;
 
diff --git a/src/Autopilot/component.hxx b/src/Autopilot/component.hxx
index cf8634a4a..7caea8370 100644
--- a/src/Autopilot/component.hxx
+++ b/src/Autopilot/component.hxx
@@ -77,6 +77,11 @@ protected:
      */
     bool _debug;
 
+    /**
+     * @brief property node to write debug values in child nodes on every
+     *        iteration
+     */
+    SGPropertyNode_ptr _debug_node;
 
     /** 
      * @brief a (historic) flag signalling the derived class that it should compute it's internal
diff --git a/src/Autopilot/digitalfilter.cxx b/src/Autopilot/digitalfilter.cxx
index d0e4b9423..06c991ecc 100644
--- a/src/Autopilot/digitalfilter.cxx
+++ b/src/Autopilot/digitalfilter.cxx
@@ -431,4 +431,7 @@ void DigitalFilter::update( bool firstTime, double dt)
     cout << "input:" << input
          << "\toutput:" << output << endl;
   }
+
+  if( _debug_node )
+    _debug_node->setDoubleValue("input", input);
 }
diff --git a/src/Autopilot/pidcontroller.cxx b/src/Autopilot/pidcontroller.cxx
index d41608c09..7438d2707 100644
--- a/src/Autopilot/pidcontroller.cxx
+++ b/src/Autopilot/pidcontroller.cxx
@@ -137,6 +137,16 @@ 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 td = Td.get_value();
         if ( td > 0.0 ) { // do we need to calcluate derivative error?
 
@@ -152,6 +162,13 @@ 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;
         }
@@ -164,13 +181,20 @@ void PIDController::update( bool firstTime, double dt )
                                + ((Ts/ti) * e_n)
                                + ((td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) );
 
-          if ( _debug ) {
+          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 )
+          {
               cout << " delta_u_n = " << delta_u_n << endl;
-              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;
-        }
+              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);
+          }
         }
 
         // Integrator anti-windup logic:
@@ -186,6 +210,9 @@ void PIDController::update( bool firstTime, double dt )
         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;
diff --git a/src/Autopilot/pisimplecontroller.cxx b/src/Autopilot/pisimplecontroller.cxx
index c33260698..c656f127b 100644
--- a/src/Autopilot/pisimplecontroller.cxx
+++ b/src/Autopilot/pisimplecontroller.cxx
@@ -88,4 +88,13 @@ void PISimpleController::update( bool firstTime, double dt )
 
     set_output_value( clamped_output );
     if ( _debug ) cout << "output = " << clamped_output << endl;
+
+    if( _debug_node )
+    {
+      _debug_node->setDoubleValue("y_n", y_n);
+      _debug_node->setDoubleValue("r_n", r_n);
+      _debug_node->setDoubleValue("e_n", error);
+      _debug_node->setDoubleValue("ep_n", prop_comp);
+      _debug_node->setDoubleValue("In", _int_sum);
+    }
 }
-- 
2.39.5