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 "heading_indicator.hxx"
   7 #include <Main/fg_props.hxx>
   8 #include <Main/util.hxx>
   9
  10
  11 HeadingIndicator::HeadingIndicator ( SGPropertyNode *node )
  12     :
  13     name("heading-indicator"),
  14     num(0),
  15     vacuum_system("/systems/vacuum")
  16 {
  17     int i;
  18     for ( i = 0; i < node->nChildren(); ++i ) {
  19         SGPropertyNode *child = node->getChild(i);
  20         string cname = child->getName();
  21         string cval = child->getStringValue();
  22         if ( cname == "name" ) {
  23             name = cval;
  24         } else if ( cname == "number" ) {
  25             num = child->getIntValue();
  26         } else if ( cname == "vacuum-system" ) {
  27             vacuum_system = cval;
  28         } else {
  29             SG_LOG( SG_INSTR, SG_WARN, "Error in heading-indicator config logic" );
  30             if ( name.length() ) {
  31                 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
  32             }
  33         }
  34     }
  35 }
  36
  37 HeadingIndicator::HeadingIndicator ()
  38 {
  39 }
  40
  41 HeadingIndicator::~HeadingIndicator ()
  42 {
  43 }
  44
  45 void
  46 HeadingIndicator::init ()
  47 {
  48     string branch;
  49     branch = "/instrumentation/" + name;
  50     vacuum_system += "/suction-inhg";
  51
  52     SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
  53     _offset_node = node->getChild("offset-deg", 0, true);
  54     _heading_in_node = fgGetNode("/orientation/heading-deg", true);
  55     _suction_node = fgGetNode(vacuum_system.c_str(), true);
  56     _heading_out_node = node->getChild("indicated-heading-deg", 0, true);
  57     _last_heading_deg = (_heading_in_node->getDoubleValue() +
  58                          _offset_node->getDoubleValue());
  59 }
  60
  61 void
  62 HeadingIndicator::bind ()
  63 {
  64     string branch;
  65     branch = "/instrumentation/" + name + "/serviceable";
  66     fgTie(branch.c_str(),
  67           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  68     branch = "/instrumentation/" + name + "/spin";
  69     fgTie(branch.c_str(),
  70           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  71 }
  72
  73 void
  74 HeadingIndicator::unbind ()
  75 {
  76     string branch;
  77     branch = "/instrumentation/" + name + "/serviceable";
  78     fgUntie(branch.c_str());
  79     branch = "/instrumentation/" + name + "/spin";
  80     fgUntie(branch.c_str());
  81 }
  82
  83 void
  84 HeadingIndicator::update (double dt)
  85 {
  86                                 // Get the spin from the gyro
  87     _gyro.set_power_norm(_suction_node->getDoubleValue()/5.0);
  88     _gyro.update(dt);
  89     double spin = _gyro.get_spin_norm();
  90
  91                                 // Next, calculate time-based precession
  92     double offset = _offset_node->getDoubleValue();
  93     offset -= dt * (0.25 / 60.0); // 360deg/day
  94     while (offset < -360)
  95         offset += 360;
  96     while (offset > 360)
  97         offset -= 360;
  98     _offset_node->setDoubleValue(offset);
  99
 100                                 // TODO: movement-induced error
 101
 102                                 // Next, calculate the indicated heading,
 103                                 // introducing errors.
 104     double factor = 0.01 / (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     while (heading < 0)
 120         heading += 360;
 121     while (heading > 360)
 122         heading -= 360;
 123
 124     _heading_out_node->setDoubleValue(heading);
 125 }
 126
 127 // end of heading_indicator.cxx