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