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);
63 MagCompass::update (double delta_time_sec)
65 // This is the real magnetic
66 // which would be displayed
67 // if the compass had no errors.
68 //double heading_mag_deg = _heading_node->getDoubleValue();
71 // don't update if the compass
73 if (!_serviceable_node->getBoolValue())
77 * Vassilii: commented out because this way, even when parked,
78 * w/o any accelerations and level, the compass is jammed.
79 * If somebody wants to model jamming, real forces (i.e. accelerations)
80 * and not sideslip angle must be considered.
83 // jam on excessive sideslip
84 if (fabs(_beta_node->getDoubleValue()) > 12.0) {
92 Formula for northernly turning error from
93 http://williams.best.vwh.net/compass/node4.html:
97 theta: bank angle (right positive; should be phi here)
98 mu: dip angle (down positive)
100 Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm))
102 This function changes the variable names to the more common psi
103 for the heading, theta for the pitch, and phi for the roll (and
104 target_deg for Hc). It also modifies the equation to
105 incorporate pitch as well as roll, as suggested by Chris
109 // bank angle (radians)
110 double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
112 // pitch angle (radians)
113 double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
115 // magnetic heading (radians)
116 double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
118 // magnetic dip (radians)
119 double mu = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
123 Tilt adjustments for accelerations.
125 The magnitudes of these are totally made up, but in real life,
126 they would depend on the fluid level, the amount of friction,
127 etc. anyway. Basically, the compass float tilts forward for
128 acceleration and backward for deceleration. Tilt about 4
129 degrees (0.07 radians) for every G (32 fps/sec) of
132 TODO: do something with the vertical acceleration.
134 double x_accel_g = _x_accel_node->getDoubleValue() / 32;
135 double y_accel_g = _y_accel_node->getDoubleValue() / 32;
136 //double z_accel_g = _z_accel_node->getDoubleValue() / 32;
138 theta -= 0.07 * x_accel_g;
139 phi -= 0.07 * y_accel_g;
141 ////////////////////////////////////////////////////////////////////
142 // calculate target compass heading degrees
143 ////////////////////////////////////////////////////////////////////
145 // these are expensive: don't repeat
146 double sin_phi = sin(phi);
147 double sin_theta = sin(theta);
148 double sin_mu = sin(mu);
149 double cos_theta = cos(theta);
150 double cos_psi = cos(psi);
151 double cos_mu = cos(mu);
153 double a = cos(phi) * sin(psi) * cos_mu
154 - sin_phi * cos_theta * sin_mu
155 - sin_phi* sin_theta * cos_mu * cos_psi;
157 double b = cos_theta * cos_psi * cos(mu)
158 - sin_theta * sin_mu;
160 // This is the value that the compass
161 // is *trying* to display.
162 double target_deg = atan2(a, b) * SGD_RADIANS_TO_DEGREES;
163 double old_deg = _out_node->getDoubleValue();
165 while ((target_deg - old_deg) > 180.0)
167 while ((target_deg - old_deg) < -180.0)
170 // The compass has a current rate of
171 // rotation -- move the rate of rotation
172 // towards one that will turn the compass
173 // to the correct heading, but lag a bit.
174 // (so that the compass can keep overshooting
176 double error = target_deg - old_deg;
177 _rate_degps = fgGetLowPass(_rate_degps, error, delta_time_sec / 5.0);
178 double indicated_deg = old_deg + _rate_degps * delta_time_sec;
179 SG_NORMALIZE_RANGE(indicated_deg, 0.0, 360.0);
181 // That's it -- set the messed-up heading.
182 _out_node->setDoubleValue(indicated_deg);
185 // end of altimeter.cxx