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/roll-deg", true);
67 fgGetNode("/orientation/side-slip-deg", true);
69 fgGetNode("/environment/magnetic-variation-deg", true);
71 fgGetNode("/environment/magnetic-dip-deg", true);
73 fgGetNode("/accelerations/ned/north-accel-fps_sec", true);
75 fgGetNode("/accelerations/ned/east-accel-fps_sec", true);
77 fgGetNode("/accelerations/ned/down-accel-fps_sec", true);
78 _out_node = node->getChild("indicated-heading-deg", 0, true);
80 _serviceable_node->setBoolValue(true);
85 MagCompass::update (double delta_time_sec)
88 Formula for northernly turning error from
89 http://williams.best.vwh.net/compass/node4.html:
93 theta: bank angle (right positive; should be phi here)
94 mu: dip angle (down positive)
96 Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm))
99 // don't update if it's broken
100 if (!_serviceable_node->getBoolValue())
103 // TODO use cached nodes
105 // magnetic heading (radians)
107 fgGetDouble("/orientation/heading-magnetic-deg")
108 * SGD_DEGREES_TO_RADIANS;
110 // bank angle (radians)
112 fgGetDouble("/orientation/roll-deg") * SGD_DEGREES_TO_RADIANS;
114 // magnetic dip (radians)
116 fgGetDouble("/environment/magnetic-dip-deg") * SGD_DEGREES_TO_RADIANS;
118 // target compass heading (radians)
120 atan2(sin(Hm) * cos(phi) - tan(mu) * sin(phi), cos(Hm));
122 Hc *= SGD_RADIANS_TO_DEGREES;
128 _out_node->setDoubleValue(Hc);
131 // algorithm from Alex Perry
132 // possibly broken by David Megginson
134 // // jam on a sideslip of 12 degrees or more
135 // if (fabs(_beta_node->getDoubleValue()) > 12.0) {
136 // _rate_degps = 0.0;
137 // _error_deg = _heading_node->getDoubleValue() -
138 // _out_node->getDoubleValue();
142 // double accelN = _north_accel_node->getDoubleValue();
143 // double accelE = _east_accel_node->getDoubleValue();
144 // double accelU = _down_accel_node->getDoubleValue() - 32.0; // why?
146 // // force vector towards magnetic north pole
147 // double var = _variation_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
148 // double dip = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
149 // double cosdip = cos(dip);
150 // double forceN = cosdip * cos(var);
151 // double forceE = cosdip * sin(var);
152 // double forceU = sin(dip);
154 // // rotation is around acceleration axis
155 // // (magnitude doesn't matter)
156 // double accel = accelN * accelN + accelE * accelE + accelU * accelU;
158 // accel = sqrt(accel);
162 // // North marking on compass card
163 // double edgeN = cos(_error_deg * SGD_DEGREES_TO_RADIANS);
164 // double edgeE = sin(_error_deg * SGD_DEGREES_TO_RADIANS);
165 // double edgeU = 0.0;
167 // // apply the force to that edge to get torques
168 // double torqueN = edgeE * forceU - edgeU * forceE;
169 // double torqueE = edgeU * forceN - edgeN * forceU;
170 // double torqueU = edgeN * forceE - edgeE * forceN;
172 // // get the component parallel to the axis
173 // double torque = (torqueN * accelN +
174 // torqueE * accelE +
175 // torqueU * accelU) * 5.0 / accel;
177 // // the compass has angular momentum,
178 // // so apply a torque and wait
179 // if (delta_time_sec < 1.0) {
180 // _rate_degps = _rate_degps * (1.0 - delta_time_sec) - torque;
181 // _error_deg += delta_time_sec * _rate_degps;
183 // if (_error_deg > 180.0)
184 // _error_deg -= 360.0;
185 // else if (_error_deg < -180.0)
186 // _error_deg += 360.0;
188 // // Set the indicated heading
189 // _out_node->setDoubleValue(_heading_node->getDoubleValue() - _error_deg);
192 // end of altimeter.cxx