1 // heading_indicator.cxx - a vacuum-powered heading indicator.
2 // Written by David Megginson, started 2002.
4 // This file is in the Public Domain and comes with no warranty.
6 #include "heading_indicator.hxx"
7 #include <Main/fg_props.hxx>
8 #include <Main/util.hxx>
11 HeadingIndicator::HeadingIndicator ( SGPropertyNode *node )
13 name("heading-indicator"),
15 vacuum_system("/systems/vacuum")
18 for ( i = 0; i < node->nChildren(); ++i ) {
19 SGPropertyNode *child = node->getChild(i);
20 string cname = child->getName();
21 string cval = child->getStringValue();
22 if ( cname == "name" ) {
24 } else if ( cname == "number" ) {
25 num = child->getIntValue();
26 } else if ( cname == "vacuum-system" ) {
29 SG_LOG( SG_INSTR, SG_WARN, "Error in heading-indicator config logic" );
30 if ( name.length() ) {
31 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
37 HeadingIndicator::HeadingIndicator ()
41 HeadingIndicator::~HeadingIndicator ()
46 HeadingIndicator::init ()
49 branch = "/instrumentation/" + name;
50 vacuum_system += "/suction-inhg";
52 SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
53 _offset_node = node->getChild("offset-deg", 0, true);
54 _heading_in_node = fgGetNode("/orientation/heading-deg", true);
55 _suction_node = fgGetNode(vacuum_system.c_str(), true);
56 _heading_out_node = node->getChild("indicated-heading-deg", 0, true);
57 _last_heading_deg = (_heading_in_node->getDoubleValue() +
58 _offset_node->getDoubleValue());
62 HeadingIndicator::bind ()
65 branch = "/instrumentation/" + name + "/serviceable";
67 &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
68 branch = "/instrumentation/" + name + "/spin";
70 &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
74 HeadingIndicator::unbind ()
77 branch = "/instrumentation/" + name + "/serviceable";
78 fgUntie(branch.c_str());
79 branch = "/instrumentation/" + name + "/spin";
80 fgUntie(branch.c_str());
84 HeadingIndicator::update (double dt)
86 // Get the spin from the gyro
87 _gyro.set_power_norm(_suction_node->getDoubleValue()/5.0);
89 double spin = _gyro.get_spin_norm();
91 // Next, calculate time-based precession
92 double offset = _offset_node->getDoubleValue();
93 offset -= dt * (0.25 / 60.0); // 360deg/day
98 _offset_node->setDoubleValue(offset);
100 // TODO: movement-induced error
102 // Next, calculate the indicated heading,
103 // introducing errors.
104 double factor = 0.01 / (spin * spin * spin * spin * spin * spin);
105 double heading = _heading_in_node->getDoubleValue();
107 // Now, we have to get the current
108 // heading and the last heading into
110 while ((heading - _last_heading_deg) > 180)
111 _last_heading_deg += 360;
112 while ((heading - _last_heading_deg) < -180)
113 _last_heading_deg -= 360;
115 heading = fgGetLowPass(_last_heading_deg, heading, dt/factor);
116 _last_heading_deg = heading;
121 while (heading > 360)
124 _heading_out_node->setDoubleValue(heading);
127 // end of heading_indicator.cxx