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 <simgear/sg_inlines.h>
  14 #include <simgear/math/SGMath.hxx>
  15
  16 #include <Main/fg_props.hxx>
  17 #include <Main/util.hxx>
  18
  19 #include "mag_compass.hxx"
  20
  21
  22 MagCompass::MagCompass ( SGPropertyNode *node )
  23     : _error_deg(0.0),
  24       _rate_degps(0.0),
  25       _name(node->getStringValue("name", "magnetic-compass")),
  26       _num(node->getIntValue("number", 0))
  27 {
  28 }
  29
  30 MagCompass::~MagCompass ()
  31 {
  32 }
  33
  34 void
  35 MagCompass::init ()
  36 {
  37     std::string branch;
  38     branch = "/instrumentation/" + _name;
  39
  40     SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
  41     _serviceable_node = node->getChild("serviceable", 0, true);
  42     _roll_node = fgGetNode("/orientation/roll-deg", true);
  43     _pitch_node = fgGetNode("/orientation/pitch-deg", true);
  44     _heading_node = fgGetNode("/orientation/heading-magnetic-deg", true);
  45     _beta_node = fgGetNode("/orientation/side-slip-deg", true);
  46     _dip_node = fgGetNode("/environment/magnetic-dip-deg", true);
  47     _x_accel_node = fgGetNode("/accelerations/pilot/x-accel-fps_sec", true);
  48     _y_accel_node = fgGetNode("/accelerations/pilot/y-accel-fps_sec", true);
  49     _z_accel_node = fgGetNode("/accelerations/pilot/z-accel-fps_sec", true);
  50     _out_node = node->getChild("indicated-heading-deg", 0, true);
  51
  52     reinit();
  53 }
  54
  55 void
  56 MagCompass::reinit ()
  57 {
  58     _error_deg = 0.0;
  59     _rate_degps = 0.0;
  60 }
  61
  62 void
  63 MagCompass::update (double delta_time_sec)
  64 {
  65                                 // This is the real magnetic
  66                                 // which would be displayed
  67                                 // if the compass had no errors.
  68     //double heading_mag_deg = _heading_node->getDoubleValue();
  69
  70
  71                                 // don't update if the compass
  72                                 // is broken
  73     if (!_serviceable_node->getBoolValue())
  74         return;
  75
  76     /*
  77      * Vassilii: commented out because this way, even when parked,
  78      * w/o any accelerations and level, the compass is jammed.
  79      * If somebody wants to model jamming, real forces (i.e. accelerations)
  80      * and not sideslip angle must be considered.
  81      */
  82 #if 0
  83                                 // jam on excessive sideslip
  84     if (fabs(_beta_node->getDoubleValue()) > 12.0) {
  85         _rate_degps = 0.0;
  86         return;
  87     }
  88 #endif
  89
  90
  91     /*
  92       Formula for northernly turning error from
  93       http://williams.best.vwh.net/compass/node4.html:
  94
  95       Hc: compass heading
  96       psi: magnetic heading
  97       theta: bank angle (right positive; should be phi here)
  98       mu: dip angle (down positive)
  99
 100       Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm))
 101
 102       This function changes the variable names to the more common psi
 103       for the heading, theta for the pitch, and phi for the roll (and
 104       target_deg for Hc).  It also modifies the equation to
 105       incorporate pitch as well as roll, as suggested by Chris
 106       Metzler.
 107     */
 108
 109                                 // bank angle (radians)
 110     double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 111
 112                                 // pitch angle (radians)
 113     double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 114
 115                                 // magnetic heading (radians)
 116     double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 117
 118                                 // magnetic dip (radians)
 119     double mu = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 120
 121
 122     /*
 123       Tilt adjustments for accelerations.
 124
 125       The magnitudes of these are totally made up, but in real life,
 126       they would depend on the fluid level, the amount of friction,
 127       etc. anyway.  Basically, the compass float tilts forward for
 128       acceleration and backward for deceleration.  Tilt about 4
 129       degrees (0.07 radians) for every G (32 fps/sec) of
 130       acceleration.
 131
 132       TODO: do something with the vertical acceleration.
 133     */
 134     double x_accel_g = _x_accel_node->getDoubleValue() / 32;
 135     double y_accel_g = _y_accel_node->getDoubleValue() / 32;
 136     //double z_accel_g = _z_accel_node->getDoubleValue() / 32;
 137
 138     theta -= 0.07 * x_accel_g;
 139     phi -= 0.07 * y_accel_g;
 140
 141     ////////////////////////////////////////////////////////////////////
 142     // calculate target compass heading degrees
 143     ////////////////////////////////////////////////////////////////////
 144
 145                                 // these are expensive: don't repeat
 146     double sin_phi = sin(phi);
 147     double sin_theta = sin(theta);
 148     double sin_mu = sin(mu);
 149     double cos_theta = cos(theta);
 150     double cos_psi = cos(psi);
 151     double cos_mu = cos(mu);
 152
 153     double a = cos(phi) * sin(psi) * cos_mu
 154         - sin_phi * cos_theta * sin_mu
 155         - sin_phi* sin_theta * cos_mu * cos_psi;
 156
 157     double b = cos_theta * cos_psi * cos(mu)
 158         - sin_theta * sin_mu;
 159
 160                                 // This is the value that the compass
 161                                 // is *trying* to display.
 162     double target_deg = atan2(a, b) * SGD_RADIANS_TO_DEGREES;
 163     double old_deg = _out_node->getDoubleValue();
 164
 165     while ((target_deg - old_deg) > 180.0)
 166         target_deg -= 360.0;
 167     while ((target_deg - old_deg) < -180.0)
 168         target_deg += 360.0;
 169
 170                                 // The compass has a current rate of
 171                                 // rotation -- move the rate of rotation
 172                                 // towards one that will turn the compass
 173                                 // to the correct heading, but lag a bit.
 174                                 // (so that the compass can keep overshooting
 175                                 // and coming back).
 176     double error = target_deg - old_deg;
 177     _rate_degps = fgGetLowPass(_rate_degps, error, delta_time_sec / 5.0);
 178     double indicated_deg = old_deg + _rate_degps * delta_time_sec;
 179     SG_NORMALIZE_RANGE(indicated_deg, 0.0, 360.0);
 180
 181                                 // That's it -- set the messed-up heading.
 182     _out_node->setDoubleValue(indicated_deg);
 183 }
 184
 185 // end of altimeter.cxx