From 179a7993334d84f0a7f1b9447ef03f18599f5ba7 Mon Sep 17 00:00:00 2001 From: torsten Date: Fri, 27 Nov 2009 15:12:34 +0000 Subject: [PATCH 1/7] - fix the element which was no longer working due to an invalid property root - allow multiple elements within an aircraft. All autopilot live in an individual FGXMLAutopilot subsystem which run within a subsystem group now. --- src/Autopilot/xmlauto.cxx | 121 +++++++++++++++++++++++++++----------- src/Autopilot/xmlauto.hxx | 14 ++++- src/Main/fg_init.cxx | 2 +- 3 files changed, 98 insertions(+), 39 deletions(-) diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx index 6d5428557..8a91a091c 100644 --- a/src/Autopilot/xmlauto.cxx +++ b/src/Autopilot/xmlauto.cxx @@ -69,7 +69,7 @@ void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffs SGPropertyNode * n; if( (n = node->getChild("condition")) != NULL ) { - _condition = sgReadCondition(node, n); + _condition = sgReadCondition(fgGetNode("/"), n); } if( (n = node->getChild( "scale" )) != NULL ) { @@ -200,7 +200,7 @@ void FGXMLAutoComponent::parseNode(SGPropertyNode* aNode) debug = child->getBoolValue(); } else if ( cname == "enable" ) { if( (prop = child->getChild("condition")) != NULL ) { - _condition = sgReadCondition(child, prop); + _condition = sgReadCondition(fgGetNode("/"), prop); } else { if ( (prop = child->getChild( "prop" )) != NULL ) { enable_prop = fgGetNode( prop->getStringValue(), true ); @@ -793,6 +793,83 @@ void FGDigitalFilter::update(double dt) } } +FGXMLAutopilotGroup::FGXMLAutopilotGroup() +{ +} + +void FGXMLAutopilotGroup::reinit() +{ + for( vector::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 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::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() { } @@ -802,38 +879,11 @@ 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!"); - } +/* 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() +{ } @@ -849,7 +899,7 @@ void FGXMLAutopilot::bind() { void FGXMLAutopilot::unbind() { } -bool FGXMLAutopilot::build() { +bool FGXMLAutopilot::build( SGPropertyNode_ptr config_props ) { SGPropertyNode *node; int i; @@ -858,6 +908,7 @@ bool FGXMLAutopilot::build() { 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" ) { diff --git a/src/Autopilot/xmlauto.hxx b/src/Autopilot/xmlauto.hxx index 3f68fb20c..549244c32 100644 --- a/src/Autopilot/xmlauto.hxx +++ b/src/Autopilot/xmlauto.hxx @@ -357,6 +357,16 @@ public: * */ +class FGXMLAutopilotGroup : public SGSubsystemGroup +{ +public: + FGXMLAutopilotGroup(); + void init(); + void reinit(); +private: + std::vector _autopilotNames; +}; + class FGXMLAutopilot : public SGSubsystem { @@ -371,16 +381,14 @@ public: void unbind(); void update( double dt ); - bool build(); + bool build( SGPropertyNode_ptr ); protected: - typedef std::vector > comp_list; private: bool serviceable; - SGPropertyNode_ptr config_props; comp_list components; }; diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx index 2e8713a17..24f8726a3 100644 --- a/src/Main/fg_init.cxx +++ b/src/Main/fg_init.cxx @@ -1537,7 +1537,7 @@ bool fgInitSubsystems() { // 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( "autobrake", new FGAutoBrake ); From 506f6894e44c76df153779a6dca4762b8bf15d41 Mon Sep 17 00:00:00 2001 From: torsten Date: Tue, 15 Dec 2009 21:27:46 +0000 Subject: [PATCH 2/7] - introduce some typedefs for SGSharedPtr<> - add a element to input and output of each filter to support periodical values like headings. See README.digitalfilters for details. --- src/Autopilot/xmlauto.cxx | 54 +++++++++++++++++++++++++++++++++++---- src/Autopilot/xmlauto.hxx | 31 ++++++++++++++++------ 2 files changed, 72 insertions(+), 13 deletions(-) diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx index 8a91a091c..8d07a66bd 100644 --- a/src/Autopilot/xmlauto.cxx +++ b/src/Autopilot/xmlauto.cxx @@ -40,14 +40,42 @@ using std::cout; 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 and/or 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(p1,p2); + double max = std::max(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) : value(0.0), abs(false), - property(NULL), - offset(NULL), - scale(NULL), - min(NULL), - max(NULL), _condition(NULL) { parse( node, value, offset, scale ); @@ -62,6 +90,7 @@ void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffs scale = NULL; min = NULL; max = NULL; + periodical = NULL; if( node == NULL ) return; @@ -92,6 +121,10 @@ void FGXMLAutoInput::parse( SGPropertyNode_ptr node, double aValue, double aOffs abs = n->getBoolValue(); } + if( (n = node->getChild( "period" )) != NULL ) { + periodical = new FGPeriodicalValue( n ); + } + SGPropertyNode *valueNode = node->getChild( "value" ); if ( valueNode != NULL ) { value = valueNode->getDoubleValue(); @@ -161,6 +194,10 @@ double FGXMLAutoInput::get_value() if( value > m ) value = m; } + + if( periodical ) { + value = periodical->normalize( value ); + } return abs ? fabs(value) : value; } @@ -246,6 +283,8 @@ void FGXMLAutoComponent::parseNode(SGPropertyNode* aNode) umaxInput.push_back( new FGXMLAutoInput( child ) ); } else if ( cname == "u_max" ) { umaxInput.push_back( new FGXMLAutoInput( child ) ); + } else if ( cname == "period" ) { + periodical = new FGPeriodicalValue( child ); } else { SG_LOG(SG_AUTOPILOT, SG_ALERT, "malformed autopilot definition - unrecognized node:" << cname << " in section " << name); @@ -314,6 +353,11 @@ void FGXMLAutoComponent::do_feedback_if_disabled() 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 if( uminInput.size() + umaxInput.size() > 0 ) { double d = umaxInput.get_value( 0.0 ); diff --git a/src/Autopilot/xmlauto.hxx b/src/Autopilot/xmlauto.hxx index 549244c32..efdf2f3ce 100644 --- a/src/Autopilot/xmlauto.hxx +++ b/src/Autopilot/xmlauto.hxx @@ -34,16 +34,28 @@ #include #include +typedef SGSharedPtr FGXMLAutoInput_ptr; +typedef SGSharedPtr 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 { private: double value; // The value as a constant or initializer for the property bool abs; // return absolute value SGPropertyNode_ptr property; // The name of the property containing the value - SGSharedPtr offset; // A fixed offset, defaults to zero - SGSharedPtr scale; // A constant scaling factor defaults to one - SGSharedPtr min; // A minimum clip defaults to no clipping - SGSharedPtr max; // A maximum clip defaults to no clipping + FGXMLAutoInput_ptr offset; // A fixed offset, defaults to zero + FGXMLAutoInput_ptr scale; // A constant scaling factor defaults to one + FGXMLAutoInput_ptr min; // A minimum clip defaults to no clipping + FGXMLAutoInput_ptr max; // A maximum clip defaults to no clipping + FGPeriodicalValue_ptr periodical; // SGSharedPtr _condition; public: @@ -71,9 +83,9 @@ public: }; -class FGXMLAutoInputList : public std::vector > { +class FGXMLAutoInputList : public std::vector { public: - FGXMLAutoInput * get_active() { + FGXMLAutoInput_ptr get_active() { for (iterator it = begin(); it != end(); ++it) { if( (*it)->is_enabled() ) return *it; @@ -82,7 +94,7 @@ class FGXMLAutoInputList : public std::vector > { } double get_value( double def = 0.0 ) { - FGXMLAutoInput * input = get_active(); + FGXMLAutoInput_ptr input = get_active(); return input == NULL ? def : input->get_value(); } @@ -147,6 +159,7 @@ protected: FGXMLAutoInputList referenceInput; FGXMLAutoInputList uminInput; FGXMLAutoInputList umaxInput; + FGPeriodicalValue_ptr periodical; // debug flag bool debug; bool enabled; @@ -221,6 +234,8 @@ public: bool isPropertyEnabled(); }; +typedef SGSharedPtr FGXMLAutoComponent_ptr; + /** * Roy Ovesen's PID controller @@ -384,7 +399,7 @@ public: bool build( SGPropertyNode_ptr ); protected: - typedef std::vector > comp_list; + typedef std::vector comp_list; private: From 2b3e07cc5f451aa5b2e915f78c31ddd0acf041cf Mon Sep 17 00:00:00 2001 From: torsten Date: Tue, 15 Dec 2009 21:28:30 +0000 Subject: [PATCH 3/7] document the element --- docs-mini/README.digitalfilters | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/docs-mini/README.digitalfilters b/docs-mini/README.digitalfilters index ad7a26e26..dce9a80dd 100644 --- a/docs-mini/README.digitalfilters +++ b/docs-mini/README.digitalfilters @@ -27,6 +27,10 @@ The complete XML syntax for a InputValue is infinity -infinity false + + -180.0 + -180.0 + The enclosing element is the element defined in each filter, like , , @@ -124,7 +128,7 @@ other computations are completed. 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. The output properties are defined in the element by adding elements within the element. For just a single output property, the element @@ -167,6 +171,8 @@ Example: min or max value defaults to 0 (zero). Note: and may also occour within a element. and may be used as a substitude for the corresponding u_xxx element. + Complex Define a periodical input or output value. The phase width is defined by the + child elements and which are of type InputValue 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 @@ -183,6 +189,17 @@ Implicit definition of the minimum value of 0 (zero) and defining a maximum of 1 100.0 +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. + + -180.0 + 180.0 + + + Complex Define a condition to enable or disable the filter. For disabled filters, no output computations are performed. Only enabled filters fill the output properties. The default for undefined From 50adab32c15ec54e4f557cf85f567bae4fda3e4e Mon Sep 17 00:00:00 2001 From: torsten Date: Fri, 18 Dec 2009 11:43:14 +0000 Subject: [PATCH 4/7] Ingore unhandled top level elements instead of not loading the autopilot. This (re-)enables usage of at top level and --- src/Autopilot/xmlauto.cxx | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx index 8d07a66bd..28bfdaffd 100644 --- a/src/Autopilot/xmlauto.cxx +++ b/src/Autopilot/xmlauto.cxx @@ -961,10 +961,10 @@ bool FGXMLAutopilot::build( SGPropertyNode_ptr config_props ) { 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; +// } else { +// SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: " +// << name ); +// return false; } } From 5cad5aa7da2476ca8323a61f81dea59676dca085 Mon Sep 17 00:00:00 2001 From: jmt Date: Sat, 26 Dec 2009 12:31:07 +0000 Subject: [PATCH 5/7] Autopilot: clean up the helpers code (which drives the various /internal/) properties. Fixes various heading modes to work in all aircraft. --- src/Autopilot/xmlauto.cxx | 72 +++++++++++++++------------------------ src/Autopilot/xmlauto.hxx | 9 ++++- 2 files changed, 36 insertions(+), 45 deletions(-) diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx index 28bfdaffd..ff149a99c 100644 --- a/src/Autopilot/xmlauto.cxx +++ b/src/Autopilot/xmlauto.cxx @@ -928,6 +928,8 @@ FGXMLAutopilot::~FGXMLAutopilot() { */ void FGXMLAutopilot::init() { + latNode = fgGetNode("/position/latitude-deg"); + lonNode = fgGetNode("/position/longitude-deg"); } @@ -975,7 +977,7 @@ bool FGXMLAutopilot::build( SGPropertyNode_ptr config_props ) { /* * Update helper values */ -static void update_helper( double dt ) { +void FGXMLAutopilot::update_helper( double dt ) { // Estimate speed in 5,10 seconds static SGPropertyNode_ptr vel = fgGetNode( "/velocities/airspeed-kt", true ); static SGPropertyNode_ptr lookahead5 @@ -1002,32 +1004,20 @@ static void update_helper( double dt ) { v_last = v; } - // Calculate heading bug error normalized to +/- 180.0 (based on - // DG indicated heading) + // Calculate heading bug error normalized to +/- 180.0 static SGPropertyNode_ptr bug = fgGetNode( "/autopilot/settings/heading-bug-deg", true ); - 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 bug_error + = fgGetNode( "/autopilot/internal/heading-bug-error-deg", true ); + + double diff = bug->getDoubleValue() - mag_hdg->getDoubleValue(); + SG_NORMALIZE_RANGE(diff, -180.0, 180.0); + bug_error->setDoubleValue( diff ); + 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 ); // Calculate true heading error normalized to +/- 180.0 @@ -1035,14 +1025,11 @@ static void update_helper( double dt ) { = 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(); - if ( diff < -180.0 ) { diff += 360.0; } - if ( diff > 180.0 ) { diff -= 360.0; } + SG_NORMALIZE_RANGE(diff, -180.0, 180.0); true_error->setDoubleValue( diff ); // Calculate nav1 target heading error normalized to +/- 180.0 @@ -1054,25 +1041,25 @@ static void update_helper( double dt ) { = fgGetNode( "/autopilot/internal/nav1-track-error-deg", true ); diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue(); - if ( diff < -180.0 ) { diff += 360.0; } - if ( diff > 180.0 ) { diff -= 360.0; } + SG_NORMALIZE_RANGE(diff, -180.0, 180.0); true_nav1->setDoubleValue( diff ); - diff = target_nav1->getDoubleValue() - true_track->getDoubleValue(); - if ( diff < -180.0 ) { diff += 360.0; } - if ( diff > 180.0 ) { diff -= 360.0; } + // Calculate true groundtrack + SGGeod currentPosition(SGGeod::fromDeg( + lonNode->getDoubleValue(), latNode->getDoubleValue())); + double true_track = SGGeodesy::courseDeg(lastPosition, currentPosition); + lastPosition = currentPosition; + diff = target_nav1->getDoubleValue() - true_track; + SG_NORMALIZE_RANGE(diff, -180.0, 180.0); true_track_nav1->setDoubleValue( diff ); // Calculate nav1 selected course error normalized to +/- 180.0 - // (based on DG indicated heading) 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; } + diff = nav1_selected_course->getDoubleValue() - mag_hdg->getDoubleValue(); SG_NORMALIZE_RANGE( diff, -180.0, 180.0 ); nav1_course_error->setDoubleValue( diff ); @@ -1088,23 +1075,20 @@ static void update_helper( double dt ) { // Calculate static port pressure rate in [inhg/s]. // Used to determine vertical speed. static SGPropertyNode_ptr static_pressure - = fgGetNode( "/systems/static[0]/pressure-inhg", true ); + = fgGetNode( "/systems/static[0]/pressure-inhg", true ); static SGPropertyNode_ptr pressure_rate - = fgGetNode( "/autopilot/internal/pressure-rate", true ); + = fgGetNode( "/autopilot/internal/pressure-rate", true ); static double last_static_pressure = 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 = - ( current_static_pressure - last_static_pressure ) / dt; - - pressure_rate->setDoubleValue(current_pressure_rate); - - last_static_pressure = current_static_pressure; + pressure_rate->setDoubleValue(current_pressure_rate); + last_static_pressure = current_static_pressure; } - } diff --git a/src/Autopilot/xmlauto.hxx b/src/Autopilot/xmlauto.hxx index efdf2f3ce..4b122caef 100644 --- a/src/Autopilot/xmlauto.hxx +++ b/src/Autopilot/xmlauto.hxx @@ -402,9 +402,16 @@ protected: typedef std::vector comp_list; private: - + /** + * Update helper values, especially the /autopilot/internal properties + */ + void update_helper( double dt ); + bool serviceable; comp_list components; + + SGPropertyNode_ptr latNode, lonNode; + SGGeod lastPosition; }; From 95e2d62d94586e694c5241fcc46ad7a37173dce1 Mon Sep 17 00:00:00 2001 From: torsten Date: Tue, 29 Dec 2009 15:34:02 +0000 Subject: [PATCH 6/7] bugfix: don't call the update_helper() for every instance of FGXMLAutopilot. Compute the helper properties only once per FGXMLAutopilotGroup's update() method. Only one AutopilotGroup shall be instantiated, but many Autopilots may exist. --- src/Autopilot/xmlauto.cxx | 223 +++++++++++++++++--------------------- src/Autopilot/xmlauto.hxx | 35 ++++-- 2 files changed, 126 insertions(+), 132 deletions(-) diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx index ff149a99c..090d9fbb7 100644 --- a/src/Autopilot/xmlauto.cxx +++ b/src/Autopilot/xmlauto.cxx @@ -837,10 +837,105 @@ void FGDigitalFilter::update(double dt) } } -FGXMLAutopilotGroup::FGXMLAutopilotGroup() +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 )), + latNode(fgGetNode("/position/latitude-deg")), + lonNode(fgGetNode("/position/longitude-deg")) { } +void FGXMLAutopilotGroup::update( double dt ) +{ + // update all configured autopilots + SGSubsystemGroup::update( dt ); + + // 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 + SGGeod currentPosition(SGGeod::fromDeg( + lonNode->getDoubleValue(), latNode->getDoubleValue())); + double true_track = SGGeodesy::courseDeg(lastPosition, currentPosition); + lastPosition = currentPosition; + diff = target_nav1->getDoubleValue() - true_track; + 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; + } +} + void FGXMLAutopilotGroup::reinit() { for( vector::size_type i = 0; i < _autopilotNames.size(); i++ ) { @@ -928,8 +1023,6 @@ FGXMLAutopilot::~FGXMLAutopilot() { */ void FGXMLAutopilot::init() { - latNode = fgGetNode("/position/latitude-deg"); - lonNode = fgGetNode("/position/longitude-deg"); } @@ -973,132 +1066,12 @@ bool FGXMLAutopilot::build( SGPropertyNode_ptr config_props ) { return true; } - -/* - * Update helper values - */ -void FGXMLAutopilot::update_helper( double 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 - - 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 - static SGPropertyNode_ptr bug - = fgGetNode( "/autopilot/settings/heading-bug-deg", true ); - static SGPropertyNode_ptr mag_hdg - = fgGetNode( "/orientation/heading-magnetic-deg", true ); - static SGPropertyNode_ptr bug_error - = fgGetNode( "/autopilot/internal/heading-bug-error-deg", true ); - - double diff = bug->getDoubleValue() - mag_hdg->getDoubleValue(); - SG_NORMALIZE_RANGE(diff, -180.0, 180.0); - bug_error->setDoubleValue( diff ); - - static SGPropertyNode_ptr fdm_bug_error - = fgGetNode( "/autopilot/internal/fdm-heading-bug-error-deg", true ); - fdm_bug_error->setDoubleValue( diff ); - - // 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_error - = fgGetNode( "/autopilot/internal/true-heading-error-deg", true ); - - 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 - 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(); - SG_NORMALIZE_RANGE(diff, -180.0, 180.0); - true_nav1->setDoubleValue( diff ); - - // Calculate true groundtrack - SGGeod currentPosition(SGGeod::fromDeg( - lonNode->getDoubleValue(), latNode->getDoubleValue())); - double true_track = SGGeodesy::courseDeg(lastPosition, currentPosition); - lastPosition = currentPosition; - diff = target_nav1->getDoubleValue() - true_track; - SG_NORMALIZE_RANGE(diff, -180.0, 180.0); - true_track_nav1->setDoubleValue( diff ); - - // Calculate nav1 selected course error normalized to +/- 180.0 - 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() - mag_hdg->getDoubleValue(); - SG_NORMALIZE_RANGE( diff, -180.0, 180.0 ); - nav1_course_error->setDoubleValue( diff ); - - // 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 ); - - - // Calculate static port pressure rate in [inhg/s]. - // 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 ) { - 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; - } -} - - /* * Update the list of autopilot components */ -void FGXMLAutopilot::update( double dt ) { - update_helper( dt ); - +void FGXMLAutopilot::update( double dt ) +{ unsigned int i; for ( i = 0; i < components.size(); ++i ) { components[i]->update( dt ); diff --git a/src/Autopilot/xmlauto.hxx b/src/Autopilot/xmlauto.hxx index 4b122caef..a29545759 100644 --- a/src/Autopilot/xmlauto.hxx +++ b/src/Autopilot/xmlauto.hxx @@ -378,8 +378,36 @@ public: FGXMLAutopilotGroup(); void init(); void reinit(); + void update( double dt ); private: std::vector _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 latNode, lonNode; + SGGeod lastPosition; }; class FGXMLAutopilot : public SGSubsystem @@ -402,16 +430,9 @@ protected: typedef std::vector comp_list; private: - /** - * Update helper values, especially the /autopilot/internal properties - */ - void update_helper( double dt ); - bool serviceable; comp_list components; - SGPropertyNode_ptr latNode, lonNode; - SGGeod lastPosition; }; From 168af9dc1e86b78fe714754253258f9c304b3d09 Mon Sep 17 00:00:00 2001 From: torsten Date: Sun, 3 Jan 2010 10:13:19 +0000 Subject: [PATCH 7/7] - added a differential filter - use /orientation/track-deg instead of computing our own track - some cosmetic changes --- src/Autopilot/xmlauto.cxx | 144 ++++++++++++++++++++------------------ src/Autopilot/xmlauto.hxx | 6 +- 2 files changed, 77 insertions(+), 73 deletions(-) diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx index 090d9fbb7..d37164a72 100644 --- a/src/Autopilot/xmlauto.cxx +++ b/src/Autopilot/xmlauto.cxx @@ -65,7 +65,7 @@ double FGPeriodicalValue::normalize( double value ) if( phase > SGLimitsd::min() ) { while( value < min ) value += phase; - while( value > max ) value -= phase; + while( value >= max ) value -= phase; } else { value = min; // phase is zero } @@ -729,6 +729,10 @@ bool FGDigitalFilter::parseNodeHook(const string& aName, SGPropertyNode* aNode) filterType = gain; } else if (val == "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" ) { TfInput.push_back( new FGXMLAutoInput( aNode, 1.0 ) ); @@ -768,72 +772,79 @@ void FGDigitalFilter::update(double dt) do_feedback(); } - if ( enabled && dt > 0.0 ) { - /* - * Exponential filter - * - * Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1] - * - */ - if( debug ) cout << "Updating " << get_name() - << " dt " << dt << endl; + if ( !enabled || dt < SGLimitsd::min() ) + return; - if (filterType == exponential) - { - double alpha = 1 / ((TfInput.get_value()/dt) + 1); - output.push_front(alpha * input[0] + - (1 - alpha) * output[0]); - } - else if (filterType == doubleExponential) - { - 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; + /* + * Exponential filter + * + * Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1] + * + */ + if( debug ) cout << "Updating " << get_name() + << " dt " << dt << endl; - if ((output[0] - input[0]) > maxChange) - { - output.push_front(output[0] - maxChange); - } - 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]; - } - else if (filterType == reciprocal) - { - if (input[0] != 0.0) { - output[0] = gainInput.get_value() / input[0]; - } - } + if (filterType == exponential) + { + double alpha = 1 / ((TfInput.get_value()/dt) + 1); + output.push_front(alpha * input[0] + + (1 - alpha) * output[0]); + } + else if (filterType == doubleExponential) + { + 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; - output[0] = clamp(output[0]) ; - set_output_value( output[0] ); - - output.resize(2); - - if (debug) + if ((output[0] - input[0]) > maxChange) { - cout << "input:" << input[0] - << "\toutput:" << output[0] << endl; + output.push_front(output[0] - maxChange); } + 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]; + } + else if (filterType == reciprocal) + { + if (input[0] != 0.0) { + output[0] = gainInput.get_value() / input[0]; + } + } + else if (filterType == differential) + { + if( dt > SGLimitsd::min() ) { + output[0] = (input[0]-input[1]) * TfInput.get_value() / dt; + } + } + + output[0] = clamp(output[0]) ; + set_output_value( output[0] ); + + output.resize(2); + + if (debug) + { + cout << "input:" << input[0] + << "\toutput:" << output[0] << endl; } } @@ -862,8 +873,7 @@ FGXMLAutopilotGroup::FGXMLAutopilotGroup() : 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 )), - latNode(fgGetNode("/position/latitude-deg")), - lonNode(fgGetNode("/position/longitude-deg")) + track(fgGetNode( "/orientation/track-deg", true )) { } @@ -907,11 +917,7 @@ void FGXMLAutopilotGroup::update( double dt ) true_nav1->setDoubleValue( diff ); // Calculate true groundtrack - SGGeod currentPosition(SGGeod::fromDeg( - lonNode->getDoubleValue(), latNode->getDoubleValue())); - double true_track = SGGeodesy::courseDeg(lastPosition, currentPosition); - lastPosition = currentPosition; - diff = target_nav1->getDoubleValue() - true_track; + diff = target_nav1->getDoubleValue() - track->getDoubleValue(); SG_NORMALIZE_RANGE(diff, -180.0, 180.0); true_track_nav1->setDoubleValue( diff ); diff --git a/src/Autopilot/xmlauto.hxx b/src/Autopilot/xmlauto.hxx index a29545759..6d7d3f2aa 100644 --- a/src/Autopilot/xmlauto.hxx +++ b/src/Autopilot/xmlauto.hxx @@ -354,7 +354,7 @@ private: std::deque output; std::deque input; enum filterTypes { exponential, doubleExponential, movingAverage, - noiseSpike, gain, reciprocal, none }; + noiseSpike, gain, reciprocal, differential, none }; filterTypes filterType; protected: @@ -405,9 +405,7 @@ private: SGPropertyNode_ptr vs_fpm; SGPropertyNode_ptr static_pressure; SGPropertyNode_ptr pressure_rate; - - SGPropertyNode_ptr latNode, lonNode; - SGGeod lastPosition; + SGPropertyNode_ptr track; }; class FGXMLAutopilot : public SGSubsystem