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