]> git.mxchange.org Git - flightgear.git/blob - src/Instrumentation/mag_compass.cxx
Merge branch 'durk/ai-atc'
[flightgear.git] / src / Instrumentation / mag_compass.cxx
1 // mag_compass.cxx - a magnetic compass.
2 // Written by David Megginson, started 2003.
3 //
4 // This file is in the Public Domain and comes with no warranty.
5
6 // This implementation is derived from an earlier one by Alex Perry,
7 // which appeared in src/Cockpit/steam.cxx
8
9 #ifdef HAVE_CONFIG_H
10 #  include <config.h>
11 #endif
12
13 #include <simgear/sg_inlines.h>
14
15 #include "mag_compass.hxx"
16 #include <Main/fg_props.hxx>
17 #include <Main/util.hxx>
18
19
20 MagCompass::MagCompass ( SGPropertyNode *node )
21     : _error_deg(0.0),
22       _rate_degps(0.0),
23       _name(node->getStringValue("name", "magnetic-compass")),
24       _num(node->getIntValue("number", 0))
25 {
26 }
27
28 MagCompass::~MagCompass ()
29 {
30 }
31
32 void
33 MagCompass::init ()
34 {
35     string branch;
36     branch = "/instrumentation/" + _name;
37
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);
49 }
50
51
52 void
53 MagCompass::update (double delta_time_sec)
54 {
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();
59
60
61                                 // don't update if the compass
62                                 // is broken
63     if (!_serviceable_node->getBoolValue())
64         return;
65
66     /*
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.
71      */
72 #if 0
73                                 // jam on excessive sideslip
74     if (fabs(_beta_node->getDoubleValue()) > 12.0) {
75         _rate_degps = 0.0;
76         return;
77     }
78 #endif
79
80
81     /*
82       Formula for northernly turning error from
83       http://williams.best.vwh.net/compass/node4.html:
84
85       Hc: compass heading
86       psi: magnetic heading
87       theta: bank angle (right positive; should be phi here)
88       mu: dip angle (down positive)
89
90       Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm))
91
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
96       Metzler.
97     */
98
99                                 // bank angle (radians)
100     double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
101
102                                 // pitch angle (radians)
103     double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
104
105                                 // magnetic heading (radians)
106     double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
107
108                                 // magnetic dip (radians)
109     double mu = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
110
111
112     /*
113       Tilt adjustments for accelerations.
114       
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
120       acceleration.
121
122       TODO: do something with the vertical acceleration.
123     */
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;
127
128     theta -= 0.07 * x_accel_g;
129     phi -= 0.07 * y_accel_g;
130     
131     ////////////////////////////////////////////////////////////////////
132     // calculate target compass heading degrees
133     ////////////////////////////////////////////////////////////////////
134
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);
142
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;
146
147     double b = cos_theta * cos_psi * cos(mu)
148         - sin_theta * sin_mu;
149
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();
154
155     while ((target_deg - old_deg) > 180.0)
156         target_deg -= 360.0;
157     while ((target_deg - old_deg) < -180.0)
158         target_deg += 360.0;
159
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
165                                 // and coming back).
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);
170
171                                 // That's it -- set the messed-up heading.
172     _out_node->setDoubleValue(indicated_deg);
173 }
174
175 // end of altimeter.cxx