+ /*
+ Formula for northernly turning error from
+ http://williams.best.vwh.net/compass/node4.html:
+
+ Hc: compass heading
+ psi: 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))
+
+ This function changes the variable names to the more common psi
+ for the heading, theta for the pitch, and phi for the roll (and
+ target_deg for Hc). It also modifies the equation to
+ incorporate pitch as well as roll, as suggested by Chris
+ Metzler.
+ */
+
+ // bank angle (radians)
+ double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
+
+ // pitch angle (radians)
+ double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
+
+ // magnetic heading (radians)
+ double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
+
+ // magnetic dip (radians)
+ 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 degrees
+ ////////////////////////////////////////////////////////////////////
+
+ // these are expensive: don't repeat
+ double sin_phi = sin(phi);
+ double sin_theta = sin(theta);
+ double sin_mu = sin(mu);
+ double cos_theta = cos(theta);
+ double cos_psi = cos(psi);
+ double cos_mu = cos(mu);
+
+ double a = cos(phi) * sin(psi) * cos_mu
+ - sin_phi * cos_theta * sin_mu
+ - sin_phi* sin_theta * cos_mu * cos_psi;
+
+ double b = cos_theta * cos_psi * cos(mu)
+ - sin_theta * sin_mu;
+
+ // 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);