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
  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("magnetic-compass"),
  24       num(0)
  25 {
  26     int i;
  27     for ( i = 0; i < node->nChildren(); ++i ) {
  28         SGPropertyNode *child = node->getChild(i);
  29         string cname = child->getName();
  30         string cval = child->getStringValue();
  31         if ( cname == "name" ) {
  32             name = cval;
  33         } else if ( cname == "number" ) {
  34             num = child->getIntValue();
  35         } else {
  36             SG_LOG( SG_INSTR, SG_WARN, "Error in magnetic-compass config logic" );
  37             if ( name.length() ) {
  38                 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
  39             }
  40         }
  41     }
  42 }
  43
  44 MagCompass::MagCompass ()
  45     : _error_deg(0.0),
  46       _rate_degps(0.0)
  47 {
  48 }
  49
  50 MagCompass::~MagCompass ()
  51 {
  52 }
  53
  54 void
  55 MagCompass::init ()
  56 {
  57     string branch;
  58     branch = "/instrumentation/" + name;
  59
  60     SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
  61     _serviceable_node = node->getChild("serviceable", 0, true);
  62     _roll_node =
  63         fgGetNode("/orientation/roll-deg", true);
  64     _pitch_node =
  65         fgGetNode("/orientation/pitch-deg", true);
  66     _heading_node =
  67         fgGetNode("/orientation/heading-magnetic-deg", true);
  68     _beta_node =
  69         fgGetNode("/orientation/side-slip-deg", true);
  70     _dip_node =
  71         fgGetNode("/environment/magnetic-dip-deg", true);
  72     _north_accel_node =
  73         fgGetNode("/accelerations/ned/north-accel-fps_sec", true);
  74     _east_accel_node =
  75         fgGetNode("/accelerations/ned/east-accel-fps_sec", true);
  76     _down_accel_node =
  77         fgGetNode("/accelerations/ned/down-accel-fps_sec", true);
  78     _out_node = node->getChild("indicated-heading-deg", 0, true);
  79
  80     _serviceable_node->setBoolValue(true);
  81 }
  82
  83
  84 void
  85 MagCompass::update (double delta_time_sec)
  86 {
  87                                 // don't update if it's broken
  88     if (!_serviceable_node->getBoolValue())
  89         return;
  90
  91     /*
  92       Formula for northernly turning error from
  93       http://williams.best.vwh.net/compass/node4.html:
  94
  95       Hc: compass heading
  96       psi: magnetic heading
  97       theta: bank angle (right positive; should be phi here)
  98       mu: dip angle (down positive)
  99
 100       Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm))
 101
 102       This function changes the variable names to the more common
 103       psi for the heading, theta for the pitch, and phi for the
 104       roll.  It also modifies the equation to incorporate pitch
 105       as well as roll, as suggested by Chris Metzler.
 106     */
 107
 108                                 // bank angle (radians)
 109     double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 110
 111                                 // pitch angle (radians)
 112     double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 113
 114                                 // magnetic heading (radians)
 115     double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 116
 117                                 // magnetic dip (radians)
 118     double mu = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 119
 120
 121     ////////////////////////////////////////////////////////////////////
 122     // calculate target compass heading Hc in degrees
 123     ////////////////////////////////////////////////////////////////////
 124
 125                                 // these are expensive: don't repeat
 126     double sin_phi = sin(phi);
 127     double sin_theta = sin(theta);
 128     double sin_mu = sin(mu);
 129     double cos_theta = cos(theta);
 130     double cos_psi = cos(psi);
 131     double cos_mu = cos(mu);
 132
 133     double a = cos(phi) * sin(psi) * cos_mu
 134         - sin_phi * cos_theta * sin_mu
 135         - sin_phi* sin_theta * cos_mu * cos_psi;
 136
 137     double b = cos_theta * cos_psi * cos(mu)
 138         - sin_theta * sin_mu;
 139
 140     double Hc = atan2(a, b) * SGD_RADIANS_TO_DEGREES;
 141
 142     while (Hc < 0)
 143         Hc += 360;
 144     while (Hc >= 360)
 145         Hc =- 360;
 146
 147     // TODO add acceleration error
 148     // TODO allow binding with excessive dip/sideslip
 149
 150     _out_node->setDoubleValue(Hc);
 151
 152
 153                                 // algorithm from Alex Perry
 154                                 // possibly broken by David Megginson
 155
 156 //                                 // jam on a sideslip of 12 degrees or more
 157 //     if (fabs(_beta_node->getDoubleValue()) > 12.0) {
 158 //         _rate_degps = 0.0;
 159 //         _error_deg = _heading_node->getDoubleValue() -
 160 //             _out_node->getDoubleValue();
 161 //         return;
 162 //     }
 163
 164 //     double accelN = _north_accel_node->getDoubleValue();
 165 //     double accelE = _east_accel_node->getDoubleValue();
 166 //     double accelU = _down_accel_node->getDoubleValue() - 32.0; // why?
 167
 168 //                                 // force vector towards magnetic north pole
 169 //     double var = _variation_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 170 //     double dip = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 171 //     double cosdip = cos(dip);
 172 //     double forceN = cosdip * cos(var);
 173 //     double forceE = cosdip * sin(var);
 174 //     double forceU = sin(dip);
 175
 176 //                                 // rotation is around acceleration axis
 177 //                                 // (magnitude doesn't matter)
 178 //     double accel = accelN * accelN + accelE * accelE + accelU * accelU;
 179 //     if (accel > 1.0)
 180 //         accel = sqrt(accel);
 181 //     else
 182 //             accel = 1.0;
 183
 184 //                                 // North marking on compass card
 185 //     double edgeN = cos(_error_deg * SGD_DEGREES_TO_RADIANS);
 186 //     double edgeE = sin(_error_deg * SGD_DEGREES_TO_RADIANS);
 187 //     double edgeU = 0.0;
 188
 189 //                                 // apply the force to that edge to get torques
 190 //     double torqueN = edgeE * forceU - edgeU * forceE;
 191 //     double torqueE = edgeU * forceN - edgeN * forceU;
 192 //     double torqueU = edgeN * forceE - edgeE * forceN;
 193
 194 //                                 // get the component parallel to the axis
 195 //     double torque = (torqueN * accelN +
 196 //                      torqueE * accelE +
 197 //                      torqueU * accelU) * 5.0 / accel;
 198
 199 //                                 // the compass has angular momentum,
 200 //                                 // so apply a torque and wait
 201 //     if (delta_time_sec < 1.0) {
 202 //         _rate_degps = _rate_degps * (1.0 - delta_time_sec) - torque;
 203 //         _error_deg += delta_time_sec * _rate_degps;
 204 //     }
 205 //     if (_error_deg > 180.0)
 206 //         _error_deg -= 360.0;
 207 //     else if (_error_deg < -180.0)
 208 //         _error_deg += 360.0;
 209
 210 //                                 // Set the indicated heading
 211 //     _out_node->setDoubleValue(_heading_node->getDoubleValue() - _error_deg);
 212 }
 213
 214 // end of altimeter.cxx