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