fly/flightgear
1
0
Fork 0
Code Activity

Added acceleration error to compass (most obvious during heavy braking

Browse source 
on the ground) -- this is based on some wild guesses, but it seems
reasonable for now.

The next step will be to give the compass some angular momentum so
that it does not snap instantly to a heading, and so that it can
overshoot a heading and oscillate.
This commit is contained in:
david 2004-11-05 00:18:58 +00:00
parent 04842a0f5b
commit b0c1fb7755
2 changed files with 51 additions and 81 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

120
src/Instrumentation/mag_compass.cxx
View file

@ -69,12 +69,12 @@ MagCompass::init ()
fgGetNode("/orientation/side-slip-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);
@ -84,10 +84,25 @@ MagCompass::init ()
void
MagCompass::update (double delta_time_sec)
{
// This is the real magnetic
// heading, which will almost
// never appear.
double heading_mag_deg = _heading_node->getDoubleValue();
// don't update if it's broken
if (!_serviceable_node->getBoolValue())
return;
/*
Jam on an excessive sideslip.
*/
if (fabs(_beta_node->getDoubleValue()) > 12.0) {
_rate_degps = 0.0;
return;
}
/*
Formula for northernly turning error from
http://williams.best.vwh.net/compass/node4.html:
@ -118,6 +133,25 @@ MagCompass::update (double delta_time_sec)
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 Hc in degrees
////////////////////////////////////////////////////////////////////
@ -137,78 +171,14 @@ MagCompass::update (double delta_time_sec)
double b = cos_theta * cos_psi * cos(mu)
- sin_theta * sin_mu;
// This is the value that the compass
// is *trying* to display, but it
// takes time to move there, and because
// of momentum, the compass will often
// overshoot.
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);
}
// end of altimeter.cxx

12
src/Instrumentation/mag_compass.hxx
View file

@ -26,9 +26,9 @@
* /orientation/heading-magnetic-deg
* /orientation/side-slip-deg
* /environment/magnetic-dip-deg
* /accelerations/ned/north-accel-fps_sec
* /accelerations/ned/east-accel-fps_sec
* /accelerations/ned/down-accel-fps_sec
* /accelerations/pilot/north-accel-fps_sec
* /accelerations/pilot/east-accel-fps_sec
* /accelerations/pilot/down-accel-fps_sec
*
* Output properties:
*
@ -60,9 +60,9 @@ private:
SGPropertyNode_ptr _heading_node;
SGPropertyNode_ptr _beta_node;
SGPropertyNode_ptr _dip_node;
SGPropertyNode_ptr _north_accel_node;
SGPropertyNode_ptr _east_accel_node;
SGPropertyNode_ptr _down_accel_node;
SGPropertyNode_ptr _x_accel_node;
SGPropertyNode_ptr _y_accel_node;
SGPropertyNode_ptr _z_accel_node;
SGPropertyNode_ptr _out_node;
};