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