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 STL_IOSTREAM
   8 #include STL_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("heading-indicator"),
  19     num(0),
  20     vacuum_system("/systems/vacuum")
  21 {
  22     int i;
  23     for ( i = 0; i < node->nChildren(); ++i ) {
  24         SGPropertyNode *child = node->getChild(i);
  25         string cname = child->getName();
  26         string cval = child->getStringValue();
  27         if ( cname == "name" ) {
  28             name = cval;
  29         } else if ( cname == "number" ) {
  30             num = child->getIntValue();
  31         } else if ( cname == "vacuum-system" ) {
  32             vacuum_system = cval;
  33         } else {
  34             SG_LOG( SG_INSTR, SG_WARN, "Error in heading-indicator config logic" );
  35             if ( name.length() ) {
  36                 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
  37             }
  38         }
  39     }
  40 }
  41
  42 HeadingIndicator::HeadingIndicator ()
  43 {
  44 }
  45
  46 HeadingIndicator::~HeadingIndicator ()
  47 {
  48 }
  49
  50 void
  51 HeadingIndicator::init ()
  52 {
  53     string branch;
  54     branch = "/instrumentation/" + name;
  55     vacuum_system += "/suction-inhg";
  56
  57     SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
  58     _offset_node = node->getChild("offset-deg", 0, true);
  59     _heading_in_node = fgGetNode("/orientation/heading-deg", true);
  60     _suction_node = fgGetNode(vacuum_system.c_str(), true);
  61     _heading_out_node = node->getChild("indicated-heading-deg", 0, true);
  62     _last_heading_deg = (_heading_in_node->getDoubleValue() +
  63                          _offset_node->getDoubleValue());
  64 }
  65
  66 void
  67 HeadingIndicator::bind ()
  68 {
  69     std::ostringstream temp;
  70     string branch;
  71     temp << num;
  72     branch = "/instrumentation/" + name + "[" + temp.str() + "]";
  73
  74     fgTie((branch + "/serviceable").c_str(),
  75           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  76     fgTie((branch + "/spin").c_str(),
  77           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  78 }
  79
  80 void
  81 HeadingIndicator::unbind ()
  82 {
  83     std::ostringstream temp;
  84     string branch;
  85     temp << num;
  86     branch = "/instrumentation/" + name + "[" + temp.str() + "]";
  87
  88     fgUntie((branch + "/serviceable").c_str());
  89     fgUntie((branch + "/spin").c_str());
  90 }
  91
  92 void
  93 HeadingIndicator::update (double dt)
  94 {
  95                                 // Get the spin from the gyro
  96     _gyro.set_power_norm(_suction_node->getDoubleValue()/5.0);
  97     _gyro.update(dt);
  98     double spin = _gyro.get_spin_norm();
  99
 100                                 // Next, calculate time-based precession
 101     double offset = _offset_node->getDoubleValue();
 102     offset -= dt * (0.25 / 60.0); // 360deg/day
 103     while (offset < -360)
 104         offset += 360;
 105     while (offset > 360)
 106         offset -= 360;
 107     _offset_node->setDoubleValue(offset);
 108
 109                                 // TODO: movement-induced error
 110
 111                                 // Next, calculate the indicated heading,
 112                                 // introducing errors.
 113     double factor = 0.01 / (spin * spin * spin * spin * spin * spin);
 114     double heading = _heading_in_node->getDoubleValue();
 115
 116                                 // Now, we have to get the current
 117                                 // heading and the last heading into
 118                                 // the same range.
 119     while ((heading - _last_heading_deg) > 180)
 120         _last_heading_deg += 360;
 121     while ((heading - _last_heading_deg) < -180)
 122         _last_heading_deg -= 360;
 123
 124     heading = fgGetLowPass(_last_heading_deg, heading, dt/factor);
 125     _last_heading_deg = heading;
 126
 127     heading += offset;
 128     while (heading < 0)
 129         heading += 360;
 130     while (heading > 360)
 131         heading -= 360;
 132
 133     _heading_out_node->setDoubleValue(heading);
 134 }
 135
 136 // end of heading_indicator.cxx