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_AUTOPILOT, SG_WARN, "Error in heading-indicator config logic" );
  30             if ( name.length() ) {
  31                 SG_LOG( SG_AUTOPILOT, 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     //_serviceable_node->setBoolValue(true);
  61 }
  62
  63 void
  64 HeadingIndicator::bind ()
  65 {
  66     string branch;
  67     branch = "/instrumentation/" + name + "/serviceable";
  68     fgTie(branch.c_str(),
  69           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  70     branch = "/instrumentation/" + name + "/spin";
  71     fgTie(branch.c_str(),
  72           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  73 }
  74
  75 void
  76 HeadingIndicator::unbind ()
  77 {
  78     string branch;
  79     branch = "/instrumentation/" + name + "/serviceable";
  80     fgUntie(branch.c_str());
  81     branch = "/instrumentation/" + name + "/spin";
  82     fgUntie(branch.c_str());
  83 }
  84
  85 void
  86 HeadingIndicator::update (double dt)
  87 {
  88                                 // Get the spin from the gyro
  89     _gyro.set_power_norm(_suction_node->getDoubleValue()/5.0);
  90     _gyro.update(dt);
  91     double spin = _gyro.get_spin_norm();
  92
  93                                 // Next, calculate time-based precession
  94     double offset = _offset_node->getDoubleValue();
  95     offset -= dt * (0.25 / 60.0); // 360deg/day
  96     while (offset < -360)
  97         offset += 360;
  98     while (offset > 360)
  99         offset -= 360;
 100     _offset_node->setDoubleValue(offset);
 101
 102                                 // TODO: movement-induced error
 103
 104                                 // Next, calculate the indicated heading,
 105                                 // introducing errors.
 106     double factor = 0.01 / (spin * spin * spin * spin * spin * spin);
 107     double heading = _heading_in_node->getDoubleValue();
 108
 109                                 // Now, we have to get the current
 110                                 // heading and the last heading into
 111                                 // the same range.
 112     while ((heading - _last_heading_deg) > 180)
 113         _last_heading_deg += 360;
 114     while ((heading - _last_heading_deg) < -180)
 115         _last_heading_deg -= 360;
 116
 117     heading = fgGetLowPass(_last_heading_deg, heading, dt/factor);
 118     _last_heading_deg = heading;
 119
 120     heading += offset;
 121     while (heading < 0)
 122         heading += 360;
 123     while (heading > 360)
 124         heading -= 360;
 125
 126     _heading_out_node->setDoubleValue(heading);
 127 }
 128
 129 // end of heading_indicator.cxx