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_AUTOPILOT, SG_WARN, "Error in heading-indicator config logic" );
30 if ( name.length() ) {
31 SG_LOG( SG_AUTOPILOT, 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());
60 //_serviceable_node->setBoolValue(true);
64 HeadingIndicator::bind ()
67 branch = "/instrumentation/" + name + "/serviceable";
69 &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
70 branch = "/instrumentation/" + name + "/spin";
72 &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
76 HeadingIndicator::unbind ()
79 branch = "/instrumentation/" + name + "/serviceable";
80 fgUntie(branch.c_str());
81 branch = "/instrumentation/" + name + "/spin";
82 fgUntie(branch.c_str());
86 HeadingIndicator::update (double dt)
88 // Get the spin from the gyro
89 _gyro.set_power_norm(_suction_node->getDoubleValue()/5.0);
91 double spin = _gyro.get_spin_norm();
93 // Next, calculate time-based precession
94 double offset = _offset_node->getDoubleValue();
95 offset -= dt * (0.25 / 60.0); // 360deg/day
100 _offset_node->setDoubleValue(offset);
102 // TODO: movement-induced error
104 // Next, calculate the indicated heading,
105 // introducing errors.
106 double factor = 0.01 / (spin * spin * spin * spin * spin * spin);
107 double heading = _heading_in_node->getDoubleValue();
109 // Now, we have to get the current
110 // heading and the last heading into
112 while ((heading - _last_heading_deg) > 180)
113 _last_heading_deg += 360;
114 while ((heading - _last_heading_deg) < -180)
115 _last_heading_deg -= 360;
117 heading = fgGetLowPass(_last_heading_deg, heading, dt/factor);
118 _last_heading_deg = heading;
123 while (heading > 360)
126 _heading_out_node->setDoubleValue(heading);
129 // end of heading_indicator.cxx