Some autopilot works

- add new <update-interval-secs> 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

diff --git a/src/Autopilot/analogcomponent.hxx b/src/Autopilot/analogcomponent.hxx
index ccbd099b61d206169ae0932d8d064891a6d50ff1..fc658befe287d38fffc9f4bbdac89c3bb6b3a7fa 100644 (file)
--- 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 be3a4f0d90d02704422a990727591a34e7442383..bf000210b9c3c0cc636767c336afc0fe9bb00fce 100644 (file)
--- 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 ccda272e0550059fd64839160c87ac6ccf30ba0f..b24977e560adf1d8178e821dd0acbf4051ee73de 100644 (file)
--- 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 6f83b01e1f6ea48d2b134f1748a572c1033445e2..d73a2585e150d062bc87a70e8042148cb424a26c 100644 (file)
--- 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 maxChange = _rateOfChangeInput.get_value() * dt;
+  double delta = input - _output_1;
+  if( delta == 0.0 ) return input; // trivial
 
-  double output_0 = _output_1;
+  double maxChange = _rateOfChangeInput.get_value() * dt;
+  const PeriodicalValue * periodical = _digitalFilter->getPeriodicalValue();
+  if( periodical ) delta = periodical->normalizeSymmetric( delta );
 
-  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<string,FunctorBase<DigitalFilterImplementation> *> 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 d9b0ac48d8d9500d7ca0af7ca941437e87cd3000..3b7afabbd4d5cf6a9980640b9aeb9f9f6a59b7fa 100644 (file)
--- 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<DigitalFilterImplementation> _implementation;
+    SGSharedPtr<class DigitalFilterImplementation> _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 cbda6651f77e12a5e3c447e9ee1b3674cf9a887b..d097cef9152e95cbcfd31e207b7614313e344d9c 100644 (file)
--- 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 bdf0b6382e3757f88dc2b784cdd45481513d274e..c85bfd74a7f164b72cf55eccc54d5f388286e365 100644 (file)
--- 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;
 };
 
 /**