X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Fautopilotgroup.cxx;h=c9df860d220a58da2a65b0442eae59204c5e94ad;hb=ff91fec1bb4b59dc2a7084de2a5ab0469abb1f3e;hp=81b4d81018e86bdc81b081bc5b2b2dabb0e3ccfc;hpb=ff408dc540b0944061b0b88bdad93b5de17dbab9;p=flightgear.git

diff --git a/src/Autopilot/autopilotgroup.cxx b/src/Autopilot/autopilotgroup.cxx
index 81b4d8101..c9df860d2 100644
--- a/src/Autopilot/autopilotgroup.cxx
+++ b/src/Autopilot/autopilotgroup.cxx
@@ -28,152 +28,106 @@
 #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();
         }
 
@@ -181,42 +135,43 @@ void FGXMLAutopilotGroup::init()
           // 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);
+}