diff --git a/src/Instrumentation/heading_indicator_dg.cxx b/src/Instrumentation/heading_indicator_dg.cxx index d9923c2e5..9bcd19b67 100644 --- a/src/Instrumentation/heading_indicator_dg.cxx +++ b/src/Instrumentation/heading_indicator_dg.cxx @@ -7,6 +7,7 @@ // This file is in the Public Domain and comes with no warranty. #include +#include #include #include #include @@ -54,15 +55,26 @@ HeadingIndicatorDG::init () branch = "/instrumentation/" + name; _heading_in_node = fgGetNode("/orientation/heading-deg", true); + _yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true); + _g_node = fgGetNode("/accelerations/pilot-g", true); + + SGPropertyNode *node = fgGetNode(branch.c_str(), num, true ); _offset_node = node->getChild("offset-deg", 0, true); _serviceable_node = node->getChild("serviceable", 0, true); - _error_node = node->getChild("heading-bug-error-deg", 0, true); + _heading_bug_error_node = node->getChild("heading-bug-error-deg", 0, true); + _error_node = node->getChild("error-deg", 0, true); _nav1_error_node = node->getChild("nav1-course-error-deg", 0, true); _heading_out_node = node->getChild("indicated-heading-deg", 0, true); - _last_heading_deg = (_heading_in_node->getDoubleValue() + - _offset_node->getDoubleValue()); + _align_node = node->getChild("align-deg", 0, true); + _electrical_node = fgGetNode("/systems/electrical/outputs/DG", true); + + _align_node->setDoubleValue(0); + _error_node->setDoubleValue(0); + + _last_heading_deg = (_heading_in_node->getDoubleValue() + + _offset_node->getDoubleValue() + _align_node->getDoubleValue()); } void @@ -100,11 +112,30 @@ HeadingIndicatorDG::update (double dt) _gyro.update(dt); double spin = _gyro.get_spin_norm(); - // No time-based precession for a flux gate compass - // No magvar - double offset = 0; + // Next, calculate time-based precession + double offset = _offset_node->getDoubleValue(); + offset -= dt * (0.25 / 60.0); // 360deg/day + SG_NORMALIZE_RANGE(offset, -360.0, 360.0); + _offset_node->setDoubleValue(offset); - // TODO: movement-induced error + // No magvar - set the alignment manually + double align = _align_node->getDoubleValue(); + + // Movement-induced error + double yaw_rate = _yaw_rate_node->getDoubleValue(); + double error = _error_node->getDoubleValue(); + double g = _g_node->getDoubleValue(); + int sign = 0; + + if ( fabs ( yaw_rate ) > 5 ) { + error += 0.033 * -yaw_rate * dt ; + } + + if ( g > 1.5 || g < -0.5){ + error += 0.033 * g * dt; + } + + _error_node->setDoubleValue(error); // Next, calculate the indicated heading, // introducing errors. @@ -122,11 +153,8 @@ HeadingIndicatorDG::update (double dt) heading = fgGetLowPass(_last_heading_deg, heading, dt * factor); _last_heading_deg = heading; - heading += offset; - while (heading < 0) - heading += 360; - while (heading > 360) - heading -= 360; + heading += offset + align + error; + SG_NORMALIZE_RANGE(heading, 0.0, 360.0); _heading_out_node->setDoubleValue(heading); @@ -138,7 +166,7 @@ HeadingIndicatorDG::update (double dt) double diff = bnode->getDoubleValue() - heading; if ( diff < -180.0 ) { diff += 360.0; } if ( diff > 180.0 ) { diff -= 360.0; } - _error_node->setDoubleValue( diff ); + _heading_bug_error_node->setDoubleValue( diff ); } // calculate the difference between the indicated heading // and the selected nav1 radial for use with an autopilot @@ -152,4 +180,4 @@ HeadingIndicatorDG::update (double dt) } } -// end of heading_indicator_fg.cxx +// end of heading_indicator_dg.cxx diff --git a/src/Instrumentation/heading_indicator_dg.hxx b/src/Instrumentation/heading_indicator_dg.hxx index 89c22f3b9..d016ca45b 100644 --- a/src/Instrumentation/heading_indicator_dg.hxx +++ b/src/Instrumentation/heading_indicator_dg.hxx @@ -1,64 +1,69 @@ -// heading_indicator.hxx - a vacuum-powered heading indicator. -// Written by David Megginson, started 2002. -// -// This file is in the Public Domain and comes with no warranty. - - -#ifndef __INSTRUMENTS_HEADING_INDICATOR_ELEC_HXX -#define __INSTRUMENTS_HEADING_INDICATOR_ELEC_HXX 1 - - -#include -#include - -#include "gyro.hxx" - - -/** - * Model an electrically-powered heading indicator. - * - * Input properties: - * - * /instrumentation/"name"/serviceable - * /instrumentation/"name"/spin - * /instrumentation/"name"/offset-deg - * /orientation/heading-deg - * /systems/electrical/outputs/DG - * - * Output properties: - * - * /instrumentation/"name"/indicated-heading-deg - */ -class HeadingIndicatorDG : public SGSubsystem -{ - -public: - - HeadingIndicatorDG ( SGPropertyNode *node ); - HeadingIndicatorDG (); - virtual ~HeadingIndicatorDG (); - - virtual void init (); - virtual void bind (); - virtual void unbind (); - virtual void update (double dt); - -private: - - Gyro _gyro; - double _last_heading_deg; - - string name; - int num; - //string vacuum_system; - - SGPropertyNode_ptr _offset_node; - SGPropertyNode_ptr _heading_in_node; - SGPropertyNode_ptr _serviceable_node; - SGPropertyNode_ptr _heading_out_node; - SGPropertyNode_ptr _electrical_node; - SGPropertyNode_ptr _error_node; - SGPropertyNode_ptr _nav1_error_node; -}; - -#endif // __INSTRUMENTS_HEADING_INDICATOR_ELEC_HXX +// heading_indicator.hxx - a vacuum-powered heading indicator. +// Written by David Megginson, started 2002. +// +// This file is in the Public Domain and comes with no warranty. + + +#ifndef __INSTRUMENTS_HEADING_INDICATOR_ELEC_HXX +#define __INSTRUMENTS_HEADING_INDICATOR_ELEC_HXX 1 + + +#include +#include +#include + +#include "gyro.hxx" + + +/** + * Model an electrically-powered heading indicator. + * + * Input properties: + * + * /instrumentation/"name"/serviceable + * /instrumentation/"name"/spin + * /instrumentation/"name"/offset-deg + * /orientation/heading-deg + * /systems/electrical/outputs/DG + * + * Output properties: + * + * /instrumentation/"name"/indicated-heading-deg + */ +class HeadingIndicatorDG : public SGSubsystem +{ + +public: + + HeadingIndicatorDG ( SGPropertyNode *node ); + HeadingIndicatorDG (); + virtual ~HeadingIndicatorDG (); + + virtual void init (); + virtual void bind (); + virtual void unbind (); + virtual void update (double dt); + +private: + + Gyro _gyro; + double _last_heading_deg; + + string name; + int num; + //string vacuum_system; + + SGPropertyNode_ptr _offset_node; + SGPropertyNode_ptr _heading_in_node; + SGPropertyNode_ptr _serviceable_node; + SGPropertyNode_ptr _heading_out_node; + SGPropertyNode_ptr _electrical_node; + SGPropertyNode_ptr _error_node; + SGPropertyNode_ptr _nav1_error_node; + SGPropertyNode_ptr _align_node; + SGPropertyNode_ptr _yaw_rate_node; + SGPropertyNode_ptr _heading_bug_error_node; + SGPropertyNode_ptr _g_node; +}; + +#endif // __INSTRUMENTS_HEADING_INDICATOR_ELEC_HXX