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