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"
21 MagCompass::MagCompass ( SGPropertyNode *node )
23 _name(node->getStringValue("name", "magnetic-compass")),
24 _num(node->getIntValue("number", 0))
26 SGPropertyNode_ptr n = node->getNode( "deviation", false );
28 SGPropertyNode_ptr deviation_table_node = n->getNode( "table", false );
29 if( NULL != deviation_table_node ) {
30 _deviation_table = new SGInterpTable( deviation_table_node );
32 std::string deviation_node_name = n->getStringValue();
33 if( false == deviation_node_name.empty() )
34 _deviation_node = fgGetNode( deviation_node_name, true );
39 MagCompass::~MagCompass ()
47 branch = "/instrumentation/" + _name;
49 SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
50 _serviceable_node = node->getChild("serviceable", 0, true);
51 _pitch_offset_node = node->getChild("pitch-offset-deg", 0, true);
52 _roll_node = fgGetNode("/orientation/roll-deg", true);
53 _pitch_node = fgGetNode("/orientation/pitch-deg", true);
54 _heading_node = fgGetNode("/orientation/heading-magnetic-deg", true);
55 _beta_node = fgGetNode("/orientation/side-slip-deg", true);
56 _dip_node = fgGetNode("/environment/magnetic-dip-deg", true);
57 _x_accel_node = fgGetNode("/accelerations/pilot/x-accel-fps_sec", true);
58 _y_accel_node = fgGetNode("/accelerations/pilot/y-accel-fps_sec", true);
59 _z_accel_node = fgGetNode("/accelerations/pilot/z-accel-fps_sec", true);
60 _out_node = node->getChild("indicated-heading-deg", 0, true);
72 MagCompass::update (double delta_time_sec)
74 // This is the real magnetic
75 // which would be displayed
76 // if the compass had no errors.
77 //double heading_mag_deg = _heading_node->getDoubleValue();
80 // don't update if the compass
82 if (!_serviceable_node->getBoolValue())
86 * Vassilii: commented out because this way, even when parked,
87 * w/o any accelerations and level, the compass is jammed.
88 * If somebody wants to model jamming, real forces (i.e. accelerations)
89 * and not sideslip angle must be considered.
92 // jam on excessive sideslip
93 if (fabs(_beta_node->getDoubleValue()) > 12.0) {
101 Formula for northernly turning error from
102 http://williams.best.vwh.net/compass/node4.html:
105 psi: magnetic heading
106 theta: bank angle (right positive; should be phi here)
107 mu: dip angle (down positive)
109 Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm))
111 This function changes the variable names to the more common psi
112 for the heading, theta for the pitch, and phi for the roll (and
113 target_deg for Hc). It also modifies the equation to
114 incorporate pitch as well as roll, as suggested by Chris
118 // bank angle (radians)
119 double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
121 // pitch angle (radians)
122 double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS
123 + _pitch_offset_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
125 // magnetic heading (radians)
126 double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
128 // magnetic dip (radians)
129 double mu = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
133 Tilt adjustments for accelerations.
135 The magnitudes of these are totally made up, but in real life,
136 they would depend on the fluid level, the amount of friction,
137 etc. anyway. Basically, the compass float tilts forward for
138 acceleration and backward for deceleration. Tilt about 4
139 degrees (0.07 radians) for every G (32 fps/sec) of
142 TODO: do something with the vertical acceleration.
144 double x_accel_g = _x_accel_node->getDoubleValue() / 32;
145 double y_accel_g = _y_accel_node->getDoubleValue() / 32;
146 //double z_accel_g = _z_accel_node->getDoubleValue() / 32;
148 theta -= 0.07 * x_accel_g;
149 phi -= 0.07 * y_accel_g;
151 ////////////////////////////////////////////////////////////////////
152 // calculate target compass heading degrees
153 ////////////////////////////////////////////////////////////////////
155 // these are expensive: don't repeat
156 double sin_phi = sin(phi);
157 double sin_theta = sin(theta);
158 double sin_mu = sin(mu);
159 double cos_theta = cos(theta);
160 double cos_psi = cos(psi);
161 double cos_mu = cos(mu);
163 double a = cos(phi) * sin(psi) * cos_mu
164 - sin_phi * cos_theta * sin_mu
165 - sin_phi* sin_theta * cos_mu * cos_psi;
167 double b = cos_theta * cos_psi * cos(mu)
168 - sin_theta * sin_mu;
170 // This is the value that the compass
171 // is *trying* to display.
172 double target_deg = atan2(a, b) * SGD_RADIANS_TO_DEGREES;
174 if( _deviation_node ) {
175 target_deg -= _deviation_node->getDoubleValue();
176 } else if( _deviation_table ) {
177 target_deg -= _deviation_table->interpolate( SGMiscd::normalizePeriodic( 0.0, 360.0, target_deg ) );
180 double old_deg = _out_node->getDoubleValue();
182 while ((target_deg - old_deg) > 180.0)
184 while ((target_deg - old_deg) < -180.0)
187 // The compass has a current rate of
188 // rotation -- move the rate of rotation
189 // towards one that will turn the compass
190 // to the correct heading, but lag a bit.
191 // (so that the compass can keep overshooting
193 double error = target_deg - old_deg;
194 _rate_degps = fgGetLowPass(_rate_degps, error, delta_time_sec / 5.0);
195 double indicated_deg = old_deg + _rate_degps * delta_time_sec;
196 SG_NORMALIZE_RANGE(indicated_deg, 0.0, 360.0);
198 // That's it -- set the messed-up heading.
199 _out_node->setDoubleValue(indicated_deg);
202 // end of altimeter.cxx