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                                 // jam on excessive sideslip
  98     if (fabs(_beta_node->getDoubleValue()) > 12.0) {
  99         _rate_degps = 0.0;
 100         return;
 101     }
 102
 103
 104     /*
 105       Formula for northernly turning error from
 106       http://williams.best.vwh.net/compass/node4.html:
 107
 108       Hc: compass heading
 109       psi: magnetic heading
 110       theta: bank angle (right positive; should be phi here)
 111       mu: dip angle (down positive)
 112
 113       Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm))
 114
 115       This function changes the variable names to the more common psi
 116       for the heading, theta for the pitch, and phi for the roll (and
 117       target_deg for Hc).  It also modifies the equation to
 118       incorporate pitch as well as roll, as suggested by Chris
 119       Metzler.
 120     */
 121
 122                                 // bank angle (radians)
 123     double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 124
 125                                 // pitch angle (radians)
 126     double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 127
 128                                 // magnetic heading (radians)
 129     double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 130
 131                                 // magnetic dip (radians)
 132     double mu = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 133
 134
 135     /*
 136       Tilt adjustments for accelerations.
 137
 138       The magnitudes of these are totally made up, but in real life,
 139       they would depend on the fluid level, the amount of friction,
 140       etc. anyway.  Basically, the compass float tilts forward for
 141       acceleration and backward for deceleration.  Tilt about 4
 142       degrees (0.07 radians) for every G (32 fps/sec) of
 143       acceleration.
 144
 145       TODO: do something with the vertical acceleration.
 146     */
 147     double x_accel_g = _x_accel_node->getDoubleValue() / 32;
 148     double y_accel_g = _y_accel_node->getDoubleValue() / 32;
 149     //double z_accel_g = _z_accel_node->getDoubleValue() / 32;
 150
 151     theta -= 0.07 * x_accel_g;
 152     phi -= 0.07 * y_accel_g;
 153
 154     ////////////////////////////////////////////////////////////////////
 155     // calculate target compass heading degrees
 156     ////////////////////////////////////////////////////////////////////
 157
 158                                 // these are expensive: don't repeat
 159     double sin_phi = sin(phi);
 160     double sin_theta = sin(theta);
 161     double sin_mu = sin(mu);
 162     double cos_theta = cos(theta);
 163     double cos_psi = cos(psi);
 164     double cos_mu = cos(mu);
 165
 166     double a = cos(phi) * sin(psi) * cos_mu
 167         - sin_phi * cos_theta * sin_mu
 168         - sin_phi* sin_theta * cos_mu * cos_psi;
 169
 170     double b = cos_theta * cos_psi * cos(mu)
 171         - sin_theta * sin_mu;
 172
 173                                 // This is the value that the compass
 174                                 // is *trying* to display.
 175     double target_deg = atan2(a, b) * SGD_RADIANS_TO_DEGREES;
 176     double old_deg = _out_node->getDoubleValue();
 177
 178     while ((target_deg - old_deg) > 180.0)
 179         target_deg -= 360.0;
 180     while ((target_deg - old_deg) < -180.0)
 181         target_deg += 360.0;
 182
 183                                 // The compass has a current rate of
 184                                 // rotation -- move the rate of rotation
 185                                 // towards one that will turn the compass
 186                                 // to the correct heading, but lag a bit.
 187                                 // (so that the compass can keep overshooting
 188                                 // and coming back).
 189     double error = target_deg - old_deg;
 190     _rate_degps = fgGetLowPass(_rate_degps, error, delta_time_sec / 5.0);
 191     double indicated_deg = old_deg + _rate_degps * delta_time_sec;
 192     SG_NORMALIZE_RANGE(indicated_deg, 0.0, 360.0);
 193
 194                                 // That's it -- set the messed-up heading.
 195     _out_node->setDoubleValue(indicated_deg);
 196 }
 197
 198 // end of altimeter.cxx