From 63cb4e59b7321d369bd8639d21d58adf5ff53c8c Mon Sep 17 00:00:00 2001 From: david Date: Wed, 3 Nov 2004 15:15:32 +0000 Subject: [PATCH] Begin a rewrite of the magnetic compass code. So far, only northerly turning error is supported, and I don't know if this works properly in the southern hemisphere. --- src/Instrumentation/mag_compass.cxx | 143 ++++++++++++++++++---------- 1 file changed, 93 insertions(+), 50 deletions(-) diff --git a/src/Instrumentation/mag_compass.cxx b/src/Instrumentation/mag_compass.cxx index c7b13553c..b069a5f64 100644 --- 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? + // magnetic heading (radians) + double Hm = + fgGetDouble("/orientation/heading-magnetic-deg") + * SGD_DEGREES_TO_RADIANS; - // 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); + // bank angle (radians) + double phi = + fgGetDouble("/orientation/roll-deg") * SGD_DEGREES_TO_RADIANS; - // 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; + // magnetic dip (radians) + double mu = + fgGetDouble("/environment/magnetic-dip-deg") * SGD_DEGREES_TO_RADIANS; - // 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; + // target compass heading (radians) + double Hc = + atan2(sin(Hm) * cos(phi) - tan(mu) * sin(phi), cos(Hm)); - // 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; + Hc *= SGD_RADIANS_TO_DEGREES; + while (Hc < 0) + Hc += 360; + while (Hc >= 360) + Hc =- 360; - // get the component parallel to the axis - double torque = (torqueN * accelN + - torqueE * accelE + - torqueU * accelU) * 5.0 / accel; + _out_node->setDoubleValue(Hc); - // 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); + // 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