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     _roll_node =
  63         fgGetNode("/orientation/roll-deg", true);
  64     _pitch_node =
  65         fgGetNode("/orientation/pitch-deg", true);
  66     _heading_node =
  67         fgGetNode("/orientation/heading-magnetic-deg", true);
  68     _beta_node =
  69         fgGetNode("/orientation/side-slip-deg", true);
  70     _dip_node =
  71         fgGetNode("/environment/magnetic-dip-deg", true);
  72     _x_accel_node =
  73         fgGetNode("/accelerations/pilot/x-accel-fps_sec", true);
  74     _y_accel_node =
  75         fgGetNode("/accelerations/pilot/y-accel-fps_sec", true);
  76     _z_accel_node =
  77         fgGetNode("/accelerations/pilot/z-accel-fps_sec", true);
  78     _out_node = node->getChild("indicated-heading-deg", 0, true);
  79
  80     _serviceable_node->setBoolValue(true);
  81 }
  82
  83
  84 void
  85 MagCompass::update (double delta_time_sec)
  86 {
  87                                 // This is the real magnetic
  88                                 // heading, which will almost
  89                                 // never appear.
  90     double heading_mag_deg = _heading_node->getDoubleValue();
  91
  92
  93                                 // don't update if it's broken
  94     if (!_serviceable_node->getBoolValue())
  95         return;
  96
  97     /*
  98       Jam on an excessive sideslip.
  99     */
 100     if (fabs(_beta_node->getDoubleValue()) > 12.0) {
 101         _rate_degps = 0.0;
 102         return;
 103     }
 104
 105
 106     /*
 107       Formula for northernly turning error from
 108       http://williams.best.vwh.net/compass/node4.html:
 109
 110       Hc: compass heading
 111       psi: magnetic heading
 112       theta: bank angle (right positive; should be phi here)
 113       mu: dip angle (down positive)
 114
 115       Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm))
 116
 117       This function changes the variable names to the more common
 118       psi for the heading, theta for the pitch, and phi for the
 119       roll.  It also modifies the equation to incorporate pitch
 120       as well as roll, as suggested by Chris Metzler.
 121     */
 122
 123                                 // bank angle (radians)
 124     double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 125
 126                                 // pitch angle (radians)
 127     double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 128
 129                                 // magnetic heading (radians)
 130     double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 131
 132                                 // magnetic dip (radians)
 133     double mu = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 134
 135
 136     /*
 137       Tilt adjustments for accelerations.
 138
 139       The magnitudes of these are totally made up, but in real life,
 140       they would depend on the fluid level, the amount of friction,
 141       etc. anyway.  Basically, the compass float tilts forward for
 142       acceleration and backward for deceleration.  Tilt about 4
 143       degrees (0.07 radians) for every G (32 fps/sec) of
 144       acceleration.
 145
 146       TODO: do something with the vertical acceleration.
 147     */
 148     double x_accel_g = _x_accel_node->getDoubleValue() / 32;
 149     double y_accel_g = _y_accel_node->getDoubleValue() / 32;
 150     double z_accel_g = _z_accel_node->getDoubleValue() / 32;
 151
 152     theta -= 0.07 * x_accel_g;
 153     phi -= 0.07 * y_accel_g;
 154
 155     ////////////////////////////////////////////////////////////////////
 156     // calculate target compass heading Hc in degrees
 157     ////////////////////////////////////////////////////////////////////
 158
 159                                 // these are expensive: don't repeat
 160     double sin_phi = sin(phi);
 161     double sin_theta = sin(theta);
 162     double sin_mu = sin(mu);
 163     double cos_theta = cos(theta);
 164     double cos_psi = cos(psi);
 165     double cos_mu = cos(mu);
 166
 167     double a = cos(phi) * sin(psi) * cos_mu
 168         - sin_phi * cos_theta * sin_mu
 169         - sin_phi* sin_theta * cos_mu * cos_psi;
 170
 171     double b = cos_theta * cos_psi * cos(mu)
 172         - sin_theta * sin_mu;
 173
 174                                 // This is the value that the compass
 175                                 // is *trying* to display, but it
 176                                 // takes time to move there, and because
 177                                 // of momentum, the compass will often
 178                                 // overshoot.
 179     double Hc = atan2(a, b) * SGD_RADIANS_TO_DEGREES;
 180
 181     _out_node->setDoubleValue(Hc);
 182 }
 183
 184 // end of altimeter.cxx