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