123 lines
3.9 KiB
C++
123 lines
3.9 KiB
C++
|
// mag_compass.cxx - a magnetic compass.
|
||
|
|
// Written by David Megginson, started 2003.
|
||
|
|
//
|
||
|
|
// This file is in the Public Domain and comes with no warranty.
|
||
|
|
|
||
|
|
// This implementation is derived from an earlier one by Alex Perry,
|
||
|
|
// which appeared in src/Cockpit/steam.cxx
|
||
|
|
|
||
|
|
#ifdef HAVE_CONFIG_H
|
||
|
|
# include <config.h>
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#include <plib/sg.h>
|
||
|
|
|
||
|
|
#include "mag_compass.hxx"
|
||
|
|
#include <Main/fg_props.hxx>
|
||
|
|
#include <Main/util.hxx>
|
||
|
|
|
||
|
|
|
||
|
|
MagCompass::MagCompass ()
|
||
|
|
: _error_deg(0.0),
|
||
|
|
_rate_degps(0.0)
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
MagCompass::~MagCompass ()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
void
|
||
|
|
MagCompass::init ()
|
||
|
|
{
|
||
|
|
_serviceable_node =
|
||
|
|
fgGetNode("/instrumentation/magnetic-compass/serviceable", true);
|
||
|
|
_heading_node =
|
||
|
|
fgGetNode("/orientation/heading-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);
|
||
|
|
_out_node =
|
||
|
|
fgGetNode("/instrumentation/magnetic-compass/indicated-heading-deg",
|
||
|
|
true);
|
||
|
|
}
|
||
|
|
|
||
|
|
void
|
||
|
|
MagCompass::update (double delta_time_sec)
|
||
|
|
{
|
||
|
|
// algorithm from Alex Perry
|
||
|
|
// possibly broken by David Megginson
|
||
|
|
|
||
|
|
// 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;
|
||
|
|
}
|
||
|
|
|
||
|
|
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
|