1 // heading_indicator_dg.cxx - a Directional Gyro (DG) compass.
2 // Based on the vacuum driven Heading Indicator Written by David Megginson,
5 // Written by Vivian Meazza, started 2005.
7 // This file is in the Public Domain and comes with no warranty.
9 #include <simgear/compiler.h>
14 #include <Main/fg_props.hxx>
15 #include <Main/util.hxx>
17 #include "heading_indicator_dg.hxx"
20 HeadingIndicatorDG::HeadingIndicatorDG ( SGPropertyNode *node ) :
21 name("heading-indicator-dg"),
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 DG heading-indicator config logic" );
35 if ( name.length() ) {
36 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
42 HeadingIndicatorDG::HeadingIndicatorDG ()
46 HeadingIndicatorDG::~HeadingIndicatorDG ()
51 HeadingIndicatorDG::init ()
54 branch = "/instrumentation/" + name;
56 _heading_in_node = fgGetNode("/orientation/heading-deg", true);
57 SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
58 _offset_node = node->getChild("offset-deg", 0, true);
59 _serviceable_node = node->getChild("serviceable", 0, true);
60 _error_node = node->getChild("heading-bug-error-deg", 0, true);
61 _nav1_error_node = node->getChild("nav1-course-error-deg", 0, true);
62 _heading_out_node = node->getChild("indicated-heading-deg", 0, true);
63 _last_heading_deg = (_heading_in_node->getDoubleValue() +
64 _offset_node->getDoubleValue());
65 _electrical_node = fgGetNode("/systems/electrical/outputs/DG", true);
69 HeadingIndicatorDG::bind ()
71 std::ostringstream temp;
74 branch = "/instrumentation/" + name + "[" + temp.str() + "]";
76 fgTie((branch + "/serviceable").c_str(),
77 &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
78 fgTie((branch + "/spin").c_str(),
79 &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
83 HeadingIndicatorDG::unbind ()
85 std::ostringstream temp;
88 branch = "/instrumentation/" + name + "[" + temp.str() + "]";
90 fgUntie((branch + "/serviceable").c_str());
91 fgUntie((branch + "/spin").c_str());
95 HeadingIndicatorDG::update (double dt)
97 // Get the spin from the gyro
98 _gyro.set_power_norm(_electrical_node->getDoubleValue());
101 double spin = _gyro.get_spin_norm();
103 // No time-based precession for a flux gate compass
107 // TODO: movement-induced error
109 // Next, calculate the indicated heading,
110 // introducing errors.
111 double factor = 0.01 / (spin * spin * spin * spin * spin * spin);
112 double heading = _heading_in_node->getDoubleValue();
114 // Now, we have to get the current
115 // heading and the last heading into
117 while ((heading - _last_heading_deg) > 180)
118 _last_heading_deg += 360;
119 while ((heading - _last_heading_deg) < -180)
120 _last_heading_deg -= 360;
122 heading = fgGetLowPass(_last_heading_deg, heading, dt/factor);
123 _last_heading_deg = heading;
128 while (heading > 360)
131 _heading_out_node->setDoubleValue(heading);
133 // calculate the difference between the indicacted heading
134 // and the selected heading for use with an autopilot
135 static SGPropertyNode *bnode
136 = fgGetNode( "/autopilot/settings/heading-bug-deg", false );
138 double diff = bnode->getDoubleValue() - heading;
139 if ( diff < -180.0 ) { diff += 360.0; }
140 if ( diff > 180.0 ) { diff -= 360.0; }
141 _error_node->setDoubleValue( diff );
143 // calculate the difference between the indicated heading
144 // and the selected nav1 radial for use with an autopilot
145 SGPropertyNode *nnode
146 = fgGetNode( "/instrumentation/nav/radials/selected-deg", false );
148 double diff = nnode->getDoubleValue() - heading;
149 if ( diff < -180.0 ) { diff += 360.0; }
150 if ( diff > 180.0 ) { diff -= 360.0; }
151 _nav1_error_node->setDoubleValue( diff );
155 // end of heading_indicator_fg.cxx