fly/flightgear
1
0
Fork 0
Code Activity

Merge branch 'torsten/auto'

Browse source
This commit is contained in:
Tim Moore 2010-02-10 00:41:44 +01:00
commit 3e0489bd91
4 changed files with 379 additions and 255 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

19
docs-mini/README.digitalfilters
View file

@ -27,6 +27,10 @@ The complete XML syntax for a InputValue is
<max>infinity</max> <max>infinity</max>
<min>-infinity<min> <min>-infinity<min>
<abs>false</abs> <abs>false</abs>
<period>
<min>-180.0</min>
<max>-180.0</max>
</period>
</some-element> </some-element>
The enclosing element <some-element> is the element defined in each filter, like <input>, <u_min>, The enclosing element <some-element> is the element defined in each filter, like <input>, <u_min>,
@ -124,7 +128,7 @@ other computations are completed.
OutputValue OutputValue
============================================================================== ==============================================================================
Each filter drives one to many output properties. No scaling of offsetting is implemented Each filter drives one to many output properties. No scaling or offsetting is implemented
for the output value, these should be done in the filter itself. for the output value, these should be done in the filter itself.
The output properties are defined in the <output/> element by adding <property/> elements The output properties are defined in the <output/> element by adding <property/> elements
within the <output/> element. For just a single output property, the <property/> element within the <output/> element. For just a single output property, the <property/> element
@ -167,6 +171,8 @@ Example:
min or max value defaults to 0 (zero). min or max value defaults to 0 (zero).
Note: <u_min> and <u_max> may also occour within a <config> element. Note: <u_min> and <u_max> may also occour within a <config> element.
<min> and <max> may be used as a substitude for the corresponding u_xxx element. <min> and <max> may be used as a substitude for the corresponding u_xxx element.
<period> Complex Define a periodical input or output value. The phase width is defined by the
child elements <min> and <max> which are of type InputValue
Example: Limit the pilot's body temperature to a constant minimum of 36 and a maximum defined in Example: Limit the pilot's body temperature to a constant minimum of 36 and a maximum defined in
/pilots/max-body-temperature-degc, initialized to 40.0 /pilots/max-body-temperature-degc, initialized to 40.0
@ -183,6 +189,17 @@ Implicit definition of the minimum value of 0 (zero) and defining a maximum of 1
<u_max>100.0</u_max> <u_max>100.0</u_max>
</config> </config>
This defines the input or output as a periodic value with a phase width of 360, like
the compass rose. Any value reaching the filter's input or leaving the filter at the
output will be transformed to fit into the given range by adding or substracting one phase
width of 360. Values of -270, 90 or 450 applied to this periodical element will allways
result in +90. A value of 630, 270 or -90 will be normalized to -90 in the given example.
<period>
<min>-180.0</min>
<max>180.0</max>
</period>
<enable> Complex Define a condition to enable or disable the filter. For disabled <enable> Complex Define a condition to enable or disable the filter. For disabled
filters, no output computations are performed. Only enabled filters, no output computations are performed. Only enabled
filters fill the output properties. The default for undefined filters fill the output properties. The default for undefined

538
src/Autopilot/xmlauto.cxx
View file

@ -40,14 +40,42 @@
using std::cout; using std::cout;
using std::endl; using std::endl;
FGPeriodicalValue::FGPeriodicalValue( SGPropertyNode_ptr root )
{
SGPropertyNode_ptr minNode = root->getChild( "min" );
SGPropertyNode_ptr maxNode = root->getChild( "max" );
if( minNode == NULL || maxNode == NULL ) {
SG_LOG(SG_AUTOPILOT, SG_ALERT, "periodical defined, but no <min> and/or <max> tag. Period ignored." );
} else {
minPeriod = new FGXMLAutoInput( minNode );
maxPeriod = new FGXMLAutoInput( maxNode );
}
}
double FGPeriodicalValue::normalize( double value )
{
if( !(minPeriod && maxPeriod )) return value;
double p1 = minPeriod->get_value();
double p2 = maxPeriod->get_value();
double min = std::min<double>(p1,p2);
double max = std::max<double>(p1,p2);
double phase = fabs(max - min);
if( phase > SGLimitsd::min() ) {
while( value < min ) value += phase;
while( value >= max ) value -= phase;
} else {
value = min; // phase is zero
}
return value;
}
FGXMLAutoInput::FGXMLAutoInput( SGPropertyNode_ptr node, double value, double offset, double scale) : FGXMLAutoInput::FGXMLAutoInput( SGPropertyNode_ptr node, double value, double offset, double scale) :
value(0.0), value(0.0),
abs(false), abs(false),
property(NULL),
offset(NULL),
scale(NULL),
min(NULL),
max(NULL),
_condition(NULL) _condition(NULL)
{ {
parse( node, value, offset, scale ); parse( node, value, offset, scale );
@ -62,6 +90,7 @@ void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffs
scale = NULL; scale = NULL;
min = NULL; min = NULL;
max = NULL; max = NULL;
periodical = NULL;
if( node == NULL ) if( node == NULL )
return; return;
@ -69,7 +98,7 @@ void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffs
SGPropertyNode * n; SGPropertyNode * n;
if( (n = node->getChild("condition")) != NULL ) { if( (n = node->getChild("condition")) != NULL ) {
_condition = sgReadCondition(node, n); _condition = sgReadCondition(fgGetNode("/"), n);
} }
if( (n = node->getChild( "scale" )) != NULL ) { if( (n = node->getChild( "scale" )) != NULL ) {
@ -92,6 +121,10 @@ void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffs
abs = n->getBoolValue(); abs = n->getBoolValue();
} }
if( (n = node->getChild( "period" )) != NULL ) {
periodical = new FGPeriodicalValue( n );
}
SGPropertyNode *valueNode = node->getChild( "value" ); SGPropertyNode *valueNode = node->getChild( "value" );
if ( valueNode != NULL ) { if ( valueNode != NULL ) {
value = valueNode->getDoubleValue(); value = valueNode->getDoubleValue();
@ -161,6 +194,10 @@ double FGXMLAutoInput::get_value()
if( value > m ) if( value > m )
value = m; value = m;
} }
if( periodical ) {
value = periodical->normalize( value );
}
return abs ? fabs(value) : value; return abs ? fabs(value) : value;
} }
@ -200,7 +237,7 @@ void FGXMLAutoComponent::parseNode(SGPropertyNode* aNode)
debug = child->getBoolValue(); debug = child->getBoolValue();
} else if ( cname == "enable" ) { } else if ( cname == "enable" ) {
if( (prop = child->getChild("condition")) != NULL ) { if( (prop = child->getChild("condition")) != NULL ) {
_condition = sgReadCondition(child, prop); _condition = sgReadCondition(fgGetNode("/"), prop);
} else { } else {
if ( (prop = child->getChild( "prop" )) != NULL ) { if ( (prop = child->getChild( "prop" )) != NULL ) {
enable_prop = fgGetNode( prop->getStringValue(), true ); enable_prop = fgGetNode( prop->getStringValue(), true );
@ -246,6 +283,8 @@ void FGXMLAutoComponent::parseNode(SGPropertyNode* aNode)
umaxInput.push_back( new FGXMLAutoInput( child ) ); umaxInput.push_back( new FGXMLAutoInput( child ) );
} else if ( cname == "u_max" ) { } else if ( cname == "u_max" ) {
umaxInput.push_back( new FGXMLAutoInput( child ) ); umaxInput.push_back( new FGXMLAutoInput( child ) );
} else if ( cname == "period" ) {
periodical = new FGPeriodicalValue( child );
} else { } else {
SG_LOG(SG_AUTOPILOT, SG_ALERT, "malformed autopilot definition - unrecognized node:" SG_LOG(SG_AUTOPILOT, SG_ALERT, "malformed autopilot definition - unrecognized node:"
<< cname << " in section " << name); << cname << " in section " << name);
@ -314,6 +353,11 @@ void FGXMLAutoComponent::do_feedback_if_disabled()
double FGXMLAutoComponent::clamp( double value ) double FGXMLAutoComponent::clamp( double value )
{ {
//If this is a periodical value, normalize it into our domain
// before clamping
if( periodical )
value = periodical->normalize( value );
// clamp, if either min or max is defined // clamp, if either min or max is defined
if( uminInput.size() + umaxInput.size() > 0 ) { if( uminInput.size() + umaxInput.size() > 0 ) {
double d = umaxInput.get_value( 0.0 ); double d = umaxInput.get_value( 0.0 );
@ -685,6 +729,10 @@ bool FGDigitalFilter::parseNodeHook(const string& aName, SGPropertyNode* aNode)
filterType = gain; filterType = gain;
} else if (val == "reciprocal") { } else if (val == "reciprocal") {
filterType = reciprocal; filterType = reciprocal;
} else if (val == "differential") {
filterType = differential;
// use a constant of two samples for current and previous input value
samplesInput.push_back( new FGXMLAutoInput(NULL, 2.0 ) );
} }
} else if (aName == "filter-time" ) { } else if (aName == "filter-time" ) {
TfInput.push_back( new FGXMLAutoInput( aNode, 1.0 ) ); TfInput.push_back( new FGXMLAutoInput( aNode, 1.0 ) );
@ -724,173 +772,117 @@ void FGDigitalFilter::update(double dt)
do_feedback(); do_feedback();
} }
if ( enabled && dt > 0.0 ) { if ( !enabled || dt < SGLimitsd::min() )
/* return;
* Exponential filter
*
* Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1]
*
*/
if( debug ) cout << "Updating " << get_name()
<< " dt " << dt << endl;
if (filterType == exponential) /*
{ * Exponential filter
double alpha = 1 / ((TfInput.get_value()/dt) + 1); *
output.push_front(alpha * input[0] + * Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1]
(1 - alpha) * output[0]); *
} */
else if (filterType == doubleExponential) if( debug ) cout << "Updating " << get_name()
{ << " dt " << dt << endl;
double alpha = 1 / ((TfInput.get_value()/dt) + 1);
output.push_front(alpha * alpha * input[0] +
2 * (1 - alpha) * output[0] -
(1 - alpha) * (1 - alpha) * output[1]);
}
else if (filterType == movingAverage)
{
output.push_front(output[0] +
(input[0] - input.back()) / samplesInput.get_value());
}
else if (filterType == noiseSpike)
{
double maxChange = rateOfChangeInput.get_value() * dt;
if ((output[0] - input[0]) > maxChange) if (filterType == exponential)
{ {
output.push_front(output[0] - maxChange); double alpha = 1 / ((TfInput.get_value()/dt) + 1);
} output.push_front(alpha * input[0] +
else if ((output[0] - input[0]) < -maxChange) (1 - alpha) * output[0]);
{ }
output.push_front(output[0] + maxChange); else if (filterType == doubleExponential)
} {
else if (fabs(input[0] - output[0]) <= maxChange) double alpha = 1 / ((TfInput.get_value()/dt) + 1);
{ output.push_front(alpha * alpha * input[0] +
output.push_front(input[0]); 2 * (1 - alpha) * output[0] -
} (1 - alpha) * (1 - alpha) * output[1]);
} }
else if (filterType == gain) else if (filterType == movingAverage)
{ {
output[0] = gainInput.get_value() * input[0]; output.push_front(output[0] +
} (input[0] - input.back()) / samplesInput.get_value());
else if (filterType == reciprocal) }
{ else if (filterType == noiseSpike)
if (input[0] != 0.0) { {
output[0] = gainInput.get_value() / input[0]; double maxChange = rateOfChangeInput.get_value() * dt;
}
}
output[0] = clamp(output[0]) ; if ((output[0] - input[0]) > maxChange)
set_output_value( output[0] );
output.resize(2);
if (debug)
{ {
cout << "input:" << input[0] output.push_front(output[0] - maxChange);
<< "\toutput:" << output[0] << endl; }
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]);
} }
} }
} else if (filterType == gain)
{
output[0] = gainInput.get_value() * input[0];
FGXMLAutopilot::FGXMLAutopilot() {
}
FGXMLAutopilot::~FGXMLAutopilot() {
}
void FGXMLAutopilot::init() {
config_props = fgGetNode( "/autopilot/new-config", true );
SGPropertyNode *path_n = fgGetNode("/sim/systems/autopilot/path");
if ( path_n ) {
SGPath config( globals->get_fg_root() );
config.append( path_n->getStringValue() );
SG_LOG( SG_ALL, SG_INFO, "Reading autopilot configuration from "
<< config.str() );
try {
readProperties( config.str(), config_props );
if ( ! build() ) {
SG_LOG( SG_ALL, SG_ALERT,
"Detected an internal inconsistency in the autopilot");
SG_LOG( SG_ALL, SG_ALERT,
" configuration. See earlier errors for" );
SG_LOG( SG_ALL, SG_ALERT,
" details.");
exit(-1);
}
} catch (const sg_exception& e) {
SG_LOG( SG_ALL, SG_ALERT, "Failed to load autopilot configuration: "
<< config.str() << ":" << e.getMessage() );
}
} else {
SG_LOG( SG_ALL, SG_WARN,
"No autopilot configuration specified for this model!");
} }
} else if (filterType == reciprocal)
{
if (input[0] != 0.0) {
void FGXMLAutopilot::reinit() { output[0] = gainInput.get_value() / input[0];
components.clear(); }
init(); }
} else if (filterType == differential)
{
if( dt > SGLimitsd::min() ) {
void FGXMLAutopilot::bind() { output[0] = (input[0]-input[1]) * TfInput.get_value() / dt;
}
void FGXMLAutopilot::unbind() {
}
bool FGXMLAutopilot::build() {
SGPropertyNode *node;
int i;
int count = config_props->nChildren();
for ( i = 0; i < count; ++i ) {
node = config_props->getChild(i);
string name = node->getName();
// cout << name << endl;
if ( name == "pid-controller" ) {
components.push_back( new FGPIDController( node ) );
} else if ( name == "pi-simple-controller" ) {
components.push_back( new FGPISimpleController( node ) );
} else if ( name == "predict-simple" ) {
components.push_back( new FGPredictor( node ) );
} else if ( name == "filter" ) {
components.push_back( new FGDigitalFilter( node ) );
} else {
SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: "
<< name );
return false;
} }
} }
return true; output[0] = clamp(output[0]) ;
set_output_value( output[0] );
output.resize(2);
if (debug)
{
cout << "input:" << input[0]
<< "\toutput:" << output[0] << endl;
}
} }
FGXMLAutopilotGroup::FGXMLAutopilotGroup() :
SGSubsystemGroup(),
average(0.0), // average/filtered prediction
v_last(0.0), // last velocity
last_static_pressure(0.0),
vel(fgGetNode( "/velocities/airspeed-kt", true )),
// Estimate speed in 5,10 seconds
lookahead5(fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true )),
lookahead10(fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true )),
bug(fgGetNode( "/autopilot/settings/heading-bug-deg", true )),
mag_hdg(fgGetNode( "/orientation/heading-magnetic-deg", true )),
bug_error(fgGetNode( "/autopilot/internal/heading-bug-error-deg", true )),
fdm_bug_error(fgGetNode( "/autopilot/internal/fdm-heading-bug-error-deg", true )),
target_true(fgGetNode( "/autopilot/settings/true-heading-deg", true )),
true_hdg(fgGetNode( "/orientation/heading-deg", true )),
true_error(fgGetNode( "/autopilot/internal/true-heading-error-deg", true )),
target_nav1(fgGetNode( "/instrumentation/nav[0]/radials/target-auto-hdg-deg", true )),
true_nav1(fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true )),
true_track_nav1(fgGetNode( "/autopilot/internal/nav1-track-error-deg", true )),
nav1_course_error(fgGetNode( "/autopilot/internal/nav1-course-error", true )),
nav1_selected_course(fgGetNode( "/instrumentation/nav[0]/radials/selected-deg", true )),
vs_fps(fgGetNode( "/velocities/vertical-speed-fps", true )),
vs_fpm(fgGetNode( "/autopilot/internal/vert-speed-fpm", true )),
static_pressure(fgGetNode( "/systems/static[0]/pressure-inhg", true )),
pressure_rate(fgGetNode( "/autopilot/internal/pressure-rate", true )),
track(fgGetNode( "/orientation/track-deg", true ))
{
}
/* void FGXMLAutopilotGroup::update( double dt )
* Update helper values {
*/ // update all configured autopilots
static void update_helper( double dt ) { SGSubsystemGroup::update( dt );
// Estimate speed in 5,10 seconds
static SGPropertyNode_ptr vel = fgGetNode( "/velocities/airspeed-kt", true );
static SGPropertyNode_ptr lookahead5
= fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true );
static SGPropertyNode_ptr lookahead10
= fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true );
static double average = 0.0; // average/filtered prediction
static double v_last = 0.0; // last velocity
// update helper values
double v = vel->getDoubleValue(); double v = vel->getDoubleValue();
double a = 0.0; double a = 0.0;
if ( dt > 0.0 ) { if ( dt > 0.0 ) {
@ -907,119 +899,185 @@ static void update_helper( double dt ) {
v_last = v; v_last = v;
} }
// Calculate heading bug error normalized to +/- 180.0 (based on // Calculate heading bug error normalized to +/- 180.0
// DG indicated heading) double diff = bug->getDoubleValue() - mag_hdg->getDoubleValue();
static SGPropertyNode_ptr bug SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
= fgGetNode( "/autopilot/settings/heading-bug-deg", true ); bug_error->setDoubleValue( diff );
static SGPropertyNode_ptr ind_hdg
= fgGetNode( "/instrumentation/heading-indicator/indicated-heading-deg",
true );
static SGPropertyNode_ptr ind_bug_error
= fgGetNode( "/autopilot/internal/heading-bug-error-deg", true );
double diff = bug->getDoubleValue() - ind_hdg->getDoubleValue();
if ( diff < -180.0 ) { diff += 360.0; }
if ( diff > 180.0 ) { diff -= 360.0; }
ind_bug_error->setDoubleValue( diff );
// Calculate heading bug error normalized to +/- 180.0 (based on
// actual/nodrift magnetic-heading, i.e. a DG slaved to magnetic
// compass.)
static SGPropertyNode_ptr mag_hdg
= fgGetNode( "/orientation/heading-magnetic-deg", true );
static SGPropertyNode_ptr fdm_bug_error
= fgGetNode( "/autopilot/internal/fdm-heading-bug-error-deg", true );
diff = bug->getDoubleValue() - mag_hdg->getDoubleValue();
if ( diff < -180.0 ) { diff += 360.0; }
if ( diff > 180.0 ) { diff -= 360.0; }
fdm_bug_error->setDoubleValue( diff ); fdm_bug_error->setDoubleValue( diff );
// Calculate true heading error normalized to +/- 180.0 // Calculate true heading error normalized to +/- 180.0
static SGPropertyNode_ptr target_true
= fgGetNode( "/autopilot/settings/true-heading-deg", true );
static SGPropertyNode_ptr true_hdg
= fgGetNode( "/orientation/heading-deg", true );
static SGPropertyNode_ptr true_track
= fgGetNode( "/instrumentation/gps/indicated-track-true-deg", true );
static SGPropertyNode_ptr true_error
= fgGetNode( "/autopilot/internal/true-heading-error-deg", true );
diff = target_true->getDoubleValue() - true_hdg->getDoubleValue(); diff = target_true->getDoubleValue() - true_hdg->getDoubleValue();
if ( diff < -180.0 ) { diff += 360.0; } SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
if ( diff > 180.0 ) { diff -= 360.0; }
true_error->setDoubleValue( diff ); true_error->setDoubleValue( diff );
// Calculate nav1 target heading error normalized to +/- 180.0 // Calculate nav1 target heading error normalized to +/- 180.0
static SGPropertyNode_ptr target_nav1
= fgGetNode( "/instrumentation/nav[0]/radials/target-auto-hdg-deg", true );
static SGPropertyNode_ptr true_nav1
= fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true );
static SGPropertyNode_ptr true_track_nav1
= fgGetNode( "/autopilot/internal/nav1-track-error-deg", true );
diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue(); diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue();
if ( diff < -180.0 ) { diff += 360.0; } SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
if ( diff > 180.0 ) { diff -= 360.0; }
true_nav1->setDoubleValue( diff ); true_nav1->setDoubleValue( diff );
diff = target_nav1->getDoubleValue() - true_track->getDoubleValue(); // Calculate true groundtrack
if ( diff < -180.0 ) { diff += 360.0; } diff = target_nav1->getDoubleValue() - track->getDoubleValue();
if ( diff > 180.0 ) { diff -= 360.0; } SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
true_track_nav1->setDoubleValue( diff ); true_track_nav1->setDoubleValue( diff );
// Calculate nav1 selected course error normalized to +/- 180.0 // Calculate nav1 selected course error normalized to +/- 180.0
// (based on DG indicated heading) diff = nav1_selected_course->getDoubleValue() - mag_hdg->getDoubleValue();
static SGPropertyNode_ptr nav1_course_error
= fgGetNode( "/autopilot/internal/nav1-course-error", true );
static SGPropertyNode_ptr nav1_selected_course
= fgGetNode( "/instrumentation/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 ); SG_NORMALIZE_RANGE( diff, -180.0, 180.0 );
nav1_course_error->setDoubleValue( diff ); nav1_course_error->setDoubleValue( diff );
// Calculate vertical speed in fpm // Calculate vertical speed in fpm
static SGPropertyNode_ptr vs_fps
= fgGetNode( "/velocities/vertical-speed-fps", true );
static SGPropertyNode_ptr vs_fpm
= fgGetNode( "/autopilot/internal/vert-speed-fpm", true );
vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 ); vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );
// Calculate static port pressure rate in [inhg/s]. // Calculate static port pressure rate in [inhg/s].
// Used to determine vertical speed. // Used to determine vertical speed.
static SGPropertyNode_ptr static_pressure
= fgGetNode( "/systems/static[0]/pressure-inhg", true );
static SGPropertyNode_ptr pressure_rate
= fgGetNode( "/autopilot/internal/pressure-rate", true );
static double last_static_pressure = 0.0;
if ( dt > 0.0 ) { if ( dt > 0.0 ) {
double current_static_pressure = static_pressure->getDoubleValue(); double current_static_pressure = static_pressure->getDoubleValue();
double current_pressure_rate =
( current_static_pressure - last_static_pressure ) / dt;
double current_pressure_rate = pressure_rate->setDoubleValue(current_pressure_rate);
( current_static_pressure - last_static_pressure ) / dt; last_static_pressure = current_static_pressure;
pressure_rate->setDoubleValue(current_pressure_rate);
last_static_pressure = current_static_pressure;
} }
} }
void FGXMLAutopilotGroup::reinit()
{
for( vector<string>::size_type i = 0; i < _autopilotNames.size(); i++ ) {
FGXMLAutopilot * ap = (FGXMLAutopilot*)get_subsystem( _autopilotNames[i] );
if( ap == NULL ) continue; // ?
remove_subsystem( _autopilotNames[i] );
delete ap;
}
_autopilotNames.clear();
init();
}
void FGXMLAutopilotGroup::init()
{
vector<SGPropertyNode_ptr> autopilotNodes = fgGetNode( "/sim/systems", true )->getChildren("autopilot");
if( autopilotNodes.size() == 0 ) {
SG_LOG( SG_ALL, SG_WARN, "No autopilot configuration specified for this model!");
return;
}
for( vector<SGPropertyNode_ptr>::size_type i = 0; i < autopilotNodes.size(); i++ ) {
SGPropertyNode_ptr pathNode = autopilotNodes[i]->getNode( "path" );
if( pathNode == NULL ) {
SG_LOG( SG_ALL, SG_WARN, "No autopilot configuration file specified for this autopilot!");
continue;
}
string apName;
SGPropertyNode_ptr nameNode = autopilotNodes[i]->getNode( "name" );
if( nameNode != NULL ) {
apName = nameNode->getStringValue();
} else {
std::ostringstream buf;
buf << "unnamed_autopilot_" << i;
apName = buf.str();
}
if( get_subsystem( apName.c_str() ) != NULL ) {
SG_LOG( SG_ALL, SG_ALERT, "Duplicate autopilot configuration name " << apName << " ignored" );
continue;
}
SGPath config( globals->get_fg_root() );
config.append( pathNode->getStringValue() );
SG_LOG( SG_ALL, SG_INFO, "Reading autopilot configuration from " << config.str() );
// FGXMLAutopilot
FGXMLAutopilot * ap = new FGXMLAutopilot;
try {
SGPropertyNode_ptr root = new SGPropertyNode();
readProperties( config.str(), root );
if ( ! ap->build( root ) ) {
SG_LOG( SG_ALL, SG_ALERT,
"Detected an internal inconsistency in the autopilot configuration." << endl << " See earlier errors for details." );
delete ap;
continue;
}
} catch (const sg_exception& e) {
SG_LOG( SG_ALL, SG_ALERT, "Failed to load autopilot configuration: "
<< config.str() << ":" << e.getMessage() );
delete ap;
continue;
}
SG_LOG( SG_ALL, SG_INFO, "adding autopilot subsystem " << apName );
set_subsystem( apName, ap );
_autopilotNames.push_back( apName );
}
SGSubsystemGroup::init();
}
FGXMLAutopilot::FGXMLAutopilot() {
}
FGXMLAutopilot::~FGXMLAutopilot() {
}
/* read all /sim/systems/autopilot[n]/path properties, try to read the file specified therein
* and configure/add the digital filters specified in that file
*/
void FGXMLAutopilot::init()
{
}
void FGXMLAutopilot::reinit() {
components.clear();
init();
}
void FGXMLAutopilot::bind() {
}
void FGXMLAutopilot::unbind() {
}
bool FGXMLAutopilot::build( SGPropertyNode_ptr config_props ) {
SGPropertyNode *node;
int i;
int count = config_props->nChildren();
for ( i = 0; i < count; ++i ) {
node = config_props->getChild(i);
string name = node->getName();
// cout << name << endl;
SG_LOG( SG_ALL, SG_INFO, "adding autopilot component " << name );
if ( name == "pid-controller" ) {
components.push_back( new FGPIDController( node ) );
} else if ( name == "pi-simple-controller" ) {
components.push_back( new FGPISimpleController( node ) );
} else if ( name == "predict-simple" ) {
components.push_back( new FGPredictor( node ) );
} else if ( name == "filter" ) {
components.push_back( new FGDigitalFilter( node ) );
// } else {
// SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: "
// << name );
// return false;
}
}
return true;
}
/* /*
* Update the list of autopilot components * Update the list of autopilot components
*/ */
void FGXMLAutopilot::update( double dt ) { void FGXMLAutopilot::update( double dt )
update_helper( dt ); {
unsigned int i; unsigned int i;
for ( i = 0; i < components.size(); ++i ) { for ( i = 0; i < components.size(); ++i ) {
components[i]->update( dt ); components[i]->update( dt );

75
src/Autopilot/xmlauto.hxx
View file

@ -34,16 +34,28 @@
#include <simgear/structure/subsystem_mgr.hxx> #include <simgear/structure/subsystem_mgr.hxx>
#include <simgear/props/condition.hxx> #include <simgear/props/condition.hxx>
typedef SGSharedPtr<class FGXMLAutoInput> FGXMLAutoInput_ptr;
typedef SGSharedPtr<class FGPeriodicalValue> FGPeriodicalValue_ptr;
class FGPeriodicalValue : public SGReferenced {
private:
FGXMLAutoInput_ptr minPeriod; // The minimum value of the period
FGXMLAutoInput_ptr maxPeriod; // The maximum value of the period
public:
FGPeriodicalValue( SGPropertyNode_ptr node );
double normalize( double value );
};
class FGXMLAutoInput : public SGReferenced { class FGXMLAutoInput : public SGReferenced {
private: private:
double value; // The value as a constant or initializer for the property double value; // The value as a constant or initializer for the property
bool abs; // return absolute value bool abs; // return absolute value
SGPropertyNode_ptr property; // The name of the property containing the value SGPropertyNode_ptr property; // The name of the property containing the value
SGSharedPtr<FGXMLAutoInput> offset; // A fixed offset, defaults to zero FGXMLAutoInput_ptr offset; // A fixed offset, defaults to zero
SGSharedPtr<FGXMLAutoInput> scale; // A constant scaling factor defaults to one FGXMLAutoInput_ptr scale; // A constant scaling factor defaults to one
SGSharedPtr<FGXMLAutoInput> min; // A minimum clip defaults to no clipping FGXMLAutoInput_ptr min; // A minimum clip defaults to no clipping
SGSharedPtr<FGXMLAutoInput> max; // A maximum clip defaults to no clipping FGXMLAutoInput_ptr max; // A maximum clip defaults to no clipping
FGPeriodicalValue_ptr periodical; //
SGSharedPtr<const SGCondition> _condition; SGSharedPtr<const SGCondition> _condition;
public: public:
@ -71,9 +83,9 @@ public:
}; };
class FGXMLAutoInputList : public std::vector<SGSharedPtr<FGXMLAutoInput> > { class FGXMLAutoInputList : public std::vector<FGXMLAutoInput_ptr> {
public: public:
FGXMLAutoInput * get_active() { FGXMLAutoInput_ptr get_active() {
for (iterator it = begin(); it != end(); ++it) { for (iterator it = begin(); it != end(); ++it) {
if( (*it)->is_enabled() ) if( (*it)->is_enabled() )
return *it; return *it;
@ -82,7 +94,7 @@ class FGXMLAutoInputList : public std::vector<SGSharedPtr<FGXMLAutoInput> > {
} }
double get_value( double def = 0.0 ) { double get_value( double def = 0.0 ) {
FGXMLAutoInput * input = get_active(); FGXMLAutoInput_ptr input = get_active();
return input == NULL ? def : input->get_value(); return input == NULL ? def : input->get_value();
} }
@ -147,6 +159,7 @@ protected:
FGXMLAutoInputList referenceInput; FGXMLAutoInputList referenceInput;
FGXMLAutoInputList uminInput; FGXMLAutoInputList uminInput;
FGXMLAutoInputList umaxInput; FGXMLAutoInputList umaxInput;
FGPeriodicalValue_ptr periodical;
// debug flag // debug flag
bool debug; bool debug;
bool enabled; bool enabled;
@ -221,6 +234,8 @@ public:
bool isPropertyEnabled(); bool isPropertyEnabled();
}; };
typedef SGSharedPtr<FGXMLAutoComponent> FGXMLAutoComponent_ptr;
/** /**
* Roy Ovesen's PID controller * Roy Ovesen's PID controller
@ -339,7 +354,7 @@ private:
std::deque <double> output; std::deque <double> output;
std::deque <double> input; std::deque <double> input;
enum filterTypes { exponential, doubleExponential, movingAverage, enum filterTypes { exponential, doubleExponential, movingAverage,
noiseSpike, gain, reciprocal, none }; noiseSpike, gain, reciprocal, differential, none };
filterTypes filterType; filterTypes filterType;
protected: protected:
@ -357,6 +372,42 @@ public:
* *
*/ */
class FGXMLAutopilotGroup : public SGSubsystemGroup
{
public:
FGXMLAutopilotGroup();
void init();
void reinit();
void update( double dt );
private:
std::vector<std::string> _autopilotNames;
double average;
double v_last;
double last_static_pressure;
SGPropertyNode_ptr vel;
SGPropertyNode_ptr lookahead5;
SGPropertyNode_ptr lookahead10;
SGPropertyNode_ptr bug;
SGPropertyNode_ptr mag_hdg;
SGPropertyNode_ptr bug_error;
SGPropertyNode_ptr fdm_bug_error;
SGPropertyNode_ptr target_true;
SGPropertyNode_ptr true_hdg;
SGPropertyNode_ptr true_error;
SGPropertyNode_ptr target_nav1;
SGPropertyNode_ptr true_nav1;
SGPropertyNode_ptr true_track_nav1;
SGPropertyNode_ptr nav1_course_error;
SGPropertyNode_ptr nav1_selected_course;
SGPropertyNode_ptr vs_fps;
SGPropertyNode_ptr vs_fpm;
SGPropertyNode_ptr static_pressure;
SGPropertyNode_ptr pressure_rate;
SGPropertyNode_ptr track;
};
class FGXMLAutopilot : public SGSubsystem class FGXMLAutopilot : public SGSubsystem
{ {
@ -371,17 +422,15 @@ public:
void unbind(); void unbind();
void update( double dt ); void update( double dt );
bool build();
bool build( SGPropertyNode_ptr );
protected: protected:
typedef std::vector<FGXMLAutoComponent_ptr> comp_list;
typedef std::vector<SGSharedPtr<FGXMLAutoComponent> > comp_list;
private: private:
bool serviceable; bool serviceable;
SGPropertyNode_ptr config_props;
comp_list components; comp_list components;
}; };

2
src/Main/fg_init.cxx
View file

@ -1536,7 +1536,7 @@ bool fgInitSubsystems() {
// Initialize the XML Autopilot subsystem. // Initialize the XML Autopilot subsystem.
//////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////
globals->add_subsystem( "xml-autopilot", new FGXMLAutopilot ); globals->add_subsystem( "xml-autopilot", new FGXMLAutopilotGroup );
globals->add_subsystem( "route-manager", new FGRouteMgr ); globals->add_subsystem( "route-manager", new FGRouteMgr );
globals->add_subsystem( "autobrake", new FGAutoBrake ); globals->add_subsystem( "autobrake", new FGAutoBrake );