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; };