]> git.mxchange.org Git - flightgear.git/blob - src/Instrumentation/mag_compass.cxx
e119bb836bb2ca5851cbeb2e84d71ea00549cb99
[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
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("magnetic-compass"),
24       num(0)
25 {
26     int i;
27     for ( i = 0; i < node->nChildren(); ++i ) {
28         SGPropertyNode *child = node->getChild(i);
29         string cname = child->getName();
30         string cval = child->getStringValue();
31         if ( cname == "name" ) {
32             name = cval;
33         } else if ( cname == "number" ) {
34             num = child->getIntValue();
35         } else {
36             SG_LOG( SG_INSTR, SG_WARN, "Error in magnetic-compass config logic" );
37             if ( name.length() ) {
38                 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
39             }
40         }
41     }
42 }
43
44 MagCompass::MagCompass ()
45     : _error_deg(0.0),
46       _rate_degps(0.0)
47 {
48 }
49
50 MagCompass::~MagCompass ()
51 {
52 }
53
54 void
55 MagCompass::init ()
56 {
57     string branch;
58     branch = "/instrumentation/" + name;
59
60     SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
61     _serviceable_node = node->getChild("serviceable", 0, true);
62     _roll_node =
63         fgGetNode("/orientation/roll-deg", true);
64     _pitch_node =
65         fgGetNode("/orientation/pitch-deg", true);
66     _heading_node =
67         fgGetNode("/orientation/heading-magnetic-deg", true);
68     _beta_node =
69         fgGetNode("/orientation/side-slip-deg", true);
70     _dip_node =
71         fgGetNode("/environment/magnetic-dip-deg", true);
72     _north_accel_node =
73         fgGetNode("/accelerations/ned/north-accel-fps_sec", true);
74     _east_accel_node =
75         fgGetNode("/accelerations/ned/east-accel-fps_sec", true);
76     _down_accel_node =
77         fgGetNode("/accelerations/ned/down-accel-fps_sec", true);
78     _out_node = node->getChild("indicated-heading-deg", 0, true);
79
80     _serviceable_node->setBoolValue(true);
81 }
82
83
84 void
85 MagCompass::update (double delta_time_sec)
86 {
87                                 // don't update if it's broken
88     if (!_serviceable_node->getBoolValue())
89         return;
90
91     /*
92       Formula for northernly turning error from
93       http://williams.best.vwh.net/compass/node4.html:
94
95       Hc: compass heading
96       psi: magnetic heading
97       theta: bank angle (right positive; should be phi here)
98       mu: dip angle (down positive)
99
100       Hc = atan2(sin(Hm)cos(theta)-tan(mu)sin(theta), cos(Hm))
101
102       This function changes the variable names to the more common
103       psi for the heading, theta for the pitch, and phi for the
104       roll.  It also modifies the equation to incorporate pitch
105       as well as roll, as suggested by Chris Metzler.
106     */
107
108                                 // bank angle (radians)
109     double phi = _roll_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
110
111                                 // pitch angle (radians)
112     double theta = _pitch_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
113
114                                 // magnetic heading (radians)
115     double psi = _heading_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
116
117                                 // magnetic dip (radians)
118     double mu = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
119
120
121     ////////////////////////////////////////////////////////////////////
122     // calculate target compass heading Hc in degrees
123     ////////////////////////////////////////////////////////////////////
124
125                                 // these are expensive: don't repeat
126     double sin_phi = sin(phi);
127     double sin_theta = sin(theta);
128     double sin_mu = sin(mu);
129     double cos_theta = cos(theta);
130     double cos_psi = cos(psi);
131     double cos_mu = cos(mu);
132
133     double a = cos(phi) * sin(psi) * cos_mu
134         - sin_phi * cos_theta * sin_mu
135         - sin_phi* sin_theta * cos_mu * cos_psi;
136
137     double b = cos_theta * cos_psi * cos(mu)
138         - sin_theta * sin_mu;
139
140     double Hc = atan2(a, b) * SGD_RADIANS_TO_DEGREES;
141
142     while (Hc < 0)
143         Hc += 360;
144     while (Hc >= 360)
145         Hc =- 360;
146
147     // TODO add acceleration error
148     // TODO allow binding with excessive dip/sideslip
149
150     _out_node->setDoubleValue(Hc);
151
152
153                                 // algorithm from Alex Perry
154                                 // possibly broken by David Megginson
155
156 //                                 // jam on a sideslip of 12 degrees or more
157 //     if (fabs(_beta_node->getDoubleValue()) > 12.0) {
158 //         _rate_degps = 0.0;
159 //         _error_deg = _heading_node->getDoubleValue() -
160 //             _out_node->getDoubleValue();
161 //         return;
162 //     }
163
164 //     double accelN = _north_accel_node->getDoubleValue();
165 //     double accelE = _east_accel_node->getDoubleValue();
166 //     double accelU = _down_accel_node->getDoubleValue() - 32.0; // why?
167
168 //                                 // force vector towards magnetic north pole
169 //     double var = _variation_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
170 //     double dip = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
171 //     double cosdip = cos(dip);
172 //     double forceN = cosdip * cos(var);
173 //     double forceE = cosdip * sin(var);
174 //     double forceU = sin(dip);
175
176 //                                 // rotation is around acceleration axis
177 //                                 // (magnitude doesn't matter)
178 //     double accel = accelN * accelN + accelE * accelE + accelU * accelU;
179 //     if (accel > 1.0)
180 //         accel = sqrt(accel);
181 //     else
182 //             accel = 1.0;
183
184 //                                 // North marking on compass card
185 //     double edgeN = cos(_error_deg * SGD_DEGREES_TO_RADIANS);
186 //     double edgeE = sin(_error_deg * SGD_DEGREES_TO_RADIANS);
187 //     double edgeU = 0.0;
188
189 //                                 // apply the force to that edge to get torques
190 //     double torqueN = edgeE * forceU - edgeU * forceE;
191 //     double torqueE = edgeU * forceN - edgeN * forceU;
192 //     double torqueU = edgeN * forceE - edgeE * forceN;
193
194 //                                 // get the component parallel to the axis
195 //     double torque = (torqueN * accelN +
196 //                      torqueE * accelE +
197 //                      torqueU * accelU) * 5.0 / accel;
198
199 //                                 // the compass has angular momentum,
200 //                                 // so apply a torque and wait
201 //     if (delta_time_sec < 1.0) {
202 //         _rate_degps = _rate_degps * (1.0 - delta_time_sec) - torque;
203 //         _error_deg += delta_time_sec * _rate_degps;
204 //     }
205 //     if (_error_deg > 180.0)
206 //         _error_deg -= 360.0;
207 //     else if (_error_deg < -180.0)
208 //         _error_deg += 360.0;
209
210 //                                 // Set the indicated heading
211 //     _out_node->setDoubleValue(_heading_node->getDoubleValue() - _error_deg);
212 }
213
214 // end of altimeter.cxx