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