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     _last_heading_deg = (_heading_in_node->getDoubleValue() +
  52                          _offset_node->getDoubleValue());
  53 }
  54
  55 void
  56 HeadingIndicator::bind ()
  57 {
  58     std::ostringstream temp;
  59     std::string branch;
  60     temp << _num;
  61     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
  62
  63     fgTie((branch + "/serviceable").c_str(),
  64           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  65     fgTie((branch + "/spin").c_str(),
  66           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  67 }
  68
  69 void
  70 HeadingIndicator::unbind ()
  71 {
  72     std::ostringstream temp;
  73     std::string branch;
  74     temp << _num;
  75     branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
  76
  77     fgUntie((branch + "/serviceable").c_str());
  78     fgUntie((branch + "/spin").c_str());
  79 }
  80
  81 void
  82 HeadingIndicator::update (double dt)
  83 {
  84                                 // Get the spin from the gyro
  85     _gyro.set_power_norm(_suction_node->getDoubleValue()/5.0);
  86     _gyro.update(dt);
  87     double spin = _gyro.get_spin_norm();
  88
  89                                 // Next, calculate time-based precession
  90     double offset = _offset_node->getDoubleValue();
  91     offset -= dt * (0.25 / 60.0); // 360deg/day
  92     SG_NORMALIZE_RANGE(offset, -360.0, 360.0);
  93
  94                                 // TODO: movement-induced error
  95
  96                                 // Next, calculate the indicated heading,
  97                                 // introducing errors.
  98     double factor = 100 * (spin * spin * spin * spin * spin * spin);
  99     double heading = _heading_in_node->getDoubleValue();
 100
 101                                 // Now, we have to get the current
 102                                 // heading and the last heading into
 103                                 // the same range.
 104     while ((heading - _last_heading_deg) > 180)
 105         _last_heading_deg += 360;
 106     while ((heading - _last_heading_deg) < -180)
 107         _last_heading_deg -= 360;
 108
 109     heading = fgGetLowPass(_last_heading_deg, heading, dt * factor);
 110     _last_heading_deg = heading;
 111
 112     heading += offset;
 113     SG_NORMALIZE_RANGE(heading, 0.0, 360.0);
 114
 115     _heading_out_node->setDoubleValue(heading);
 116
 117     // Calculate heading bug error normalized to +/- 180.0
 118     double heading_bug = _heading_bug_node->getDoubleValue();
 119     double diff = heading_bug - heading;
 120
 121     SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
 122     _heading_bug_error_node->setDoubleValue( diff );
 123 }
 124
 125 // end of heading_indicator.cxx