Begin a rewrite of the magnetic compass code. So far, only northerly

[flightgear.git] / src / Instrumentation / mag_compass.cxx

diff --git a/src/Instrumentation/mag_compass.cxx b/src/Instrumentation/mag_compass.cxx
index c7b13553c37f2d9793eb47c00620590de4341945..b069a5f6417e32dff20420366241c9657c215d0d 100644 (file)
--- a/src/Instrumentation/mag_compass.cxx
+++ b/src/Instrumentation/mag_compass.cxx
@@ -61,6 +61,8 @@ MagCompass::init ()
     _serviceable_node = node->getChild("serviceable", 0, true);
     _heading_node =
         fgGetNode("/orientation/heading-deg", true);
+    _roll_node =
+        fgGetNode("/orientation/roll-deg", true);
     _beta_node =
         fgGetNode("/orientation/side-slip-deg", true);
     _variation_node =
@@ -78,72 +80,113 @@ MagCompass::init ()
     _serviceable_node->setBoolValue(true);
 }
 
+
 void
 MagCompass::update (double delta_time_sec)
 {
-                                // algorithm from Alex Perry
-                                // possibly broken by David Megginson
+    /*
+      Formula for northernly turning error from
+      http://williams.best.vwh.net/compass/node4.html:
+
+      Hc: compass heading
+      Hm: magnetic heading
+      theta: bank angle (right positive; should be phi here)
+      mu: dip angle (down positive)
+
+      Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm))
+    */
 
                                 // don't update if it's broken
     if (!_serviceable_node->getBoolValue())
         return;
 
-                                // jam on a sideslip of 12 degrees or more
-    if (fabs(_beta_node->getDoubleValue()) > 12.0) {
-        _rate_degps = 0.0;
-        _error_deg = _heading_node->getDoubleValue() -
-            _out_node->getDoubleValue();
-        return;
-    }
+    // TODO use cached nodes
 
-    double accelN = _north_accel_node->getDoubleValue();
-    double accelE = _east_accel_node->getDoubleValue();
-    double accelU = _down_accel_node->getDoubleValue() - 32.0; // why?
-
-                                // force vector towards magnetic north pole
-    double var = _variation_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
-    double dip = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
-    double cosdip = cos(dip);
-    double forceN = cosdip * cos(var);
-    double forceE = cosdip * sin(var);
-    double forceU = sin(dip);
-
-                                // rotation is around acceleration axis
-                                // (magnitude doesn't matter)
-    double accel = accelN * accelN + accelE * accelE + accelU * accelU;
-    if (accel > 1.0)
-        accel = sqrt(accel);
-    else
-            accel = 1.0;
-
-                                // North marking on compass card
-    double edgeN = cos(_error_deg * SGD_DEGREES_TO_RADIANS);
-    double edgeE = sin(_error_deg * SGD_DEGREES_TO_RADIANS);
-    double edgeU = 0.0;
-
-                                // apply the force to that edge to get torques
-    double torqueN = edgeE * forceU - edgeU * forceE;
-    double torqueE = edgeU * forceN - edgeN * forceU;
-    double torqueU = edgeN * forceE - edgeE * forceN;
-
-                                // get the component parallel to the axis
-    double torque = (torqueN * accelN +
-                     torqueE * accelE +
-                     torqueU * accelU) * 5.0 / accel;
-
-                                // the compass has angular momentum,
-                                // so apply a torque and wait
-    if (delta_time_sec < 1.0) {
-        _rate_degps = _rate_degps * (1.0 - delta_time_sec) - torque;
-        _error_deg += delta_time_sec * _rate_degps;
-    }
-    if (_error_deg > 180.0)
-        _error_deg -= 360.0;
-    else if (_error_deg < -180.0)
-        _error_deg += 360.0;
+    // magnetic heading (radians)
+    double Hm = 
+        fgGetDouble("/orientation/heading-magnetic-deg")
+        * SGD_DEGREES_TO_RADIANS;
+
+    // bank angle (radians)
+    double phi =
+        fgGetDouble("/orientation/roll-deg") * SGD_DEGREES_TO_RADIANS;
+
+    // magnetic dip (radians)
+    double mu =
+        fgGetDouble("/environment/magnetic-dip-deg") * SGD_DEGREES_TO_RADIANS;
+
+    // target compass heading (radians)
+    double Hc =
+        atan2(sin(Hm) * cos(phi) - tan(mu) * sin(phi), cos(Hm));
+
+    Hc *= SGD_RADIANS_TO_DEGREES;
+    while (Hc < 0)
+        Hc += 360;
+    while (Hc >= 360)
+        Hc =- 360;
+
+    _out_node->setDoubleValue(Hc);
+
+
+                                // algorithm from Alex Perry
+                                // possibly broken by David Megginson
 
-                                // Set the indicated heading
-    _out_node->setDoubleValue(_heading_node->getDoubleValue() - _error_deg);
+//                                 // jam on a sideslip of 12 degrees or more
+//     if (fabs(_beta_node->getDoubleValue()) > 12.0) {
+//         _rate_degps = 0.0;
+//         _error_deg = _heading_node->getDoubleValue() -
+//             _out_node->getDoubleValue();
+//         return;
+//     }
+
+//     double accelN = _north_accel_node->getDoubleValue();
+//     double accelE = _east_accel_node->getDoubleValue();
+//     double accelU = _down_accel_node->getDoubleValue() - 32.0; // why?
+
+//                                 // force vector towards magnetic north pole
+//     double var = _variation_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
+//     double dip = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
+//     double cosdip = cos(dip);
+//     double forceN = cosdip * cos(var);
+//     double forceE = cosdip * sin(var);
+//     double forceU = sin(dip);
+
+//                                 // rotation is around acceleration axis
+//                                 // (magnitude doesn't matter)
+//     double accel = accelN * accelN + accelE * accelE + accelU * accelU;
+//     if (accel > 1.0)
+//         accel = sqrt(accel);
+//     else
+//             accel = 1.0;
+
+//                                 // North marking on compass card
+//     double edgeN = cos(_error_deg * SGD_DEGREES_TO_RADIANS);
+//     double edgeE = sin(_error_deg * SGD_DEGREES_TO_RADIANS);
+//     double edgeU = 0.0;
+
+//                                 // apply the force to that edge to get torques
+//     double torqueN = edgeE * forceU - edgeU * forceE;
+//     double torqueE = edgeU * forceN - edgeN * forceU;
+//     double torqueU = edgeN * forceE - edgeE * forceN;
+
+//                                 // get the component parallel to the axis
+//     double torque = (torqueN * accelN +
+//                      torqueE * accelE +
+//                      torqueU * accelU) * 5.0 / accel;
+
+//                                 // the compass has angular momentum,
+//                                 // so apply a torque and wait
+//     if (delta_time_sec < 1.0) {
+//         _rate_degps = _rate_degps * (1.0 - delta_time_sec) - torque;
+//         _error_deg += delta_time_sec * _rate_degps;
+//     }
+//     if (_error_deg > 180.0)
+//         _error_deg -= 360.0;
+//     else if (_error_deg < -180.0)
+//         _error_deg += 360.0;
+
+//                                 // Set the indicated heading
+//     _out_node->setDoubleValue(_heading_node->getDoubleValue() - _error_deg);
 }
 
 // end of altimeter.cxx