1 // mag_compass.cxx - a magnetic compass.
2 // Written by David Megginson, started 2003.
4 // This file is in the Public Domain and comes with no warranty.
6 // This implementation is derived from an earlier one by Alex Perry,
7 // which appeared in src/Cockpit/steam.cxx
15 #include "mag_compass.hxx"
16 #include <Main/fg_props.hxx>
17 #include <Main/util.hxx>
20 MagCompass::MagCompass ( SGPropertyNode *node )
23 name("magnetic-compass"),
27 for ( i = 0; i < node->nChildren(); ++i ) {
28 SGPropertyNode *child = node->getChild(i);
29 string cname = child->getName();
30 string cval = child->getStringValue();
31 if ( cname == "name" ) {
33 } else if ( cname == "number" ) {
34 num = child->getIntValue();
36 SG_LOG( SG_INSTR, SG_WARN, "Error in magnetic-compass config logic" );
37 if ( name.length() ) {
38 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
44 MagCompass::MagCompass ()
50 MagCompass::~MagCompass ()
58 branch = "/instrumentation/" + name;
60 SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
61 _serviceable_node = node->getChild("serviceable", 0, true);
63 fgGetNode("/orientation/heading-deg", true);
65 fgGetNode("/orientation/side-slip-deg", true);
67 fgGetNode("/environment/magnetic-variation-deg", true);
69 fgGetNode("/environment/magnetic-dip-deg", true);
71 fgGetNode("/accelerations/ned/north-accel-fps_sec", true);
73 fgGetNode("/accelerations/ned/east-accel-fps_sec", true);
75 fgGetNode("/accelerations/ned/down-accel-fps_sec", true);
76 _out_node = node->getChild("indicated-heading-deg", 0, true);
78 _serviceable_node->setBoolValue(true);
82 MagCompass::update (double delta_time_sec)
84 // algorithm from Alex Perry
85 // possibly broken by David Megginson
87 // don't update if it's broken
88 if (!_serviceable_node->getBoolValue())
91 // jam on a sideslip of 12 degrees or more
92 if (fabs(_beta_node->getDoubleValue()) > 12.0) {
94 _error_deg = _heading_node->getDoubleValue() -
95 _out_node->getDoubleValue();
99 double accelN = _north_accel_node->getDoubleValue();
100 double accelE = _east_accel_node->getDoubleValue();
101 double accelU = _down_accel_node->getDoubleValue() - 32.0; // why?
103 // force vector towards magnetic north pole
104 double var = _variation_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
105 double dip = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
106 double cosdip = cos(dip);
107 double forceN = cosdip * cos(var);
108 double forceE = cosdip * sin(var);
109 double forceU = sin(dip);
111 // rotation is around acceleration axis
112 // (magnitude doesn't matter)
113 double accel = accelN * accelN + accelE * accelE + accelU * accelU;
119 // North marking on compass card
120 double edgeN = cos(_error_deg * SGD_DEGREES_TO_RADIANS);
121 double edgeE = sin(_error_deg * SGD_DEGREES_TO_RADIANS);
124 // apply the force to that edge to get torques
125 double torqueN = edgeE * forceU - edgeU * forceE;
126 double torqueE = edgeU * forceN - edgeN * forceU;
127 double torqueU = edgeN * forceE - edgeE * forceN;
129 // get the component parallel to the axis
130 double torque = (torqueN * accelN +
132 torqueU * accelU) * 5.0 / accel;
134 // the compass has angular momentum,
135 // so apply a torque and wait
136 if (delta_time_sec < 1.0) {
137 _rate_degps = _rate_degps * (1.0 - delta_time_sec) - torque;
138 _error_deg += delta_time_sec * _rate_degps;
140 if (_error_deg > 180.0)
142 else if (_error_deg < -180.0)
145 // Set the indicated heading
146 _out_node->setDoubleValue(_heading_node->getDoubleValue() - _error_deg);
149 // end of altimeter.cxx