src/Instrumentation/heading_indicator.cxx

   1 // heading_indicator.cxx - a vacuum-powered heading indicator.
   2 // Written by David Megginson, started 2002.
   3 //
   4 // This file is in the Public Domain and comes with no warranty.
   5
   6 #include <simgear/compiler.h>
   7 #include <simgear/sg_inlines.h>
   8 #include <iostream>
   9 #include <string>
  10 #include <sstream>
  11
  12 #include "heading_indicator.hxx"
  13 #include <Main/fg_props.hxx>
  14 #include <Main/util.hxx>
  15
  16
  17 HeadingIndicator::HeadingIndicator ( SGPropertyNode *node )
  18     :
  19     _name(node->getStringValue("name", "heading-indicator")),
  20     _num(node->getIntValue("number", 0)),
  21     _suction(node->getStringValue("suction", "/systems/vacuum/suction-inhg"))
  22 {
  23 }
  24
  25 HeadingIndicator::~HeadingIndicator ()
  26 {
  27 }
  28
  29 void
  30 HeadingIndicator::init ()
  31 {
  32     string branch;
  33     branch = "/instrumentation/" + _name;
  34
  35     SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
  36     _offset_node = node->getChild("offset-deg", 0, true);
  37     _heading_in_node = fgGetNode("/orientation/heading-deg", true);
  38     _suction_node = fgGetNode(_suction.c_str(), true);
  39     _heading_out_node = node->getChild("indicated-heading-deg", 0, true);
  40     _heading_bug_error_node = node->getChild("heading-bug-error-deg", 0, true);
  41     _heading_bug_node = node->getChild("heading-bug-deg", 0, true);
  42     _last_heading_deg = (_heading_in_node->getDoubleValue() +
  43                          _offset_node->getDoubleValue());
  44 }
  45
  46 void
  47 HeadingIndicator::bind ()
  48 {
  49     std::ostringstream temp;
  50     string branch;
  51     temp << _num;
  52     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
  53
  54     fgTie((branch + "/serviceable").c_str(),
  55           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  56     fgTie((branch + "/spin").c_str(),
  57           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  58 }
  59
  60 void
  61 HeadingIndicator::unbind ()
  62 {
  63     std::ostringstream temp;
  64     string branch;
  65     temp << _num;
  66     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
  67
  68     fgUntie((branch + "/serviceable").c_str());
  69     fgUntie((branch + "/spin").c_str());
  70 }
  71
  72 void
  73 HeadingIndicator::update (double dt)
  74 {
  75                                 // Get the spin from the gyro
  76     _gyro.set_power_norm(_suction_node->getDoubleValue()/5.0);
  77     _gyro.update(dt);
  78     double spin = _gyro.get_spin_norm();
  79
  80                                 // Next, calculate time-based precession
  81     double offset = _offset_node->getDoubleValue();
  82     offset -= dt * (0.25 / 60.0); // 360deg/day
  83     SG_NORMALIZE_RANGE(offset, -360.0, 360.0);
  84
  85                                 // TODO: movement-induced error
  86
  87                                 // Next, calculate the indicated heading,
  88                                 // introducing errors.
  89     double factor = 100 * (spin * spin * spin * spin * spin * spin);
  90     double heading = _heading_in_node->getDoubleValue();
  91
  92                                 // Now, we have to get the current
  93                                 // heading and the last heading into
  94                                 // the same range.
  95     while ((heading - _last_heading_deg) > 180)
  96         _last_heading_deg += 360;
  97     while ((heading - _last_heading_deg) < -180)
  98         _last_heading_deg -= 360;
  99
 100     heading = fgGetLowPass(_last_heading_deg, heading, dt * factor);
 101     _last_heading_deg = heading;
 102
 103     heading += offset;
 104     SG_NORMALIZE_RANGE(heading, 0.0, 360.0);
 105
 106     _heading_out_node->setDoubleValue(heading);
 107
 108     // Calculate heading bug error normalized to +/- 180.0
 109     double heading_bug = _heading_bug_node->getDoubleValue();
 110     double diff = heading_bug - heading;
 111
 112     SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
 113     _heading_bug_error_node->setDoubleValue( diff );
 114 }
 115
 116 // end of heading_indicator.cxx