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     _heading_node =
  63         fgGetNode("/orientation/heading-deg", true);
  64     _roll_node =
  65         fgGetNode("/orientation/roll-deg", true);
  66     _beta_node =
  67         fgGetNode("/orientation/side-slip-deg", true);
  68     _variation_node =
  69         fgGetNode("/environment/magnetic-variation-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     /*
  88       Formula for northernly turning error from
  89       http://williams.best.vwh.net/compass/node4.html:
  90
  91       Hc: compass heading
  92       Hm: magnetic heading
  93       theta: bank angle (right positive; should be phi here)
  94       mu: dip angle (down positive)
  95
  96       Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm))
  97     */
  98
  99                                 // don't update if it's broken
 100     if (!_serviceable_node->getBoolValue())
 101         return;
 102
 103     // TODO use cached nodes
 104
 105     // magnetic heading (radians)
 106     double Hm =
 107         fgGetDouble("/orientation/heading-magnetic-deg")
 108         * SGD_DEGREES_TO_RADIANS;
 109
 110     // bank angle (radians)
 111     double phi =
 112         fgGetDouble("/orientation/roll-deg") * SGD_DEGREES_TO_RADIANS;
 113
 114     // magnetic dip (radians)
 115     double mu =
 116         fgGetDouble("/environment/magnetic-dip-deg") * SGD_DEGREES_TO_RADIANS;
 117
 118     // target compass heading (radians)
 119     double Hc =
 120         atan2(sin(Hm) * cos(phi) - tan(mu) * sin(phi), cos(Hm));
 121
 122     Hc *= SGD_RADIANS_TO_DEGREES;
 123     while (Hc < 0)
 124         Hc += 360;
 125     while (Hc >= 360)
 126         Hc =- 360;
 127
 128     _out_node->setDoubleValue(Hc);
 129
 130
 131                                 // algorithm from Alex Perry
 132                                 // possibly broken by David Megginson
 133
 134 //                                 // jam on a sideslip of 12 degrees or more
 135 //     if (fabs(_beta_node->getDoubleValue()) > 12.0) {
 136 //         _rate_degps = 0.0;
 137 //         _error_deg = _heading_node->getDoubleValue() -
 138 //             _out_node->getDoubleValue();
 139 //         return;
 140 //     }
 141
 142 //     double accelN = _north_accel_node->getDoubleValue();
 143 //     double accelE = _east_accel_node->getDoubleValue();
 144 //     double accelU = _down_accel_node->getDoubleValue() - 32.0; // why?
 145
 146 //                                 // force vector towards magnetic north pole
 147 //     double var = _variation_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 148 //     double dip = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 149 //     double cosdip = cos(dip);
 150 //     double forceN = cosdip * cos(var);
 151 //     double forceE = cosdip * sin(var);
 152 //     double forceU = sin(dip);
 153
 154 //                                 // rotation is around acceleration axis
 155 //                                 // (magnitude doesn't matter)
 156 //     double accel = accelN * accelN + accelE * accelE + accelU * accelU;
 157 //     if (accel > 1.0)
 158 //         accel = sqrt(accel);
 159 //     else
 160 //             accel = 1.0;
 161
 162 //                                 // North marking on compass card
 163 //     double edgeN = cos(_error_deg * SGD_DEGREES_TO_RADIANS);
 164 //     double edgeE = sin(_error_deg * SGD_DEGREES_TO_RADIANS);
 165 //     double edgeU = 0.0;
 166
 167 //                                 // apply the force to that edge to get torques
 168 //     double torqueN = edgeE * forceU - edgeU * forceE;
 169 //     double torqueE = edgeU * forceN - edgeN * forceU;
 170 //     double torqueU = edgeN * forceE - edgeE * forceN;
 171
 172 //                                 // get the component parallel to the axis
 173 //     double torque = (torqueN * accelN +
 174 //                      torqueE * accelE +
 175 //                      torqueU * accelU) * 5.0 / accel;
 176
 177 //                                 // the compass has angular momentum,
 178 //                                 // so apply a torque and wait
 179 //     if (delta_time_sec < 1.0) {
 180 //         _rate_degps = _rate_degps * (1.0 - delta_time_sec) - torque;
 181 //         _error_deg += delta_time_sec * _rate_degps;
 182 //     }
 183 //     if (_error_deg > 180.0)
 184 //         _error_deg -= 360.0;
 185 //     else if (_error_deg < -180.0)
 186 //         _error_deg += 360.0;
 187
 188 //                                 // Set the indicated heading
 189 //     _out_node->setDoubleValue(_heading_node->getDoubleValue() - _error_deg);
 190 }
 191
 192 // end of altimeter.cxx