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