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     _serviceable_node =
  23         fgGetNode("/instrumentation/heading-indicator/serviceable", true);
  24     _offset_node =
  25         fgGetNode("/instrumentation/heading-indicator/offset-deg", true);
  26     _heading_in_node = fgGetNode("/orientation/heading-deg", true);
  27     _suction_node = fgGetNode("/systems/vacuum[0]/suction-inhg", true);
  28     _heading_out_node =
  29         fgGetNode("/instrumentation/heading-indicator/indicated-heading-deg",
  30                   true);
  31     _last_heading_deg = (_heading_in_node->getDoubleValue() +
  32                          _offset_node->getDoubleValue());
  33 }
  34
  35 void
  36 HeadingIndicator::bind ()
  37 {
  38 }
  39
  40 void
  41 HeadingIndicator::unbind ()
  42 {
  43 }
  44
  45 void
  46 HeadingIndicator::update (double dt)
  47 {
  48                                 // First, calculate the bogo-spin from 0 to 1.
  49                                 // All numbers are made up.
  50
  51     _spin -= 0.005 * dt;         // spin decays every 0.5% every second.
  52
  53                                 // spin increases up to 25% every second
  54                                 // if suction is available and the gauge
  55                                 // is serviceable.
  56     if (_serviceable_node->getBoolValue()) {
  57         double suction = _suction_node->getDoubleValue();
  58         double step = 0.25 * (suction / 5.0) * dt;
  59         if ((_spin + step) <= (suction / 5.0))
  60             _spin += step;
  61     }
  62     if (_spin > 1.0)
  63         _spin = 1.0;
  64     else if (_spin < 0.0)
  65         _spin = 0.0;
  66
  67                                 // Next, calculate time-based precession
  68     double offset = _offset_node->getDoubleValue();
  69     offset -= dt * (0.25 / 60.0); // 360deg/day
  70     while (offset < -360)
  71         offset += 360;
  72     while (offset > 360)
  73         offset -= 360;
  74     _offset_node->setDoubleValue(offset);
  75
  76                                 // TODO: movement-induced error
  77
  78                                 // Next, calculate the indicated heading,
  79                                 // introducing errors.
  80     double factor = 0.01 / (_spin * _spin * _spin * _spin * _spin * _spin);
  81     double heading = _heading_in_node->getDoubleValue();
  82     heading = fgGetLowPass(_last_heading_deg, heading, dt/factor);
  83     _last_heading_deg = heading;
  84
  85     heading += offset;
  86     while (heading < 0)
  87         heading += 360;
  88     while (heading > 360)
  89         heading -= 360;
  90
  91     _heading_out_node->setDoubleValue(heading);
  92 }
  93
  94 // end of heading_indicator.cxx