]> git.mxchange.org Git - flightgear.git/blob - src/Instrumentation/mag_compass.cxx
Melchior FRANZ:
[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 <plib/sg.h>
14 #include <simgear/sg_inlines.h>
15
16 #include "mag_compass.hxx"
17 #include <Main/fg_props.hxx>
18 #include <Main/util.hxx>
19
20
21 MagCompass::MagCompass ( SGPropertyNode *node )
22     : _error_deg(0.0),
23       _rate_degps(0.0),
24       name("magnetic-compass"),
25       num(0)
26 {
27     int i;
28     for ( i = 0; i < node->nChildren(); ++i ) {
29         SGPropertyNode *child = node->getChild(i);
30         string cname = child->getName();
31         string cval = child->getStringValue();
32         if ( cname == "name" ) {
33             name = cval;
34         } else if ( cname == "number" ) {
35             num = child->getIntValue();
36         } else {
37             SG_LOG( SG_INSTR, SG_WARN, "Error in magnetic-compass config logic" );
38             if ( name.length() ) {
39                 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
40             }
41         }
42     }
43 }
44
45 MagCompass::MagCompass ()
46     : _error_deg(0.0),
47       _rate_degps(0.0)
48 {
49 }
50
51 MagCompass::~MagCompass ()
52 {
53 }
54
55 void
56 MagCompass::init ()
57 {
58     string branch;
59     branch = "/instrumentation/" + name;
60
61     SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
62     _serviceable_node = node->getChild("serviceable", 0, true);
63     _roll_node =
64         fgGetNode("/orientation/roll-deg", true);
65     _pitch_node =
66         fgGetNode("/orientation/pitch-deg", true);
67     _heading_node =
68         fgGetNode("/orientation/heading-magnetic-deg", true);
69     _beta_node =
70         fgGetNode("/orientation/side-slip-deg", true);
71     _dip_node =
72         fgGetNode("/environment/magnetic-dip-deg", true);
73     _x_accel_node =
74         fgGetNode("/accelerations/pilot/x-accel-fps_sec", true);
75     _y_accel_node =
76         fgGetNode("/accelerations/pilot/y-accel-fps_sec", true);
77     _z_accel_node =
78         fgGetNode("/accelerations/pilot/z-accel-fps_sec", true);
79     _out_node = node->getChild("indicated-heading-deg", 0, true);
80 }
81
82
83 void
84 MagCompass::update (double delta_time_sec)
85 {
86                                 // This is the real magnetic
87                                 // which would be displayed
88                                 // if the compass had no errors.
89     double heading_mag_deg = _heading_node->getDoubleValue();
90
91
92                                 // don't update if the compass
93                                 // is broken
94     if (!_serviceable_node->getBoolValue())
95         return;
96
97                                 // jam on excessive sideslip
98     if (fabs(_beta_node->getDoubleValue()) > 12.0) {
99         _rate_degps = 0.0;
100         return;
101     }
102
103
104     /*
105       Formula for northernly turning error from
106       http://williams.best.vwh.net/compass/node4.html:
107
108       Hc: compass heading
109       psi: magnetic heading
110       theta: bank angle (right positive; should be phi here)
111       mu: dip angle (down positive)
112
113       Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm))
114
115       This function changes the variable names to the more common psi
116       for the heading, theta for the pitch, and phi for the roll (and
117       target_deg for Hc).  It also modifies the equation to
118       incorporate pitch as well as roll, as suggested by Chris
119       Metzler.
120     */
121
122                                 // bank angle (radians)
123     double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
124
125                                 // pitch angle (radians)
126     double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
127
128                                 // magnetic heading (radians)
129     double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
130
131                                 // magnetic dip (radians)
132     double mu = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
133
134
135     /*
136       Tilt adjustments for accelerations.
137       
138       The magnitudes of these are totally made up, but in real life,
139       they would depend on the fluid level, the amount of friction,
140       etc. anyway.  Basically, the compass float tilts forward for
141       acceleration and backward for deceleration.  Tilt about 4
142       degrees (0.07 radians) for every G (32 fps/sec) of
143       acceleration.
144
145       TODO: do something with the vertical acceleration.
146     */
147     double x_accel_g = _x_accel_node->getDoubleValue() / 32;
148     double y_accel_g = _y_accel_node->getDoubleValue() / 32;
149     double z_accel_g = _z_accel_node->getDoubleValue() / 32;
150
151     theta -= 0.07 * x_accel_g;
152     phi -= 0.07 * y_accel_g;
153     
154     ////////////////////////////////////////////////////////////////////
155     // calculate target compass heading degrees
156     ////////////////////////////////////////////////////////////////////
157
158                                 // these are expensive: don't repeat
159     double sin_phi = sin(phi);
160     double sin_theta = sin(theta);
161     double sin_mu = sin(mu);
162     double cos_theta = cos(theta);
163     double cos_psi = cos(psi);
164     double cos_mu = cos(mu);
165
166     double a = cos(phi) * sin(psi) * cos_mu
167         - sin_phi * cos_theta * sin_mu
168         - sin_phi* sin_theta * cos_mu * cos_psi;
169
170     double b = cos_theta * cos_psi * cos(mu)
171         - sin_theta * sin_mu;
172
173                                 // This is the value that the compass
174                                 // is *trying* to display.
175     double target_deg = atan2(a, b) * SGD_RADIANS_TO_DEGREES;
176     double old_deg = _out_node->getDoubleValue();
177
178     while ((target_deg - old_deg) > 180.0)
179         target_deg -= 360.0;
180     while ((target_deg - old_deg) < -180.0)
181         target_deg += 360.0;
182
183                                 // The compass has a current rate of
184                                 // rotation -- move the rate of rotation
185                                 // towards one that will turn the compass
186                                 // to the correct heading, but lag a bit.
187                                 // (so that the compass can keep overshooting
188                                 // and coming back).
189     double error = target_deg - old_deg;
190     _rate_degps = fgGetLowPass(_rate_degps, error, delta_time_sec / 5.0);
191     double indicated_deg = old_deg + _rate_degps * delta_time_sec;
192     SG_NORMALIZE_RANGE(indicated_deg, 0.0, 360.0);
193
194                                 // That's it -- set the messed-up heading.
195     _out_node->setDoubleValue(indicated_deg);
196 }
197
198 // end of altimeter.cxx