X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FInstrumentation%2Fmag_compass.cxx;h=e67d35074b1d248f9e2b45e73b00535204928f5d;hb=4ce94a0ec44de6aa5c426a24bda310cf26a76045;hp=b069a5f6417e32dff20420366241c9657c215d0d;hpb=63cb4e59b7321d369bd8639d21d58adf5ff53c8c;p=flightgear.git diff --git a/src/Instrumentation/mag_compass.cxx b/src/Instrumentation/mag_compass.cxx index b069a5f64..e67d35074 100644 --- a/src/Instrumentation/mag_compass.cxx +++ b/src/Instrumentation/mag_compass.cxx @@ -11,6 +11,7 @@ #endif #include +#include #include "mag_compass.hxx" #include
@@ -59,134 +60,147 @@ MagCompass::init () SGPropertyNode *node = fgGetNode(branch.c_str(), num, true ); _serviceable_node = node->getChild("serviceable", 0, true); - _heading_node = - fgGetNode("/orientation/heading-deg", true); _roll_node = fgGetNode("/orientation/roll-deg", true); + _pitch_node = + fgGetNode("/orientation/pitch-deg", true); + _heading_node = + fgGetNode("/orientation/heading-magnetic-deg", true); _beta_node = fgGetNode("/orientation/side-slip-deg", true); - _variation_node = - fgGetNode("/environment/magnetic-variation-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); } void MagCompass::update (double delta_time_sec) { + // This is the real magnetic + // which would be displayed + // if the compass had no errors. + //double heading_mag_deg = _heading_node->getDoubleValue(); + + + // don't update if the compass + // is broken + if (!_serviceable_node->getBoolValue()) + return; + + /* + * Vassilii: commented out because this way, even when parked, + * w/o any accelerations and level, the compass is jammed. + * If somebody wants to model jamming, real forces (i.e. accelerations) + * and not sideslip angle must be considered. + */ +#if 0 + // jam on excessive sideslip + if (fabs(_beta_node->getDoubleValue()) > 12.0) { + _rate_degps = 0.0; + return; + } +#endif + + /* Formula for northernly turning error from http://williams.best.vwh.net/compass/node4.html: Hc: compass heading - Hm: magnetic 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. */ - // don't update if it's broken - if (!_serviceable_node->getBoolValue()) - return; + // bank angle (radians) + double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; + + // pitch angle (radians) + double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS; - // TODO use cached nodes - - // 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 - -// // 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); + // 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); } // end of altimeter.cxx