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