fly/flightgear
1
0
Fork 0
Code Activity

Roy Vegard Ovesen:

Browse source 
I've added some digital filters to the autopilot. They are all low-pass
filters that filter away high frequency signals/noise. There are 4 different
filters:

1. Exponential - The algorithm is essentially the same as the one used in the
   fgGetLowPass() function.

2. Double exponential - Two exponential filters in series. This filter has a
   "steeper" frequency response curve. It filters "better" than the single
   exponential.

3. Moving average - Averages a number of inputs.

4. Noise spike - limits the amount that the output value can change from one
   sample to the next.

Filters 1 and 2 are characterised by it's filter-time in seconds. For filter 3
you have to set the number of input samples to average over. For filter 4 you
set the maximum allowed rate of change as [1/s]. Since the sampling interval
(dt) isn't constant we have to calculate the maximum allowed change for every
update.

Example of a double exponential filter with filter time 0.1 seconds, that is
1/0.1 = 10 Hz.

  <filter>
    <name>pressure-rate-filter</name>
    <debug>true</debug>
    <type>double-exponential</type>
    <input>/autopilot/internal/pressure-rate</input>
    <output>/autopilot/internal/filtered-pressure-rate</output>
    <filter-time>0.1</filter-time>
  </filter>

This would go in the autopilot configuration file.

I've also removed the filtering of the "pressure-rate" helper value, use the
new filters if you want to filter it!  ;-)
This commit is contained in:
ehofman 2004-10-14 09:19:44 +00:00
parent c72e6e281e
commit fa7f2468c2
2 changed files with 168 additions and 14 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

149
src/Autopilot/xmlauto.cxx
View file

@ -23,6 +23,7 @@
#include <simgear/structure/exception.hxx>
#include <simgear/misc/sg_path.hxx>
#include <simgear/sg_inlines.h>
#include <Main/fg_props.hxx>
#include <Main/globals.hxx>
@ -592,6 +593,123 @@ void FGPredictor::update( double dt ) {
}
FGDigitalFilter::FGDigitalFilter(SGPropertyNode *node)
{
samples = 1;
int i;
for ( i = 0; i < node->nChildren(); ++i ) {
SGPropertyNode *child = node->getChild(i);
string cname = child->getName();
string cval = child->getStringValue();
if ( cname == "name" ) {
name = cval;
} else if ( cname == "debug" ) {
debug = child->getBoolValue();
} else if ( cname == "type" ) {
filterType = cval;
} else if ( cname == "input" ) {
input_prop = fgGetNode( child->getStringValue(), true );
} else if ( cname == "filter-time" ) {
Tf = child->getDoubleValue();
} else if ( cname == "samples" ) {
samples = child->getIntValue();
} else if ( cname == "max-rate-of-change" ) {
rateOfChange = child->getDoubleValue();
} else if ( cname == "output" ) {
SGPropertyNode *tmp = fgGetNode( child->getStringValue(), true );
output_list.push_back( tmp );
}
}
output.resize(2, 0.0);
input.resize(samples, 0.0);
}
void FGDigitalFilter::update(double dt)
{
if ( input_prop != NULL ) {
input.push_front(input_prop->getDoubleValue());
input.resize(samples, 0.0);
// no sense if there isn't an input :-)
enabled = true;
} else {
enabled = false;
}
if ( enabled && dt > 0.0 ) {
/*
* Exponential filter
*
* Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1]
*
*/
if (filterType == "exponential")
{
double alpha = 1 / ((Tf/dt) + 1);
output.push_front(alpha * input[0] +
(1 - alpha) * output[0]);
unsigned int i;
for ( i = 0; i < output_list.size(); ++i ) {
output_list[i]->setDoubleValue( output[0] );
}
output.resize(1);
}
else if (filterType == "double-exponential")
{
double alpha = 1 / ((Tf/dt) + 1);
output.push_front(alpha * alpha * input[0] +
2 * (1 - alpha) * output[0] -
(1 - alpha) * (1 - alpha) * output[1]);
unsigned int i;
for ( i = 0; i < output_list.size(); ++i ) {
output_list[i]->setDoubleValue( output[0] );
}
output.resize(2);
}
else if (filterType == "moving-average")
{
output.push_front(output[0] +
(input[0] - input.back()) / samples);
unsigned int i;
for ( i = 0; i < output_list.size(); ++i ) {
output_list[i]->setDoubleValue( output[0] );
}
output.resize(1);
}
else if (filterType == "noise-spike")
{
double maxChange = rateOfChange * dt;
if ((output[0] - input[0]) > maxChange)
{
output.push_front(output[0] - maxChange);
}
else if ((output[0] - input[0]) < -maxChange)
{
output.push_front(output[0] + maxChange);
}
else if (fabs(input[0] - output[0]) <= maxChange)
{
output.push_front(input[0]);
}
unsigned int i;
for ( i = 0; i < output_list.size(); ++i ) {
output_list[i]->setDoubleValue( output[0] );
}
output.resize(1);
}
if (debug)
{
cout << "input:" << input[0]
<< "\toutput:" << output[0] << endl;
}
}
}
FGXMLAutopilot::FGXMLAutopilot() {
}
@ -666,6 +784,9 @@ bool FGXMLAutopilot::build() {
} else if ( name == "predict-simple" ) {
FGXMLAutoComponent *c = new FGPredictor( node );
components.push_back( c );
} else if ( name == "filter" ) {
FGXMLAutoComponent *c = new FGDigitalFilter( node );
components.push_back( c );
} else {
SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: "
<< name );
@ -768,6 +889,19 @@ static void update_helper( double dt ) {
if ( diff > 180.0 ) { diff -= 360.0; }
true_track_nav1->setDoubleValue( diff );
// Calculate nav1 selected course error normalized to +/- 180.0
// (based on DG indicated heading)
static SGPropertyNode *nav1_course_error
= fgGetNode( "/autopilot/internal/nav1-course-error", true );
static SGPropertyNode *nav1_selected_course
= fgGetNode( "/radios/nav[0]/radials/selected-deg", true );
diff = nav1_selected_course->getDoubleValue() - ind_hdg->getDoubleValue();
// if ( diff < -180.0 ) { diff += 360.0; }
// if ( diff > 180.0 ) { diff -= 360.0; }
SG_NORMALIZE_RANGE( diff, -180.0, 180.0 );
nav1_course_error->setDoubleValue( diff );
// Calculate vertical speed in fpm
static SGPropertyNode *vs_fps
= fgGetNode( "/velocities/vertical-speed-fps", true );
@ -783,31 +917,18 @@ static void update_helper( double dt ) {
= fgGetNode( "/systems/static[0]/pressure-inhg", true );
static SGPropertyNode *pressure_rate
= fgGetNode( "/autopilot/internal/pressure-rate", true );
static SGPropertyNode *filter_time
= fgGetNode( "/autopilot/internal/ft", true );
static double last_static_pressure = 0.0;
static double last_pressure_rate = 0.0;
double Tf = filter_time->getDoubleValue();
if ( dt > 0.0 && Tf > 0.0) {
if ( dt > 0.0 ) {
double current_static_pressure = static_pressure->getDoubleValue();
double current_pressure_rate =
( current_static_pressure - last_static_pressure ) / dt;
double W = dt/Tf;
// Low Pass Filter
current_pressure_rate =
(1.0/(W + 1.0))*last_pressure_rate +
((W)/(W + 1.0))*current_pressure_rate;
pressure_rate->setDoubleValue(current_pressure_rate);
last_static_pressure = current_static_pressure;
last_pressure_rate = current_pressure_rate;
}
}

33
src/Autopilot/xmlauto.hxx
View file

@ -36,9 +36,11 @@
#include STL_STRING
#include <vector>
#include <deque>
SG_USING_STD(string);
SG_USING_STD(vector);
SG_USING_STD(deque);
#include <simgear/props/props.hxx>
#include <simgear/structure/subsystem_mgr.hxx>
@ -208,6 +210,37 @@ public:
};
/**
* FGDigitalFilter - a selection of digital filters
*
* Exponential filter
* Double exponential filter
* Moving average filter
* Noise spike filter
*
* All these filters are low-pass filters.
*
*/
class FGDigitalFilter : public FGXMLAutoComponent
{
private:
double Tf; // Filter time [s]
unsigned int samples; // Number of input samples to average
double rateOfChange; // The maximum allowable rate of change [1/s]
deque <double> output;
deque <double> input;
string filterType;
bool debug;
public:
FGDigitalFilter(SGPropertyNode *node);
~FGDigitalFilter() {}
void update(double dt);
};
/**
* Model an autopilot system.
*