- 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);
+ // This is the value that the compass
+ // is *trying* to display.
+ double target_deg = atan2(a, b) * SGD_RADIANS_TO_DEGREES;
+ double old_deg = _out_node->getDoubleValue();
+
+ while ((target_deg - old_deg) > 180.0)
+ target_deg -= 360.0;
+ while ((target_deg - old_deg) < -180.0)
+ target_deg += 360.0;
+
+ // The compass has a current rate of
+ // rotation -- move the rate of rotation
+ // towards one that will turn the compass
+ // to the correct heading, but lag a bit.
+ // (so that the compass can keep overshooting
+ // and coming back).
+ double error = target_deg - old_deg;
+ _rate_degps = fgGetLowPass(_rate_degps, error, delta_time_sec / 5.0);
+ double indicated_deg = old_deg + _rate_degps * delta_time_sec;
+ SG_NORMALIZE_RANGE(indicated_deg, 0.0, 360.0);
+
+ // That's it -- set the messed-up heading.
+ _out_node->setDoubleValue(indicated_deg);