]> git.mxchange.org Git - flightgear.git/blob - src/Instrumentation/mag_compass.cxx
Don't start paused for in air starts.
[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 ()
21     : _error_deg(0.0),
22       _rate_degps(0.0)
23 {
24 }
25
26 MagCompass::~MagCompass ()
27 {
28 }
29
30 void
31 MagCompass::init ()
32 {
33     _serviceable_node =
34         fgGetNode("/instrumentation/magnetic-compass/serviceable", true);
35     _heading_node =
36         fgGetNode("/orientation/heading-deg", true);
37     _beta_node =
38         fgGetNode("/orientation/side-slip-deg", true);
39     _variation_node =
40         fgGetNode("/environment/magnetic-variation-deg", true);
41     _dip_node =
42         fgGetNode("/environment/magnetic-dip-deg", true);
43     _north_accel_node =
44         fgGetNode("/accelerations/ned/north-accel-fps_sec", true);
45     _east_accel_node =
46         fgGetNode("/accelerations/ned/east-accel-fps_sec", true);
47     _down_accel_node =
48         fgGetNode("/accelerations/ned/down-accel-fps_sec", true);
49     _out_node =
50         fgGetNode("/instrumentation/magnetic-compass/indicated-heading-deg",
51                   true);
52 }
53
54 void
55 MagCompass::update (double delta_time_sec)
56 {
57                                 // algorithm from Alex Perry
58                                 // possibly broken by David Megginson
59
60                                 // don't update if it's broken
61     if (!_serviceable_node->getBoolValue())
62         return;
63
64                                 // jam on a sideslip of 12 degrees or more
65     if (fabs(_beta_node->getDoubleValue()) > 12.0) {
66         _rate_degps = 0.0;
67         _error_deg = _heading_node->getDoubleValue() -
68             _out_node->getDoubleValue();
69         return;
70     }
71
72     double accelN = _north_accel_node->getDoubleValue();
73     double accelE = _east_accel_node->getDoubleValue();
74     double accelU = _down_accel_node->getDoubleValue() - 32.0; // why?
75
76                                 // force vector towards magnetic north pole
77     double var = _variation_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
78     double dip = _dip_node->getDoubleValue() * SGD_DEGREES_TO_RADIANS;
79     double cosdip = cos(dip);
80     double forceN = cosdip * cos(var);
81     double forceE = cosdip * sin(var);
82     double forceU = sin(dip);
83
84                                 // rotation is around acceleration axis
85                                 // (magnitude doesn't matter)
86     double accel = accelN * accelN + accelE * accelE + accelU * accelU;
87     if (accel > 1.0)
88         accel = sqrt(accel);
89     else
90             accel = 1.0;
91
92                                 // North marking on compass card
93     double edgeN = cos(_error_deg * SGD_DEGREES_TO_RADIANS);
94     double edgeE = sin(_error_deg * SGD_DEGREES_TO_RADIANS);
95     double edgeU = 0.0;
96
97                                 // apply the force to that edge to get torques
98     double torqueN = edgeE * forceU - edgeU * forceE;
99     double torqueE = edgeU * forceN - edgeN * forceU;
100     double torqueU = edgeN * forceE - edgeE * forceN;
101
102                                 // get the component parallel to the axis
103     double torque = (torqueN * accelN +
104                      torqueE * accelE +
105                      torqueU * accelU) * 5.0 / accel;
106
107                                 // the compass has angular momentum,
108                                 // so apply a torque and wait
109     if (delta_time_sec < 1.0) {
110         _rate_degps = _rate_degps * (1.0 - delta_time_sec) - torque;
111         _error_deg += delta_time_sec * _rate_degps;
112     }
113     if (_error_deg > 180.0)
114         _error_deg -= 360.0;
115     else if (_error_deg < -180.0)
116         _error_deg += 360.0;
117
118                                 // Set the indicated heading
119     _out_node->setDoubleValue(_heading_node->getDoubleValue() - _error_deg);
120 }
121
122 // end of altimeter.cxx