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 ( SGPropertyNode *node )
  21     : _error_deg(0.0),
  22       _rate_degps(0.0),
  23       name("magnetic-compass"),
  24       num(0)
  25 {
  26     int i;
  27     for ( i = 0; i < node->nChildren(); ++i ) {
  28         SGPropertyNode *child = node->getChild(i);
  29         string cname = child->getName();
  30         string cval = child->getStringValue();
  31         if ( cname == "name" ) {
  32             name = cval;
  33         } else if ( cname == "number" ) {
  34             num = child->getIntValue();
  35         } else {
  36             SG_LOG( SG_INSTR, SG_WARN, "Error in magnetic-compass config logic" );
  37             if ( name.length() ) {
  38                 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
  39             }
  40         }
  41     }
  42 }
  43
  44 MagCompass::MagCompass ()
  45     : _error_deg(0.0),
  46       _rate_degps(0.0)
  47 {
  48 }
  49
  50 MagCompass::~MagCompass ()
  51 {
  52 }
  53
  54 void
  55 MagCompass::init ()
  56 {
  57     string branch;
  58     branch = "/instrumentation/" + name;
  59
  60     SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
  61     _serviceable_node = node->getChild("serviceable", 0, true);
  62     _heading_node =
  63         fgGetNode("/orientation/heading-deg", true);
  64     _beta_node =
  65         fgGetNode("/orientation/side-slip-deg", true);
  66     _variation_node =
  67         fgGetNode("/environment/magnetic-variation-deg", true);
  68     _dip_node =
  69         fgGetNode("/environment/magnetic-dip-deg", true);
  70     _north_accel_node =
  71         fgGetNode("/accelerations/ned/north-accel-fps_sec", true);
  72     _east_accel_node =
  73         fgGetNode("/accelerations/ned/east-accel-fps_sec", true);
  74     _down_accel_node =
  75         fgGetNode("/accelerations/ned/down-accel-fps_sec", true);
  76     _out_node = node->getChild("indicated-heading-deg", 0, true);
  77
  78     _serviceable_node->setBoolValue(true);
  79 }
  80
  81 void
  82 MagCompass::update (double delta_time_sec)
  83 {
  84                                 // algorithm from Alex Perry
  85                                 // possibly broken by David Megginson
  86
  87                                 // don't update if it's broken
  88     if (!_serviceable_node->getBoolValue())
  89         return;
  90
  91                                 // jam on a sideslip of 12 degrees or more
  92     if (fabs(_beta_node->getDoubleValue()) > 12.0) {
  93         _rate_degps = 0.0;
  94         _error_deg = _heading_node->getDoubleValue() -
  95             _out_node->getDoubleValue();
  96         return;
  97     }
  98
  99     double accelN = _north_accel_node->getDoubleValue();
 100     double accelE = _east_accel_node->getDoubleValue();
 101     double accelU = _down_accel_node->getDoubleValue() - 32.0; // why?
 102
 103                                 // force vector towards magnetic north pole
 104     double var = _variation_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 105     double dip = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 106     double cosdip = cos(dip);
 107     double forceN = cosdip * cos(var);
 108     double forceE = cosdip * sin(var);
 109     double forceU = sin(dip);
 110
 111                                 // rotation is around acceleration axis
 112                                 // (magnitude doesn't matter)
 113     double accel = accelN * accelN + accelE * accelE + accelU * accelU;
 114     if (accel > 1.0)
 115         accel = sqrt(accel);
 116     else
 117             accel = 1.0;
 118
 119                                 // North marking on compass card
 120     double edgeN = cos(_error_deg * SGD_DEGREES_TO_RADIANS);
 121     double edgeE = sin(_error_deg * SGD_DEGREES_TO_RADIANS);
 122     double edgeU = 0.0;
 123
 124                                 // apply the force to that edge to get torques
 125     double torqueN = edgeE * forceU - edgeU * forceE;
 126     double torqueE = edgeU * forceN - edgeN * forceU;
 127     double torqueU = edgeN * forceE - edgeE * forceN;
 128
 129                                 // get the component parallel to the axis
 130     double torque = (torqueN * accelN +
 131                      torqueE * accelE +
 132                      torqueU * accelU) * 5.0 / accel;
 133
 134                                 // the compass has angular momentum,
 135                                 // so apply a torque and wait
 136     if (delta_time_sec < 1.0) {
 137         _rate_degps = _rate_degps * (1.0 - delta_time_sec) - torque;
 138         _error_deg += delta_time_sec * _rate_degps;
 139     }
 140     if (_error_deg > 180.0)
 141         _error_deg -= 360.0;
 142     else if (_error_deg < -180.0)
 143         _error_deg += 360.0;
 144
 145                                 // Set the indicated heading
 146     _out_node->setDoubleValue(_heading_node->getDoubleValue() - _error_deg);
 147 }
 148
 149 // end of altimeter.cxx