#include "functor.hxx"
#include <deque>
+using std::map;
+using std::string;
+using std::endl;
+using std::cout;
+
namespace FGXMLAutopilot {
/* --------------------------------------------------------------------------------- */
double ReciprocalFilterImplementation::compute( double dt, double input )
{
- if( input >= -SGLimitsd::min() || input <= SGLimitsd::min() )
+ if( input >= -SGLimitsd::min() && input <= SGLimitsd::min() )
return SGLimitsd::max();
return _gainInput.get_value() / input;
double ExponentialFilterImplementation::compute( double dt, double input )
{
input = GainFilterImplementation::compute( dt, input );
+ double tf = _TfInput.get_value();
double output_0;
- double alpha = 1 / ((_TfInput.get_value()/dt) + 1);
+
+ // avoid negative filter times
+ // and div by zero if -tf == dt
+
+ double alpha = tf > 0.0 ? 1 / ((tf/dt) + 1) : 1.0;
if(_isSecondOrder) {
output_0 = alpha * alpha * input +
/* --------------------------------------------------------------------------------- */
DigitalFilter::DigitalFilter() :
- AnalogComponent()
+ AnalogComponent(),
+ _initializeTo(INITIALIZE_INPUT)
{
}
return true;
}
+ if( nodeName == "initialize-to" ) {
+ string s( configNode->getStringValue() );
+ if( s == "input" ) {
+ _initializeTo = INITIALIZE_INPUT;
+ } else if( s == "output" ) {
+ _initializeTo = INITIALIZE_OUTPUT;
+ } else if( s == "none" ) {
+ _initializeTo = INITIALIZE_NONE;
+ } else {
+ SG_LOG( SG_AUTOPILOT, SG_WARN, "unhandled initialize-to value '" << s << "' ignored" );
+ }
+ return true;
+ }
+
SG_LOG( SG_AUTOPILOT, SG_BULK, "DigitalFilter::configure(" << nodeName << ") [unhandled]" << endl );
return false; // not handled by us, let the base class try
}
if( _implementation == NULL ) return;
if( firstTime ) {
- SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
- _implementation->initialize( _valueInput.get_value() );
+ switch( _initializeTo ) {
+
+ case INITIALIZE_INPUT:
+ SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << _valueInput.get_value() );
+ _implementation->initialize( _valueInput.get_value() );
+ break;
+
+ case INITIALIZE_OUTPUT:
+ SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to " << get_output_value() );
+ _implementation->initialize( get_output_value() );
+ break;
+
+ default:
+ SG_LOG(SG_AUTOPILOT,SG_DEBUG, "First time initialization of " << get_name() << " to (uninitialized)" );
+ break;
+ }
}
double input = _valueInput.get_value() - _referenceInput.get_value();