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