1 // heading_indicator_fg.cxx - a flux_gate compass.
2 // Based on the vacuum driven Heading Indicator Written by David Megginson, started 2002.
4 // Written by Vivian Meazza, started 2005.
6 // This file is in the Public Domain and comes with no warranty.
8 #include <simgear/compiler.h>
13 #include "heading_indicator_fg.hxx"
14 #include <Main/fg_props.hxx>
15 #include <Main/util.hxx>
19 HeadingIndicatorFG::HeadingIndicatorFG ( SGPropertyNode *node )
21 name("heading-indicator-fg"),
25 for ( i = 0; i < node->nChildren(); ++i ) {
26 SGPropertyNode *child = node->getChild(i);
27 string cname = child->getName();
28 string cval = child->getStringValue();
29 if ( cname == "name" ) {
31 } else if ( cname == "number" ) {
32 num = child->getIntValue();
34 SG_LOG( SG_INSTR, SG_WARN, "Error in flux-gate heading-indicator config logic" );
35 if ( name.length() ) {
36 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
42 HeadingIndicatorFG::HeadingIndicatorFG ()
46 HeadingIndicatorFG::~HeadingIndicatorFG ()
51 HeadingIndicatorFG::init ()
54 branch = "/instrumentation/" + name;
56 _heading_in_node = fgGetNode("/orientation/heading-deg", true);
58 SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
59 _offset_node = node->getChild("offset-deg", 0, true);
60 _serviceable_node = node->getChild("serviceable", 0, true);
61 _error_node = node->getChild("heading-bug-error-deg", 0, true);
62 _nav1_error_node = node->getChild("nav1-course-error-deg", 0, true);
63 _heading_out_node = node->getChild("indicated-heading-deg", 0, true);
64 _off_node = node->getChild("off-flag", 0, true);
66 _last_heading_deg = (_heading_in_node->getDoubleValue() +
67 _offset_node->getDoubleValue());
68 _electrical_node = fgGetNode("/systems/electrical/outputs/DG", true);
72 HeadingIndicatorFG::bind ()
74 std::ostringstream temp;
77 branch = "/instrumentation/" + name + "[" + temp.str() + "]";
79 fgTie((branch + "/serviceable").c_str(),
80 &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
81 fgTie((branch + "/spin").c_str(),
82 &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
87 HeadingIndicatorFG::unbind ()
89 std::ostringstream temp;
92 branch = "/instrumentation/" + name + "[" + temp.str() + "]";
94 fgUntie((branch + "/serviceable").c_str());
95 fgUntie((branch + "/spin").c_str());
99 HeadingIndicatorFG::update (double dt)
101 // Get the spin from the gyro
102 _gyro.set_power_norm(_electrical_node->getDoubleValue());
104 double spin = _gyro.get_spin_norm();
106 if ( _electrical_node->getDoubleValue() > 0 && spin >= 0.25) {
107 _off_node->setBoolValue(false);
109 _off_node->setBoolValue(true);
113 // No time-based precession for a flux gate compass
114 // We just use offset to get the magvar
115 double offset = _offset_node->getDoubleValue();
117 // TODO: movement-induced error
119 // Next, calculate the indicated heading,
120 // introducing errors.
121 double factor = 100 * (spin * spin * spin * spin * spin * spin);
122 double heading = _heading_in_node->getDoubleValue();
124 // Now, we have to get the current
125 // heading and the last heading into
127 if ((heading - _last_heading_deg) > 180)
128 _last_heading_deg += 360;
129 if ((heading - _last_heading_deg) < -180)
130 _last_heading_deg -= 360;
132 heading = fgGetLowPass(_last_heading_deg, heading, dt * factor);
133 _last_heading_deg = heading;
142 _heading_out_node->setDoubleValue(heading);
144 // calculate the difference between the indicated heading
145 // and the selected heading for use with an autopilot
146 static SGPropertyNode *bnode
147 = fgGetNode( "/autopilot/settings/heading-bug-deg", false );
150 diff = bnode->getDoubleValue() - heading;
151 if ( diff < -180.0 ) { diff += 360.0; }
152 if ( diff > 180.0 ) { diff -= 360.0; }
153 _error_node->setDoubleValue( diff );
155 // calculate the difference between the indicated heading
156 // and the selected nav1 radial for use with an autopilot
157 SGPropertyNode *nnode
158 = fgGetNode( "/instrumentation/nav/radials/selected-deg", true );
161 ndiff = nnode->getDoubleValue() - heading;
162 if ( ndiff < -180.0 ) { ndiff += 360.0; }
163 if ( ndiff > 180.0 ) { ndiff -= 360.0; }
164 _nav1_error_node->setDoubleValue( ndiff );
170 // end of heading_indicator_fg.cxx