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 ()
  12 {
  13 }
  14
  15 HeadingIndicator::~HeadingIndicator ()
  16 {
  17 }
  18
  19 void
  20 HeadingIndicator::init ()
  21 {
  22     _offset_node =
  23         fgGetNode("/instrumentation/heading-indicator/offset-deg", true);
  24     _heading_in_node = fgGetNode("/orientation/heading-deg", true);
  25     _suction_node = fgGetNode("/systems/vacuum[0]/suction-inhg", true);
  26     _heading_out_node =
  27         fgGetNode("/instrumentation/heading-indicator/indicated-heading-deg",
  28                   true);
  29     _last_heading_deg = (_heading_in_node->getDoubleValue() +
  30                          _offset_node->getDoubleValue());
  31 }
  32
  33 void
  34 HeadingIndicator::bind ()
  35 {
  36     fgTie("/instrumentation/heading-indicator/serviceable",
  37           &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
  38     fgTie("/instrumentation/heading-indicator/spin",
  39           &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
  40 }
  41
  42 void
  43 HeadingIndicator::unbind ()
  44 {
  45     fgUntie("/instrumentation/heading-indicator/serviceable");
  46     fgUntie("/instrumentation/heading-indicator/spin");
  47 }
  48
  49 void
  50 HeadingIndicator::update (double dt)
  51 {
  52                                 // Get the spin from the gyro
  53     _gyro.set_power_norm(_suction_node->getDoubleValue()/5.0);
  54     _gyro.update(dt);
  55     double spin = _gyro.get_spin_norm();
  56
  57                                 // Next, calculate time-based precession
  58     double offset = _offset_node->getDoubleValue();
  59     offset -= dt * (0.25 / 60.0); // 360deg/day
  60     while (offset < -360)
  61         offset += 360;
  62     while (offset > 360)
  63         offset -= 360;
  64     _offset_node->setDoubleValue(offset);
  65
  66                                 // TODO: movement-induced error
  67
  68                                 // Next, calculate the indicated heading,
  69                                 // introducing errors.
  70     double factor = 0.01 / (spin * spin * spin * spin * spin * spin);
  71     double heading = _heading_in_node->getDoubleValue();
  72
  73                                 // Now, we have to get the current
  74                                 // heading and the last heading into
  75                                 // the same range.
  76     while ((heading - _last_heading_deg) > 180)
  77         _last_heading_deg += 360;
  78     while ((heading - _last_heading_deg) < -180)
  79         _last_heading_deg -= 360;
  80
  81     heading = fgGetLowPass(_last_heading_deg, heading, dt/factor);
  82     _last_heading_deg = heading;
  83
  84     heading += offset;
  85     while (heading < 0)
  86         heading += 360;
  87     while (heading > 360)
  88         heading -= 360;
  89
  90     _heading_out_node->setDoubleValue(heading);
  91 }
  92
  93 // end of heading_indicator.cxx