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