From 54e6757211475d2ca0a3d6547958f6789710a51f Mon Sep 17 00:00:00 2001 From: Torsten Dreyer <Torsten@t3r.de> Date: Wed, 18 Aug 2010 18:50:07 +0200 Subject: [PATCH] Autopilot is more than just an autopilot: Introducing: "property rule" 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. --- src/Autopilot/autopilotgroup.cxx | 155 ++++++++----------------------- src/Autopilot/autopilotgroup.hxx | 14 +-- src/Main/fg_init.cxx | 6 +- 3 files changed, 46 insertions(+), 129 deletions(-) diff --git a/src/Autopilot/autopilotgroup.cxx b/src/Autopilot/autopilotgroup.cxx index e5bb6d03c..022b83d0c 100644 --- a/src/Autopilot/autopilotgroup.cxx +++ b/src/Autopilot/autopilotgroup.cxx @@ -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(); +} diff --git a/src/Autopilot/autopilotgroup.hxx b/src/Autopilot/autopilotgroup.hxx index e27a6b16d..1ccb0be76 100644 --- a/src/Autopilot/autopilotgroup.hxx +++ b/src/Autopilot/autopilotgroup.hxx @@ -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() {} }; diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx index b98d8dbbb..7d1b82fd8 100644 --- a/src/Main/fg_init.cxx +++ b/src/Main/fg_init.cxx @@ -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 ); ////////////////////////////////////////////////////////////////////