Added acceleration error to compass (most obvious during heavy braking

on the ground) -- this is based on some wild guesses, but it seems
reasonable for now.

The next step will be to give the compass some angular momentum so
that it does not snap instantly to a heading, and so that it can
overshoot a heading and oscillate.

diff --git a/src/Instrumentation/mag_compass.cxx b/src/Instrumentation/mag_compass.cxx
index e119bb836bb2ca5851cbeb2e84d71ea00549cb99..e44981cf577cd9606ca2199bc5c5ee3fae5c3157 100644 (file)
--- a/src/Instrumentation/mag_compass.cxx
+++ b/src/Instrumentation/mag_compass.cxx
@@ -69,12 +69,12 @@ MagCompass::init ()
         fgGetNode("/orientation/side-slip-deg", true);
     _dip_node =
         fgGetNode("/environment/magnetic-dip-deg", true);
-    _north_accel_node =
-        fgGetNode("/accelerations/ned/north-accel-fps_sec", true);
-    _east_accel_node =
-        fgGetNode("/accelerations/ned/east-accel-fps_sec", true);
-    _down_accel_node =
-        fgGetNode("/accelerations/ned/down-accel-fps_sec", true);
+    _x_accel_node =
+        fgGetNode("/accelerations/pilot/x-accel-fps_sec", true);
+    _y_accel_node =
+        fgGetNode("/accelerations/pilot/y-accel-fps_sec", true);
+    _z_accel_node =
+        fgGetNode("/accelerations/pilot/z-accel-fps_sec", true);
     _out_node = node->getChild("indicated-heading-deg", 0, true);
 
     _serviceable_node->setBoolValue(true);
@@ -84,10 +84,25 @@ MagCompass::init ()
 void
 MagCompass::update (double delta_time_sec)
 {
+                                // This is the real magnetic
+                                // heading, which will almost
+                                // never appear.
+    double heading_mag_deg = _heading_node->getDoubleValue();
+
+
                                 // don't update if it's broken
     if (!_serviceable_node->getBoolValue())
         return;
 
+    /*
+      Jam on an excessive sideslip.
+    */
+    if (fabs(_beta_node->getDoubleValue()) > 12.0) {
+        _rate_degps = 0.0;
+        return;
+    }
+
+
     /*
       Formula for northernly turning error from
       http://williams.best.vwh.net/compass/node4.html:
@@ -118,6 +133,25 @@ MagCompass::update (double delta_time_sec)
     double mu = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
 
 
+    /*
+      Tilt adjustments for accelerations.
+      
+      The magnitudes of these are totally made up, but in real life,
+      they would depend on the fluid level, the amount of friction,
+      etc. anyway.  Basically, the compass float tilts forward for
+      acceleration and backward for deceleration.  Tilt about 4
+      degrees (0.07 radians) for every G (32 fps/sec) of
+      acceleration.
+
+      TODO: do something with the vertical acceleration.
+    */
+    double x_accel_g = _x_accel_node->getDoubleValue() / 32;
+    double y_accel_g = _y_accel_node->getDoubleValue() / 32;
+    double z_accel_g = _z_accel_node->getDoubleValue() / 32;
+
+    theta -= 0.07 * x_accel_g;
+    phi -= 0.07 * y_accel_g;
+    
     ////////////////////////////////////////////////////////////////////
     // calculate target compass heading Hc in degrees
     ////////////////////////////////////////////////////////////////////
@@ -137,78 +171,14 @@ MagCompass::update (double delta_time_sec)
     double b = cos_theta * cos_psi * cos(mu)
         - sin_theta * sin_mu;
 
+                                // This is the value that the compass
+                                // is *trying* to display, but it
+                                // takes time to move there, and because
+                                // of momentum, the compass will often
+                                // overshoot.
     double Hc = atan2(a, b) * SGD_RADIANS_TO_DEGREES;
 
-    while (Hc < 0)
-        Hc += 360;
-    while (Hc >= 360)
-        Hc =- 360;
-
-    // TODO add acceleration error
-    // TODO allow binding with excessive dip/sideslip
-
     _out_node->setDoubleValue(Hc);
-
-
-                                // algorithm from Alex Perry
-                                // possibly broken by David Megginson
-
-//                                 // 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

diff --git a/src/Instrumentation/mag_compass.hxx b/src/Instrumentation/mag_compass.hxx
index a5403bfa594fd5baf6a49b12515e638633610c5c..ea5ac60ec73bde37fcb822a2d757ff73ec09a1ef 100644 (file)
--- a/src/Instrumentation/mag_compass.hxx
+++ b/src/Instrumentation/mag_compass.hxx
@@ -26,9 +26,9 @@
  * /orientation/heading-magnetic-deg
  * /orientation/side-slip-deg
  * /environment/magnetic-dip-deg
- * /accelerations/ned/north-accel-fps_sec
- * /accelerations/ned/east-accel-fps_sec
- * /accelerations/ned/down-accel-fps_sec
+ * /accelerations/pilot/north-accel-fps_sec
+ * /accelerations/pilot/east-accel-fps_sec
+ * /accelerations/pilot/down-accel-fps_sec
  *
  * Output properties:
  *
@@ -60,9 +60,9 @@ private:
     SGPropertyNode_ptr _heading_node;
     SGPropertyNode_ptr _beta_node;
     SGPropertyNode_ptr _dip_node;
-    SGPropertyNode_ptr _north_accel_node;
-    SGPropertyNode_ptr _east_accel_node;
-    SGPropertyNode_ptr _down_accel_node;
+    SGPropertyNode_ptr _x_accel_node;
+    SGPropertyNode_ptr _y_accel_node;
+    SGPropertyNode_ptr _z_accel_node;
     SGPropertyNode_ptr _out_node;
 
 };