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);
+ }
}