From 5ebdcdc3d5b447e9be68b19349ef7ab09340e0bb Mon Sep 17 00:00:00 2001 From: Torsten Dreyer Date: Sun, 12 Feb 2012 11:41:21 +0100 Subject: [PATCH] Some autopilot works - add new for autopilot components. This does what it says. - add new method to PeriodicalValue to normalize symmetric around zero - move the DigitalFilterImplementation out of the header file - refactor NoiseSpikeFilter: spare some cpu-cycles and respect periodical output --- src/Autopilot/analogcomponent.hxx | 3 ++ src/Autopilot/autopilot.cxx | 10 ++++--- src/Autopilot/autopilot.hxx | 2 +- src/Autopilot/digitalfilter.cxx | 49 ++++++++++++++++++++++++------- src/Autopilot/digitalfilter.hxx | 17 ++--------- src/Autopilot/inputvalue.cxx | 9 +++++- src/Autopilot/inputvalue.hxx | 3 +- 7 files changed, 60 insertions(+), 33 deletions(-) diff --git a/src/Autopilot/analogcomponent.hxx b/src/Autopilot/analogcomponent.hxx index ccbd099b6..fc658befe 100644 --- a/src/Autopilot/analogcomponent.hxx +++ b/src/Autopilot/analogcomponent.hxx @@ -133,6 +133,9 @@ protected: it != _output_list.end(); ++it) (*it)->setDoubleValue( value ); } + +public: + const PeriodicalValue * getPeriodicalValue() const { return _periodical; } }; inline void AnalogComponent::disabled( double dt ) diff --git a/src/Autopilot/autopilot.cxx b/src/Autopilot/autopilot.cxx index be3a4f0d9..bf000210b 100644 --- a/src/Autopilot/autopilot.cxx +++ b/src/Autopilot/autopilot.cxx @@ -85,8 +85,10 @@ Autopilot::Autopilot( SGPropertyNode_ptr rootNode, SGPropertyNode_ptr configNode component->set_name( buf.str() ); } - SG_LOG( SG_AUTOPILOT, SG_INFO, "adding autopilot component \"" << childName << "\" as \"" << component->get_name() << "\"" ); - add_component(component); + double updateInterval = node->getDoubleValue( "update-interval-secs", 0.0 ); + + SG_LOG( SG_AUTOPILOT, SG_INFO, "adding autopilot component \"" << childName << "\" as \"" << component->get_name() << "\" with interval=" << updateInterval ); + add_component(component,updateInterval); } } @@ -105,7 +107,7 @@ void Autopilot::unbind() _rootNode->untie( "serviceable" ); } -void Autopilot::add_component( Component * component ) +void Autopilot::add_component( Component * component, double updateInterval ) { if( component == NULL ) return; @@ -119,7 +121,7 @@ void Autopilot::add_component( Component * component ) if( name != component->get_name() ) SG_LOG( SG_ALL, SG_WARN, "Duplicate autopilot component " << component->get_name() << ", renamed to " << name ); - set_subsystem( name.c_str(), component ); + set_subsystem( name.c_str(), component, updateInterval ); } void Autopilot::update( double dt ) diff --git a/src/Autopilot/autopilot.hxx b/src/Autopilot/autopilot.hxx index ccda272e0..b24977e56 100644 --- a/src/Autopilot/autopilot.hxx +++ b/src/Autopilot/autopilot.hxx @@ -54,7 +54,7 @@ public: std::string get_name() const { return _name; } void set_name( const std::string & name ) { _name = name; } - void add_component( Component * component ); + void add_component( Component * component, double updateInterval ); protected: diff --git a/src/Autopilot/digitalfilter.cxx b/src/Autopilot/digitalfilter.cxx index 6f83b01e1..d73a2585e 100644 --- a/src/Autopilot/digitalfilter.cxx +++ b/src/Autopilot/digitalfilter.cxx @@ -32,6 +32,25 @@ using std::cout; namespace FGXMLAutopilot { +/** + * + * + */ +class DigitalFilterImplementation : public SGReferenced { +protected: + virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode) = 0; +public: + DigitalFilterImplementation(); + virtual void initialize( double output ) {} + virtual double compute( double dt, double input ) = 0; + bool configure( SGPropertyNode_ptr configNode ); + + void setDigitalFilter( DigitalFilter * digitalFilter ) { _digitalFilter = digitalFilter; } + +protected: + DigitalFilter * _digitalFilter; +}; + /* --------------------------------------------------------------------------------- */ /* --------------------------------------------------------------------------------- */ class GainFilterImplementation : public DigitalFilterImplementation { @@ -101,6 +120,10 @@ using namespace FGXMLAutopilot; /* --------------------------------------------------------------------------------- */ /* --------------------------------------------------------------------------------- */ +DigitalFilterImplementation::DigitalFilterImplementation() : + _digitalFilter(NULL) +{ +} bool DigitalFilterImplementation::configure( SGPropertyNode_ptr configNode ) { @@ -227,19 +250,17 @@ void NoiseSpikeFilterImplementation::initialize( double output ) double NoiseSpikeFilterImplementation::compute( double dt, double input ) { + double delta = input - _output_1; + if( delta == 0.0 ) return input; // trivial + double maxChange = _rateOfChangeInput.get_value() * dt; + const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue(); + if( periodical ) delta = periodical->normalizeSymmetric( delta ); - double output_0 = _output_1; - - if (_output_1 - input > maxChange) { - output_0 = _output_1 - maxChange; - } else if( _output_1 - input < -maxChange ) { - output_0 = _output_1 + maxChange; - } else if (fabs(input - _output_1) <= maxChange) { - output_0 = input; - } - _output_1 = output_0; - return output_0; + if( fabs(delta) <= maxChange ) + return (_output_1 = input); + else + return (_output_1 = _output_1 + copysign( maxChange, delta )); } bool NoiseSpikeFilterImplementation::configure( const std::string & nodeName, SGPropertyNode_ptr configNode ) @@ -318,6 +339,11 @@ DigitalFilter::DigitalFilter() : { } +DigitalFilter::~DigitalFilter() +{ +} + + static map *> componentForge; bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configNode) @@ -343,6 +369,7 @@ bool DigitalFilter::configure(const string& nodeName, SGPropertyNode_ptr configN return true; } _implementation = (*componentForge[type])( configNode->getParent() ); + _implementation->setDigitalFilter( this ); return true; } diff --git a/src/Autopilot/digitalfilter.hxx b/src/Autopilot/digitalfilter.hxx index d9b0ac48d..3b7afabbd 100644 --- a/src/Autopilot/digitalfilter.hxx +++ b/src/Autopilot/digitalfilter.hxx @@ -31,19 +31,6 @@ namespace FGXMLAutopilot { -/** - * - * - */ -class DigitalFilterImplementation : public SGReferenced { -protected: - virtual bool configure( const std::string & nodeName, SGPropertyNode_ptr configNode) = 0; -public: - virtual void initialize( double output ) {} - virtual double compute( double dt, double input ) = 0; - bool configure( SGPropertyNode_ptr configNode ); -}; - /** * brief@ DigitalFilter - a selection of digital filters * @@ -51,7 +38,7 @@ public: class DigitalFilter : public AnalogComponent { private: - SGSharedPtr _implementation; + SGSharedPtr _implementation; enum InitializeTo { INITIALIZE_OUTPUT, @@ -71,7 +58,7 @@ protected: public: DigitalFilter(); - ~DigitalFilter() {} + ~DigitalFilter(); }; diff --git a/src/Autopilot/inputvalue.cxx b/src/Autopilot/inputvalue.cxx index cbda6651f..d097cef91 100644 --- a/src/Autopilot/inputvalue.cxx +++ b/src/Autopilot/inputvalue.cxx @@ -34,11 +34,18 @@ PeriodicalValue::PeriodicalValue( SGPropertyNode_ptr root ) } } -double PeriodicalValue::normalize( double value ) +double PeriodicalValue::normalize( double value ) const { return SGMiscd::normalizePeriodic( minPeriod->get_value(), maxPeriod->get_value(), value ); } +double PeriodicalValue::normalizeSymmetric( double value ) const +{ + value = SGMiscd::normalizePeriodic( minPeriod->get_value(), maxPeriod->get_value(), value ); + double width_2 = (maxPeriod->get_value() - minPeriod->get_value())/2; + return value > width_2 ? width_2 - value : value; +} + InputValue::InputValue( SGPropertyNode_ptr node, double value, double offset, double scale) : _value(0.0), _abs(false) diff --git a/src/Autopilot/inputvalue.hxx b/src/Autopilot/inputvalue.hxx index bdf0b6382..c85bfd74a 100644 --- a/src/Autopilot/inputvalue.hxx +++ b/src/Autopilot/inputvalue.hxx @@ -45,7 +45,8 @@ private: InputValue_ptr maxPeriod; // The maximum value of the period public: PeriodicalValue( SGPropertyNode_ptr node ); - double normalize( double value ); + double normalize( double value ) const; + double normalizeSymmetric( double value ) const; }; /**