#include "autopilot.hxx"
#include "autopilotgroup.hxx"
-#include <Main/fg_props.hxx>
-#include <Main/globals.hxx>
-#include <Main/util.hxx>
+#include <string>
+#include <vector>
-#include <simgear/misc/sg_path.hxx>
#include <simgear/props/props_io.hxx>
-#include <simgear/structure/SGExpression.hxx>
-
-using std::cout;
-using std::endl;
+#include <simgear/structure/subsystem_mgr.hxx>
+#include <simgear/structure/exception.hxx>
+#include <Main/fg_props.hxx>
+#include <boost/foreach.hpp>
+using std::vector;
+using std::string;
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
{
-}
-
-void FGXMLAutopilotGroup::update( double dt )
+public:
+ FGXMLAutopilotGroupImplementation(const std::string& nodeName) :
+ FGXMLAutopilotGroup(), _nodeName(nodeName) {}
+ virtual void addAutopilot( const std::string & name, SGPropertyNode_ptr apNode, SGPropertyNode_ptr config );
+ virtual void removeAutopilot( const std::string & name );
+ void init();
+ void reinit();
+ void update( double dt );
+private:
+ void initFrom( SGPropertyNode_ptr rootNode, const char * childName );
+ vector<string> _autopilotNames;
+ std::string _nodeName;
+
+};
+
+void FGXMLAutopilotGroupImplementation::addAutopilot( const std::string & name, SGPropertyNode_ptr apNode, SGPropertyNode_ptr config )
{
- // 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;
+ BOOST_FOREACH( std::string & n, _autopilotNames ) {
+ if( n == name ) {
+ SG_LOG(SG_ALL, SG_ALERT, "NOT adding duplicate property rule name " << name );
+ return;
}
-
- lookahead5->setDoubleValue( v + average * 5.0 );
- lookahead10->setDoubleValue( v + average * 10.0 );
- v_last = v;
}
+ FGXMLAutopilot::Autopilot * ap = new FGXMLAutopilot::Autopilot( apNode, config );
+ ap->set_name( name );
- // 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 );
+ double updateInterval = config->getDoubleValue( "update-interval-secs", 0.0 );
+ set_subsystem( name, ap, updateInterval );
+ _autopilotNames.push_back( name );
+}
- // Calculate vertical speed in fpm
- vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 );
+void FGXMLAutopilotGroupImplementation::removeAutopilot( const std::string & name )
+{
+ FGXMLAutopilot::Autopilot * ap = (FGXMLAutopilot::Autopilot*)get_subsystem( name );
+ if( ap == NULL ) return; // ?
+ remove_subsystem( name );
+ delete ap;
+}
- // 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 FGXMLAutopilotGroupImplementation::update( double dt )
+{
+ // update all configured autopilots
+ SGSubsystemGroup::update( dt );
}
-void FGXMLAutopilotGroup::reinit()
+void FGXMLAutopilotGroupImplementation::reinit()
{
SGSubsystemGroup::unbind();
for( vector<string>::size_type i = 0; i < _autopilotNames.size(); i++ ) {
- FGXMLAutopilot::Autopilot * ap = (FGXMLAutopilot::Autopilot*)get_subsystem( _autopilotNames[i] );
- if( ap == NULL ) continue; // ?
- remove_subsystem( _autopilotNames[i] );
- delete ap;
+ removeAutopilot( _autopilotNames[i] );
}
_autopilotNames.clear();
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!");
+ initFrom( fgGetNode( "/sim/systems" ), _nodeName.c_str() );
+
+ SGSubsystemGroup::bind();
+ SGSubsystemGroup::init();
+}
+
+void FGXMLAutopilotGroupImplementation::initFrom( SGPropertyNode_ptr rootNode, const char * childName )
+{
+ if( rootNode == NULL )
return;
- }
- for( PropertyList::size_type i = 0; i < autopilotNodes.size(); i++ ) {
- SGPropertyNode_ptr pathNode = autopilotNodes[i]->getNode( "path" );
+ BOOST_FOREACH( SGPropertyNode_ptr autopilotNode, rootNode->getChildren(childName) ) {
+ SGPropertyNode_ptr pathNode = autopilotNode->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;
}
string apName;
- SGPropertyNode_ptr nameNode = autopilotNodes[i]->getNode( "name" );
+ SGPropertyNode_ptr nameNode = autopilotNode->getNode( "name" );
if( nameNode != NULL ) {
apName = nameNode->getStringValue();
} else {
std::ostringstream buf;
- buf << "unnamed_autopilot_" << i;
+ buf << "unnamed_autopilot_" << autopilotNode->getIndex();
apName = buf.str();
}
// check for duplicate names
string name = apName;
for( unsigned i = 0; get_subsystem( apName.c_str() ) != NULL; i++ ) {
- ostringstream buf;
- buf << apName << "_" << i;
+ std::ostringstream buf;
+ buf << name << "_" << i;
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_WARN, "Duplicate property-rule configuration name " << name << ", renamed to " << apName );
}
- SGPath config( globals->get_fg_root() );
- config.append( pathNode->getStringValue() );
+ addAutopilotFromFile( apName, autopilotNode, pathNode->getStringValue() );
+ }
+}
- SG_LOG( SG_ALL, SG_INFO, "Reading autopilot configuration from " << config.str() );
+void FGXMLAutopilotGroup::addAutopilotFromFile( const std::string & name, SGPropertyNode_ptr apNode, const char * path )
+{
+ SGPath config = globals->resolve_maybe_aircraft_path(path);
+ if (config.isNull())
+ {
+ SG_LOG( SG_ALL, SG_ALERT, "Cannot find property-rule configuration file '" << path << "'." );
+ return;
+ }
+ SG_LOG( SG_ALL, SG_INFO, "Reading property-rule configuration from " << config.str() );
- try {
- SGPropertyNode_ptr root = new SGPropertyNode();
- readProperties( config.str(), root );
+ try {
+ SGPropertyNode_ptr configNode = new SGPropertyNode();
+ readProperties( config.str(), configNode );
- SG_LOG( SG_AUTOPILOT, SG_INFO, "adding autopilot subsystem " << apName );
- FGXMLAutopilot::Autopilot * ap = new FGXMLAutopilot::Autopilot( autopilotNodes[i], root );
- ap->set_name( apName );
- set_subsystem( apName, ap );
- _autopilotNames.push_back( apName );
+ SG_LOG( SG_AUTOPILOT, SG_INFO, "adding property-rule subsystem " << name );
+ addAutopilot( name, apNode, configNode );
- } catch (const sg_exception& e) {
- SG_LOG( SG_AUTOPILOT, SG_ALERT, "Failed to load autopilot configuration: "
- << config.str() << ":" << e.getMessage() );
- continue;
- }
+ } catch (const sg_exception& e) {
+ SG_LOG( SG_AUTOPILOT, SG_ALERT, "Failed to load property-rule configuration: "
+ << config.str() << ":" << e.getMessage() );
+ return;
}
-
- SGSubsystemGroup::bind();
- SGSubsystemGroup::init();
}
+FGXMLAutopilotGroup * FGXMLAutopilotGroup::createInstance(const std::string& nodeName)
+{
+ return new FGXMLAutopilotGroupImplementation(nodeName);
+}
projects / flightgear.git / blobdiff