Line data Source code
1 : /*
2 : * auto_calibration.cpp - airspeed auto calibration
3 : *
4 : * Algorithm by Paul Riseborough
5 : *
6 : */
7 :
8 : #include "AP_Airspeed_config.h"
9 :
10 : #if AP_AIRSPEED_ENABLED
11 :
12 : #include <AP_Common/AP_Common.h>
13 : #include <AP_Math/AP_Math.h>
14 : #include <GCS_MAVLink/GCS.h>
15 : #include <AP_AHRS/AP_AHRS.h>
16 : #include <SRV_Channel/SRV_Channel.h>
17 :
18 : #include "AP_Airspeed.h"
19 :
20 :
21 : // constructor - fill in all the initial values
22 4196 : Airspeed_Calibration::Airspeed_Calibration()
23 4196 : : P(100, 0, 0,
24 : 0, 100, 0,
25 : 0, 0, 0.000001f)
26 4196 : , Q0(0.01f)
27 4196 : , Q1(0.0000005f)
28 4196 : , state(0, 0, 0)
29 : {
30 4196 : }
31 :
32 : /*
33 : initialise the ratio
34 : */
35 1398 : void Airspeed_Calibration::init(float initial_ratio)
36 : {
37 1398 : state.z = 1.0f / sqrtf(initial_ratio);
38 1398 : }
39 :
40 : /*
41 : update the state of the airspeed calibration - needs to be called
42 : once a second
43 : */
44 402 : float Airspeed_Calibration::update(float airspeed, const Vector3f &vg, int16_t max_airspeed_allowed_during_cal)
45 : {
46 : // Perform the covariance prediction
47 : // Q is a diagonal matrix so only need to add three terms in
48 : // C code implementation
49 : // P = P + Q;
50 402 : P.a.x += Q0;
51 402 : P.b.y += Q0;
52 402 : P.c.z += Q1;
53 :
54 : // Perform the predicted measurement using the current state estimates
55 : // No state prediction required because states are assumed to be time
56 : // invariant plus process noise
57 : // Ignore vertical wind component
58 402 : float TAS_pred = state.z * norm(vg.x - state.x, vg.y - state.y, vg.z);
59 402 : float TAS_mea = airspeed;
60 :
61 : // Calculate the observation Jacobian H_TAS
62 402 : float SH1 = sq(vg.y - state.y) + sq(vg.x - state.x);
63 402 : if (SH1 < 0.000001f) {
64 : // avoid division by a small number
65 0 : return state.z;
66 : }
67 402 : float SH2 = 1/sqrtf(SH1);
68 :
69 : // observation Jacobian
70 : Vector3f H_TAS(
71 402 : -(state.z*SH2*(2*vg.x - 2*state.x))/2,
72 402 : -(state.z*SH2*(2*vg.y - 2*state.y))/2,
73 402 : 1/SH2);
74 :
75 : // Calculate the fusion innovation covariance assuming a TAS measurement
76 : // noise of 1.0 m/s
77 : // S = H_TAS*P*H_TAS' + 1.0; % [1 x 3] * [3 x 3] * [3 x 1] + [1 x 1]
78 402 : Vector3f PH = P * H_TAS;
79 402 : float S = H_TAS * PH + 1.0f;
80 :
81 : // Calculate the Kalman gain
82 : // [3 x 3] * [3 x 1] / [1 x 1]
83 402 : Vector3f KG = PH / S;
84 :
85 : // Update the states
86 402 : state += KG*(TAS_mea - TAS_pred); // [3 x 1] + [3 x 1] * [1 x 1]
87 :
88 : // Update the covariance matrix
89 402 : Vector3f HP2 = H_TAS.row_times_mat(P);
90 402 : P -= KG.mul_rowcol(HP2);
91 :
92 : // force symmetry on the covariance matrix - necessary due to rounding
93 : // errors
94 402 : float P12 = 0.5f * (P.a.y + P.b.x);
95 402 : float P13 = 0.5f * (P.a.z + P.c.x);
96 402 : float P23 = 0.5f * (P.b.z + P.c.y);
97 402 : P.a.y = P.b.x = P12;
98 402 : P.a.z = P.c.x = P13;
99 402 : P.b.z = P.c.y = P23;
100 :
101 : // Constrain diagonals to be non-negative - protects against rounding errors
102 402 : P.a.x = MAX(P.a.x, 0.0f);
103 402 : P.b.y = MAX(P.b.y, 0.0f);
104 402 : P.c.z = MAX(P.c.z, 0.0f);
105 :
106 402 : state.x = constrain_float(state.x, -max_airspeed_allowed_during_cal, max_airspeed_allowed_during_cal);
107 402 : state.y = constrain_float(state.y, -max_airspeed_allowed_during_cal, max_airspeed_allowed_during_cal);
108 402 : state.z = constrain_float(state.z, 0.5f, 1.0f);
109 :
110 402 : return state.z;
111 : }
112 :
113 :
114 : /*
115 : called once a second to do calibration update
116 : */
117 56510 : void AP_Airspeed::update_calibration(uint8_t i, const Vector3f &vground, int16_t max_airspeed_allowed_during_cal)
118 : {
119 : #if AP_AIRSPEED_AUTOCAL_ENABLE
120 56510 : if (!param[i].autocal && !calibration_enabled) {
121 : // auto-calibration not enabled
122 56108 : return;
123 : }
124 :
125 402 : if (param[i].use == 2 && !is_zero(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle))) {
126 : // special case for gliders with airspeed sensors behind the
127 : // propeller. Allow airspeed to be disabled when throttle is
128 : // running
129 0 : return;
130 : }
131 :
132 : // set state.z based on current ratio, this allows the operator to
133 : // override the current ratio in flight with autocal, which is
134 : // very useful both for testing and to force a reasonable value.
135 402 : float ratio = constrain_float(param[i].ratio, 1.0f, 4.0f);
136 :
137 402 : state[i].calibration.state.z = 1.0f / sqrtf(ratio);
138 :
139 : // calculate true airspeed, assuming a airspeed ratio of 1.0
140 402 : float dpress = MAX(get_differential_pressure(i), 0);
141 402 : float true_airspeed = sqrtf(dpress) * AP::ahrs().get_EAS2TAS();
142 :
143 402 : float zratio = state[i].calibration.update(true_airspeed, vground, max_airspeed_allowed_during_cal);
144 :
145 402 : if (isnan(zratio) || isinf(zratio)) {
146 0 : return;
147 : }
148 :
149 : // this constrains the resulting ratio to between 1.0 and 4.0
150 402 : zratio = constrain_float(zratio, 0.5f, 1.0f);
151 402 : param[i].ratio.set(1/sq(zratio));
152 402 : if (state[i].counter > 60) {
153 233 : if (state[i].last_saved_ratio > 1.05f*param[i].ratio ||
154 116 : state[i].last_saved_ratio < 0.95f*param[i].ratio) {
155 2 : param[i].ratio.save();
156 2 : state[i].last_saved_ratio = param[i].ratio;
157 2 : state[i].counter = 0;
158 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u ratio reset: %f", i , static_cast<double> (param[i].ratio));
159 : }
160 : } else {
161 285 : state[i].counter++;
162 : }
163 : #endif // AP_AIRSPEED_AUTOCAL_ENABLE
164 : }
165 :
166 : /*
167 : called once a second to do calibration update
168 : */
169 28255 : void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal)
170 : {
171 84765 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
172 56510 : update_calibration(i, vground, max_airspeed_allowed_during_cal);
173 : }
174 : #if HAL_GCS_ENABLED && AP_AIRSPEED_AUTOCAL_ENABLE
175 28255 : send_airspeed_calibration(vground);
176 : #endif
177 28255 : }
178 :
179 :
180 : #if HAL_GCS_ENABLED && AP_AIRSPEED_AUTOCAL_ENABLE
181 28255 : void AP_Airspeed::send_airspeed_calibration(const Vector3f &vground)
182 : {
183 : /*
184 : the AIRSPEED_AUTOCAL message doesn't have an instance number
185 : so we can only send it for one sensor at a time
186 : */
187 84363 : for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
188 56397 : if (!param[i].autocal && !calibration_enabled) {
189 : // auto-calibration not enabled on this sensor
190 56108 : continue;
191 : }
192 : const mavlink_airspeed_autocal_t packet{
193 289 : vx: vground.x,
194 289 : vy: vground.y,
195 289 : vz: vground.z,
196 289 : diff_pressure: get_differential_pressure(i),
197 289 : EAS2TAS: AP::ahrs().get_EAS2TAS(),
198 578 : ratio: param[i].ratio.get(),
199 289 : state_x: state[i].calibration.state.x,
200 289 : state_y: state[i].calibration.state.y,
201 289 : state_z: state[i].calibration.state.z,
202 289 : Pax: state[i].calibration.P.a.x,
203 289 : Pby: state[i].calibration.P.b.y,
204 289 : Pcz: state[i].calibration.P.c.z
205 578 : };
206 289 : gcs().send_to_active_channels(MAVLINK_MSG_ID_AIRSPEED_AUTOCAL,
207 : (const char *)&packet);
208 289 : break; // we can only send for one sensor
209 : }
210 28255 : }
211 : #endif // HAL_GCS_ENABLED && AP_AIRSPEED_AUTOCAL_ENABLE
212 :
213 : #endif // AP_AIRSPEED_ENABLED
|
