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