src/Instrumentation/mag_compass.cxx

   1 // mag_compass.cxx - a magnetic compass.
   2 // Written by David Megginson, started 2003.
   3 //
   4 // This file is in the Public Domain and comes with no warranty.
   5
   6 // This implementation is derived from an earlier one by Alex Perry,
   7 // which appeared in src/Cockpit/steam.cxx
   8
   9 #ifdef HAVE_CONFIG_H
  10 #  include <config.h>
  11 #endif
  12
  13 #include <plib/sg.h>
  14
  15 #include "mag_compass.hxx"
  16 #include <Main/fg_props.hxx>
  17 #include <Main/util.hxx>
  18
  19
  20 MagCompass::MagCompass ()
  21     : _error_deg(0.0),
  22       _rate_degps(0.0)
  23 {
  24 }
  25
  26 MagCompass::~MagCompass ()
  27 {
  28 }
  29
  30 void
  31 MagCompass::init ()
  32 {
  33     _serviceable_node =
  34         fgGetNode("/instrumentation/magnetic-compass/serviceable", true);
  35     _heading_node =
  36         fgGetNode("/orientation/heading-deg", true);
  37     _beta_node =
  38         fgGetNode("/orientation/side-slip-deg", true);
  39     _variation_node =
  40         fgGetNode("/environment/magnetic-variation-deg", true);
  41     _dip_node =
  42         fgGetNode("/environment/magnetic-dip-deg", true);
  43     _north_accel_node =
  44         fgGetNode("/accelerations/ned/north-accel-fps_sec", true);
  45     _east_accel_node =
  46         fgGetNode("/accelerations/ned/east-accel-fps_sec", true);
  47     _down_accel_node =
  48         fgGetNode("/accelerations/ned/down-accel-fps_sec", true);
  49     _out_node =
  50         fgGetNode("/instrumentation/magnetic-compass/indicated-heading-deg",
  51                   true);
  52 }
  53
  54 void
  55 MagCompass::update (double delta_time_sec)
  56 {
  57                                 // algorithm from Alex Perry
  58                                 // possibly broken by David Megginson
  59
  60                                 // don't update if it's broken
  61     if (!_serviceable_node->getBoolValue())
  62         return;
  63
  64                                 // jam on a sideslip of 12 degrees or more
  65     if (fabs(_beta_node->getDoubleValue()) > 12.0) {
  66         _rate_degps = 0.0;
  67         _error_deg = _heading_node->getDoubleValue() -
  68             _out_node->getDoubleValue();
  69         return;
  70     }
  71
  72     double accelN = _north_accel_node->getDoubleValue();
  73     double accelE = _east_accel_node->getDoubleValue();
  74     double accelU = _down_accel_node->getDoubleValue() - 32.0; // why?
  75
  76                                 // force vector towards magnetic north pole
  77     double var = _variation_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
  78     double dip = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
  79     double cosdip = cos(dip);
  80     double forceN = cosdip * cos(var);
  81     double forceE = cosdip * sin(var);
  82     double forceU = sin(dip);
  83
  84                                 // rotation is around acceleration axis
  85                                 // (magnitude doesn't matter)
  86     double accel = accelN * accelN + accelE * accelE + accelU * accelU;
  87     if (accel > 1.0)
  88         accel = sqrt(accel);
  89     else
  90             accel = 1.0;
  91
  92                                 // North marking on compass card
  93     double edgeN = cos(_error_deg * SGD_DEGREES_TO_RADIANS);
  94     double edgeE = sin(_error_deg * SGD_DEGREES_TO_RADIANS);
  95     double edgeU = 0.0;
  96
  97                                 // apply the force to that edge to get torques
  98     double torqueN = edgeE * forceU - edgeU * forceE;
  99     double torqueE = edgeU * forceN - edgeN * forceU;
 100     double torqueU = edgeN * forceE - edgeE * forceN;
 101
 102                                 // get the component parallel to the axis
 103     double torque = (torqueN * accelN +
 104                      torqueE * accelE +
 105                      torqueU * accelU) * 5.0 / accel;
 106
 107                                 // the compass has angular momentum,
 108                                 // so apply a torque and wait
 109     if (delta_time_sec < 1.0) {
 110         _rate_degps = _rate_degps * (1.0 - delta_time_sec) - torque;
 111         _error_deg += delta_time_sec * _rate_degps;
 112     }
 113     if (_error_deg > 180.0)
 114         _error_deg -= 360.0;
 115     else if (_error_deg < -180.0)
 116         _error_deg += 360.0;
 117
 118                                 // Set the indicated heading
 119     _out_node->setDoubleValue(_heading_node->getDoubleValue() - _error_deg);
 120 }
 121
 122 // end of altimeter.cxx