fly/flightgear
1
0
Fork 0
Code Activity

Begin a rewrite of the magnetic compass code. So far, only northerly

Browse source 
turning error is supported, and I don't know if this works properly in
the southern hemisphere.
This commit is contained in:
david 2004-11-03 15:15:32 +00:00
parent 889dba8d65
commit 63cb4e59b7
1 changed files with 93 additions and 50 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

143
src/Instrumentation/mag_compass.cxx
View file

@ -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