fly/flightgear
1
0
Fork 0
Code Activity

Autopilot is more than just an autopilot: Introducing: "property rule"

Browse source 
The autopilot has grown beyond being just an autopilot. It's
components, filters and controllers could be used to set property
values based on complex rules and driven from other property values.
That's why I chose to give it the name "property-rule".
This patch allows to define an arbitrary number of property rules under
a/sim/property-rule[n]/path
using the same syntax as autopilot configurations.
This commit is contained in:
Torsten Dreyer 2010-08-18 18:50:07 +02:00
parent ae50c054a9
commit 54e6757211
3 changed files with 46 additions and 129 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

155
src/Autopilot/autopilotgroup.cxx
View file

@ -28,117 +28,35 @@
#include "autopilot.hxx"
#include "autopilotgroup.hxx"
#include <string>
#include <vector>
#include <simgear/structure/subsystem_mgr.hxx>
#include <simgear/structure/exception.hxx>
#include <Main/fg_props.hxx>
#include <Main/globals.hxx>
#include <Main/util.hxx>
#include <simgear/misc/sg_path.hxx>
#include <simgear/props/props_io.hxx>
#include <simgear/structure/SGExpression.hxx>
using std::cout;
using std::endl;
using simgear::PropertyList;
FGXMLAutopilotGroup::FGXMLAutopilotGroup() :
SGSubsystemGroup()
#ifdef XMLAUTO_USEHELPER
,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 ))
#endif
class FGXMLAutopilotGroupImplementation : public FGXMLAutopilotGroup
{
}
public:
void init();
void reinit();
void update( double dt );
private:
void initFrom( SGPropertyNode_ptr rootNode, const char * childName );
vector<string> _autopilotNames;
void FGXMLAutopilotGroup::update( double dt )
};
void FGXMLAutopilotGroupImplementation::update( double dt )
{
// update all configured autopilots
SGSubsystemGroup::update( dt );
#ifdef XMLAUTO_USEHELPER
// update helper values
double v = vel->getDoubleValue();
double a = 0.0;
if ( dt > 0.0 ) {
a = (v - v_last) / dt;
if ( dt < 1.0 ) {
average = (1.0 - dt) * average + dt * a;
} else {
average = a;
}
lookahead5->setDoubleValue( v + average * 5.0 );
lookahead10->setDoubleValue( v + average * 10.0 );
v_last = v;
}
// Calculate heading bug error normalized to +/- 180.0
double diff = bug->getDoubleValue() - mag_hdg->getDoubleValue();
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
bug_error->setDoubleValue( diff );
fdm_bug_error->setDoubleValue( diff );
// Calculate true heading error normalized to +/- 180.0
diff = target_true->getDoubleValue() - true_hdg->getDoubleValue();
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
true_error->setDoubleValue( diff );
// Calculate nav1 target heading error normalized to +/- 180.0
diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue();
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
true_nav1->setDoubleValue( diff );
// Calculate true groundtrack
diff = target_nav1->getDoubleValue() - track->getDoubleValue();
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
true_track_nav1->setDoubleValue( diff );
// Calculate nav1 selected course error normalized to +/- 180.0
diff = nav1_selected_course->getDoubleValue() - mag_hdg->getDoubleValue();
SG_NORMALIZE_RANGE( diff, -180.0, 180.0 );
nav1_course_error->setDoubleValue( diff );
// Calculate vertical speed in fpm
vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );
// Calculate static port pressure rate in [inhg/s].
// Used to determine vertical speed.
if ( dt > 0.0 ) {
double current_static_pressure = static_pressure->getDoubleValue();
double current_pressure_rate =
( current_static_pressure - last_static_pressure ) / dt;
pressure_rate->setDoubleValue(current_pressure_rate);
last_static_pressure = current_static_pressure;
}
#endif
}
void FGXMLAutopilotGroup::reinit()
void FGXMLAutopilotGroupImplementation::reinit()
{
SGSubsystemGroup::unbind();
@ -152,18 +70,26 @@ void FGXMLAutopilotGroup::reinit()
init();
}
void FGXMLAutopilotGroup::init()
void FGXMLAutopilotGroupImplementation::init()
{
PropertyList 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;
}
static const char * nodeNames[] = {
"autopilot",
"property-rule"
};
for( unsigned i = 0; i < sizeof(nodeNames)/sizeof(nodeNames[0]); i++ )
initFrom( fgGetNode( "/sim/systems" ), nodeNames[i] );
}
void FGXMLAutopilotGroupImplementation::initFrom( SGPropertyNode_ptr rootNode, const char * childName )
{
if( rootNode == NULL )
return;
PropertyList autopilotNodes = rootNode->getChildren(childName);
for( PropertyList::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!");
SG_LOG( SG_ALL, SG_WARN, "No configuration file specified for this property-rule!");
continue;
}
@ -186,30 +112,25 @@ void FGXMLAutopilotGroup::init()
apName = buf.str();
}
if( apName != name )
SG_LOG( SG_ALL, SG_ALERT, "Duplicate autopilot component " << name << ", renamed to " << apName );
}
if( get_subsystem( apName.c_str() ) != NULL ) {
SG_LOG( SG_ALL, SG_ALERT, "Duplicate autopilot configuration name " << apName << " ignored" );
continue;
SG_LOG( SG_ALL, SG_ALERT, "Duplicate property-rule configuration name " << name << ", renamed to " << apName );
}
SGPath config = globals->resolve_aircraft_path(pathNode->getStringValue());
SG_LOG( SG_ALL, SG_INFO, "Reading autopilot configuration from " << config.str() );
SG_LOG( SG_ALL, SG_INFO, "Reading property-rule configuration from " << config.str() );
try {
SGPropertyNode_ptr root = new SGPropertyNode();
readProperties( config.str(), root );
SG_LOG( SG_AUTOPILOT, SG_INFO, "adding autopilot subsystem " << apName );
SG_LOG( SG_AUTOPILOT, SG_INFO, "adding property-rule subsystem " << apName );
FGXMLAutopilot::Autopilot * ap = new FGXMLAutopilot::Autopilot( autopilotNodes[i], root );
ap->set_name( apName );
set_subsystem( apName, ap );
_autopilotNames.push_back( apName );
} catch (const sg_exception& e) {
SG_LOG( SG_AUTOPILOT, SG_ALERT, "Failed to load autopilot configuration: "
SG_LOG( SG_AUTOPILOT, SG_ALERT, "Failed to load property-rule configuration: "
<< config.str() << ":" << e.getMessage() );
continue;
}
@ -219,3 +140,7 @@ void FGXMLAutopilotGroup::init()
SGSubsystemGroup::init();
}
FGXMLAutopilotGroup * FGXMLAutopilotGroup::createInstance()
{
return new FGXMLAutopilotGroupImplementation();
}

14
src/Autopilot/autopilotgroup.hxx
View file

@ -24,11 +24,6 @@
#ifndef _XMLAUTO_HXX
#define _XMLAUTO_HXX 1
#include <string>
#include <vector>
#include <simgear/props/props.hxx>
#include <simgear/structure/subsystem_mgr.hxx>
/**
* @brief Model an autopilot system by implementing a SGSubsystemGroup
@ -37,12 +32,9 @@
class FGXMLAutopilotGroup : public SGSubsystemGroup
{
public:
FGXMLAutopilotGroup();
void init();
void reinit();
void update( double dt );
private:
std::vector<std::string> _autopilotNames;
static FGXMLAutopilotGroup * createInstance();
protected:
FGXMLAutopilotGroup() : SGSubsystemGroup() {}
};

6
src/Main/fg_init.cxx
View file

@ -1258,8 +1258,8 @@ bool fgInitGeneral() {
curr->setStringValue(cwd ? cwd : "");
curr->setAttribute(SGPropertyNode::WRITE, false);
fgSetBool("/sim/startup/stdout-to-terminal", isatty(1));
fgSetBool("/sim/startup/stderr-to-terminal", isatty(2));
fgSetBool("/sim/startup/stdout-to-terminal", isatty(1) != 0 );
fgSetBool("/sim/startup/stderr-to-terminal", isatty(2) != 0 );
return true;
}
@ -1360,7 +1360,7 @@ bool fgInitSubsystems() {
// Initialize the XML Autopilot subsystem.
////////////////////////////////////////////////////////////////////
globals->add_subsystem( "xml-autopilot", new FGXMLAutopilotGroup, SGSubsystemMgr::FDM );
globals->add_subsystem( "xml-autopilot", FGXMLAutopilotGroup::createInstance(), SGSubsystemMgr::FDM );
globals->add_subsystem( "route-manager", new FGRouteMgr );
////////////////////////////////////////////////////////////////////