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
14 #include <simgear/sg_inlines.h>
16 #include "mag_compass.hxx"
17 #include <Main/fg_props.hxx>
18 #include <Main/util.hxx>
21 MagCompass::MagCompass ( SGPropertyNode *node )
24 _name(node->getStringValue("name", "magnetic-compass")),
25 _num(node->getIntValue("number", 0))
29 MagCompass::~MagCompass ()
37 branch = "/instrumentation/" + _name;
39 SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
40 _serviceable_node = node->getChild("serviceable", 0, true);
41 _roll_node = fgGetNode("/orientation/roll-deg", true);
42 _pitch_node = fgGetNode("/orientation/pitch-deg", true);
43 _heading_node = fgGetNode("/orientation/heading-magnetic-deg", true);
44 _beta_node = fgGetNode("/orientation/side-slip-deg", true);
45 _dip_node = fgGetNode("/environment/magnetic-dip-deg", true);
46 _x_accel_node = fgGetNode("/accelerations/pilot/x-accel-fps_sec", true);
47 _y_accel_node = fgGetNode("/accelerations/pilot/y-accel-fps_sec", true);
48 _z_accel_node = fgGetNode("/accelerations/pilot/z-accel-fps_sec", true);
49 _out_node = node->getChild("indicated-heading-deg", 0, true);
54 MagCompass::update (double delta_time_sec)
56 // This is the real magnetic
57 // which would be displayed
58 // if the compass had no errors.
59 //double heading_mag_deg = _heading_node->getDoubleValue();
62 // don't update if the compass
64 if (!_serviceable_node->getBoolValue())
68 * Vassilii: commented out because this way, even when parked,
69 * w/o any accelerations and level, the compass is jammed.
70 * If somebody wants to model jamming, real forces (i.e. accelerations)
71 * and not sideslip angle must be considered.
74 // jam on excessive sideslip
75 if (fabs(_beta_node->getDoubleValue()) > 12.0) {
83 Formula for northernly turning error from
84 http://williams.best.vwh.net/compass/node4.html:
88 theta: bank angle (right positive; should be phi here)
89 mu: dip angle (down positive)
91 Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm))
93 This function changes the variable names to the more common psi
94 for the heading, theta for the pitch, and phi for the roll (and
95 target_deg for Hc). It also modifies the equation to
96 incorporate pitch as well as roll, as suggested by Chris
100 // bank angle (radians)
101 double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
103 // pitch angle (radians)
104 double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
106 // magnetic heading (radians)
107 double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
109 // magnetic dip (radians)
110 double mu = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
114 Tilt adjustments for accelerations.
116 The magnitudes of these are totally made up, but in real life,
117 they would depend on the fluid level, the amount of friction,
118 etc. anyway. Basically, the compass float tilts forward for
119 acceleration and backward for deceleration. Tilt about 4
120 degrees (0.07 radians) for every G (32 fps/sec) of
123 TODO: do something with the vertical acceleration.
125 double x_accel_g = _x_accel_node->getDoubleValue() / 32;
126 double y_accel_g = _y_accel_node->getDoubleValue() / 32;
127 //double z_accel_g = _z_accel_node->getDoubleValue() / 32;
129 theta -= 0.07 * x_accel_g;
130 phi -= 0.07 * y_accel_g;
132 ////////////////////////////////////////////////////////////////////
133 // calculate target compass heading degrees
134 ////////////////////////////////////////////////////////////////////
136 // these are expensive: don't repeat
137 double sin_phi = sin(phi);
138 double sin_theta = sin(theta);
139 double sin_mu = sin(mu);
140 double cos_theta = cos(theta);
141 double cos_psi = cos(psi);
142 double cos_mu = cos(mu);
144 double a = cos(phi) * sin(psi) * cos_mu
145 - sin_phi * cos_theta * sin_mu
146 - sin_phi* sin_theta * cos_mu * cos_psi;
148 double b = cos_theta * cos_psi * cos(mu)
149 - sin_theta * sin_mu;
151 // This is the value that the compass
152 // is *trying* to display.
153 double target_deg = atan2(a, b) * SGD_RADIANS_TO_DEGREES;
154 double old_deg = _out_node->getDoubleValue();
156 while ((target_deg - old_deg) > 180.0)
158 while ((target_deg - old_deg) < -180.0)
161 // The compass has a current rate of
162 // rotation -- move the rate of rotation
163 // towards one that will turn the compass
164 // to the correct heading, but lag a bit.
165 // (so that the compass can keep overshooting
167 double error = target_deg - old_deg;
168 _rate_degps = fgGetLowPass(_rate_degps, error, delta_time_sec / 5.0);
169 double indicated_deg = old_deg + _rate_degps * delta_time_sec;
170 SG_NORMALIZE_RANGE(indicated_deg, 0.0, 360.0);
172 // That's it -- set the messed-up heading.
173 _out_node->setDoubleValue(indicated_deg);
176 // end of altimeter.cxx