Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AP_OpticalFlow_config.h"
17 :
18 : #if AP_OPTICALFLOW_CALIBRATOR_ENABLED
19 :
20 : #include "AP_OpticalFlow_Calibrator.h"
21 : #include <AP_InternalError/AP_InternalError.h>
22 :
23 : #include <GCS_MAVLink/GCS.h>
24 : #include <AP_Logger/AP_Logger.h>
25 : #include <AP_AHRS/AP_AHRS.h>
26 :
27 : const uint32_t AP_OPTICALFLOW_CAL_TIMEOUT_SEC = 120; // calibration timesout after 120 seconds
28 : const uint32_t AP_OPTICALFLOW_CAL_STATUSINTERVAL_SEC = 3; // status updates printed at 3 second intervals
29 : const float AP_OPTICALFLOW_CAL_YAW_MAX_RADS = radians(20); // maximum yaw rotation (must be low to ensure good scaling)
30 : const float AP_OPTICALFLOW_CAL_ROLLPITCH_MIN_RADS = radians(20); // minimum acceptable roll or pitch rotation
31 : const float AP_OPTICALFLOW_CAL_SCALE_MIN = 0.20f; // min acceptable scaling value. If resulting scaling is below this then the calibration fails
32 : const float AP_OPTICALFLOW_CAL_SCALE_MAX = 4.0f; // max acceptable scaling value. If resulting scaling is above this then the calibration fails
33 : const float AP_OPTICALFLOW_CAL_FITNESS_THRESH = 0.5f; // min acceptable fitness
34 : const float AP_OPTICALFLOW_CAL_RMS_FAILED = 1.0e30f; // calc_mean_squared_residuals returns this value when it fails to calculate a good value
35 :
36 : extern const AP_HAL::HAL& hal;
37 :
38 : // start the calibration
39 1 : void AP_OpticalFlow_Calibrator::start()
40 : {
41 : // exit immediately if already running
42 1 : if (_cal_state == CalState::RUNNING) {
43 0 : return;
44 : }
45 :
46 1 : _cal_state = CalState::RUNNING;
47 1 : _start_time_ms = AP_HAL::millis();
48 :
49 : // clear samples buffers
50 1 : _cal_data[0].num_samples = 0;
51 1 : _cal_data[1].num_samples = 0;
52 :
53 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FlowCal: Started");
54 : }
55 :
56 1 : void AP_OpticalFlow_Calibrator::stop()
57 : {
58 : // exit immediately if already stopped
59 1 : if (_cal_state == CalState::NOT_STARTED) {
60 0 : return;
61 : }
62 :
63 1 : _cal_state = CalState::NOT_STARTED;
64 :
65 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FlowCal: Stopped");
66 : }
67 :
68 : // update the state machine and calculate scaling
69 18751 : bool AP_OpticalFlow_Calibrator::update()
70 : {
71 : // prefix for reporting
72 18751 : const char* prefix_str = "FlowCal:";
73 : (void)prefix_str; // in case !HAL_GCS_ENABLED
74 :
75 : // while running add samples
76 18751 : if (_cal_state == CalState::RUNNING) {
77 11051 : uint32_t now_ms = AP_HAL::millis();
78 : uint32_t timestamp_ms;
79 11051 : Vector2f flow_rate, body_rate, los_pred;
80 11051 : if (AP::ahrs().getOptFlowSample(timestamp_ms, flow_rate, body_rate, los_pred)) {
81 11051 : add_sample(timestamp_ms, flow_rate, body_rate, los_pred);
82 :
83 : // while collecting samples display percentage complete
84 11051 : if (now_ms - _last_report_ms > AP_OPTICALFLOW_CAL_STATUSINTERVAL_SEC * 1000UL) {
85 19 : _last_report_ms = now_ms;
86 19 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s x:%d%% y:%d%%",
87 : prefix_str,
88 : (int)((_cal_data[0].num_samples * 100.0 / AP_OPTICALFLOW_CAL_MAX_SAMPLES)),
89 : (int)((_cal_data[1].num_samples * 100.0 / AP_OPTICALFLOW_CAL_MAX_SAMPLES)));
90 : }
91 :
92 : // advance state once sample buffers are full
93 11051 : if (sample_buffers_full()) {
94 1 : _cal_state = CalState::READY_TO_CALIBRATE;
95 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s samples collected", prefix_str);
96 : }
97 : }
98 :
99 : // check for timeout
100 11051 : if (now_ms - _start_time_ms > AP_OPTICALFLOW_CAL_TIMEOUT_SEC * 1000UL) {
101 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s timeout", prefix_str);
102 0 : _cal_state = CalState::FAILED;
103 : }
104 : }
105 :
106 : // start calibration
107 18751 : if (_cal_state == CalState::READY_TO_CALIBRATE) {
108 : // run calibration and mark failure or success
109 1 : if (run_calibration()) {
110 1 : _cal_state = CalState::SUCCESS;
111 1 : return true;
112 : } else {
113 0 : _cal_state = CalState::FAILED;
114 : }
115 : }
116 :
117 : // return indicating calibration is not complete
118 18750 : return false;
119 : }
120 :
121 : // get final scaling values
122 : // scaling values used during sample collection should be multiplied by these scalars
123 1 : Vector2f AP_OpticalFlow_Calibrator::get_scalars()
124 : {
125 : // return best scaling values
126 1 : return Vector2f{_cal_data[0].best_scalar, _cal_data[1].best_scalar};
127 : }
128 :
129 : // add new sample to the calibrator
130 11051 : void AP_OpticalFlow_Calibrator::add_sample(uint32_t timestamp_ms, const Vector2f& flow_rate, const Vector2f& body_rate, const Vector2f& los_pred)
131 : {
132 : // return immediately if not running
133 11051 : if (_cal_state != CalState::RUNNING) {
134 10497 : return;
135 : }
136 :
137 : // check for duplicates
138 11051 : if (timestamp_ms == _last_sample_timestamp_ms) {
139 10497 : return;
140 : }
141 554 : _last_sample_timestamp_ms = timestamp_ms;
142 :
143 : // check yaw rotation is low
144 554 : const Vector3f gyro = AP::ahrs().get_gyro();
145 554 : if (fabsf(gyro.z) > AP_OPTICALFLOW_CAL_YAW_MAX_RADS) {
146 0 : return;
147 : }
148 :
149 : // check enough roll or pitch movement and record sample
150 554 : const bool rates_x_sufficient = (fabsf(body_rate.x) >= AP_OPTICALFLOW_CAL_ROLLPITCH_MIN_RADS) && (fabsf(flow_rate.x) >= AP_OPTICALFLOW_CAL_ROLLPITCH_MIN_RADS);
151 554 : if (rates_x_sufficient && (_cal_data[0].num_samples < ARRAY_SIZE(_cal_data[0].samples))) {
152 : # if HAL_LOGGING_ENABLED
153 50 : log_sample(0, _cal_data[0].num_samples, flow_rate.x, body_rate.x, los_pred.x);
154 : #endif
155 50 : _cal_data[0].samples[_cal_data[0].num_samples].flow_rate = flow_rate.x;
156 50 : _cal_data[0].samples[_cal_data[0].num_samples].body_rate = body_rate.x;
157 50 : _cal_data[0].samples[_cal_data[0].num_samples].los_pred = los_pred.x;
158 50 : _cal_data[0].num_samples++;
159 : }
160 554 : const bool rates_y_sufficient = (fabsf(body_rate.y) >= AP_OPTICALFLOW_CAL_ROLLPITCH_MIN_RADS) && (fabsf(flow_rate.y) >= AP_OPTICALFLOW_CAL_ROLLPITCH_MIN_RADS);
161 554 : if (rates_y_sufficient && (_cal_data[1].num_samples < ARRAY_SIZE(_cal_data[1].samples))) {
162 : # if HAL_LOGGING_ENABLED
163 50 : log_sample(1, _cal_data[1].num_samples, flow_rate.y, body_rate.y, los_pred.y);
164 : #endif
165 50 : _cal_data[1].samples[_cal_data[1].num_samples].flow_rate = flow_rate.y;
166 50 : _cal_data[1].samples[_cal_data[1].num_samples].body_rate = body_rate.y;
167 50 : _cal_data[1].samples[_cal_data[1].num_samples].los_pred = los_pred.y;
168 50 : _cal_data[1].num_samples++;
169 : }
170 : }
171 :
172 : // returns true once the sample buffer is full
173 11051 : bool AP_OpticalFlow_Calibrator::sample_buffers_full() const
174 : {
175 11051 : return ((_cal_data[0].num_samples >= ARRAY_SIZE(_cal_data[0].samples)) && (_cal_data[1].num_samples >= ARRAY_SIZE(_cal_data[1].samples)));
176 : }
177 :
178 : // run calibration algorithm for both axis
179 : // returns true on success and updates _cal_data[0,1].best_scale and best_scale_fitness
180 1 : bool AP_OpticalFlow_Calibrator::run_calibration()
181 : {
182 : // run calibration for x and y axis
183 1 : const bool calx_res = calc_scalars(0, _cal_data[0].best_scalar, _cal_data[0].best_scalar_fitness);
184 1 : const bool caly_res = calc_scalars(1, _cal_data[1].best_scalar, _cal_data[1].best_scalar_fitness);
185 :
186 1 : return calx_res && caly_res;
187 : }
188 :
189 : // Run Gauss Newton fitting algorithm for all samples of the given axis
190 : // returns a scalar and fitness (lower numbers mean a better result) in the arguments provided
191 2 : bool AP_OpticalFlow_Calibrator::calc_scalars(uint8_t axis, float& scalar, float& fitness)
192 : {
193 : // prefix for reporting
194 2 : const char* prefix_str = "FlowCal:";
195 : (void)prefix_str; // in case !HAL_GCS_ENABLED
196 2 : const char* axis_str = axis == 0 ? "x" : "y";
197 : (void)axis_str; // in case !HAL_GCS_ENABLED
198 :
199 : // check we have samples
200 : // this should never fail because this method should only be called once the sample buffer is full
201 2 : const uint8_t num_samples = _cal_data[axis].num_samples;
202 2 : if (num_samples == 0) {
203 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s failed because no samples", prefix_str);
204 0 : return false;
205 : }
206 :
207 : // calculate total absolute residual from all samples
208 2 : float total_abs_residual = 0;
209 102 : for (uint8_t i = 0; i < num_samples; i++) {
210 100 : const sample_t& samplei = _cal_data[axis].samples[i];
211 100 : total_abs_residual += fabsf(calc_sample_residual(samplei, 1.0));
212 : }
213 :
214 : // if there are no residuals then scaling is perfect
215 2 : if (is_zero(total_abs_residual)) {
216 0 : scalar = 1.0;
217 0 : fitness = 0;
218 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s perfect scalar%s of 1.0", prefix_str, axis_str);
219 0 : return true;
220 : }
221 :
222 : // for each sample calculate the residual and scalar that best reduces the residual
223 2 : float best_scalar_total = 0;
224 102 : for (uint8_t i = 0; i < num_samples; i++) {
225 : float sample_best_scalar;
226 100 : const sample_t& samplei = _cal_data[axis].samples[i];
227 100 : if (!calc_sample_best_scalar(samplei, sample_best_scalar)) {
228 : // failed to find the best scalar for a single sample
229 : // this should never happen because of checks when capturing samples
230 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s failed because of zero flow rate", prefix_str);
231 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
232 0 : return false;
233 : }
234 100 : const float sample_residual = calc_sample_residual(samplei, 1.0);
235 100 : best_scalar_total += sample_best_scalar * fabsf(sample_residual) / total_abs_residual;
236 : }
237 :
238 : // check for out of range results
239 2 : if (best_scalar_total < AP_OPTICALFLOW_CAL_SCALE_MIN) {
240 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s scalar%s:%4.3f too low (<%3.1f)", prefix_str, axis_str, (double)best_scalar_total, (double)AP_OPTICALFLOW_CAL_SCALE_MIN);
241 0 : return false;
242 : }
243 2 : if (best_scalar_total > AP_OPTICALFLOW_CAL_SCALE_MAX) {
244 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s scalar%s:%4.3f too high (>%3.1f)", prefix_str, axis_str, (double)best_scalar_total, (double)AP_OPTICALFLOW_CAL_SCALE_MAX);
245 0 : return false;
246 : }
247 :
248 : // check for poor fitness
249 2 : float fitness_new = calc_mean_squared_residuals(axis, best_scalar_total);
250 2 : if (fitness_new > AP_OPTICALFLOW_CAL_FITNESS_THRESH) {
251 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s scalar%s:%4.3f fit:%4.3f too high (>%3.1f)", prefix_str, axis_str, (double)scalar, (double)fitness_new, (double)AP_OPTICALFLOW_CAL_FITNESS_THRESH);
252 0 : return false;
253 : }
254 :
255 : // success if fitness has improved
256 2 : float fitness_orig = calc_mean_squared_residuals(axis, 1.0);
257 2 : if (fitness_new <= fitness_orig) {
258 2 : scalar = best_scalar_total;
259 2 : fitness = fitness_new;
260 2 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s scalar%s:%4.3f fit:%4.2f", prefix_str, axis_str, (double)scalar, (double)fitness);
261 2 : return true;
262 : }
263 :
264 : // failed to find a better scalar than 1.0
265 0 : scalar = 1.0;
266 0 : fitness = fitness_orig;
267 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s no better scalar%s:%4.3f (fit:%4.3f > orig:%4.3f)", prefix_str, axis_str, (double)best_scalar_total, (double)fitness_new, (double)fitness_orig);
268 0 : return false;
269 : }
270 :
271 : // calculate a single sample's residual
272 400 : float AP_OpticalFlow_Calibrator::calc_sample_residual(const sample_t& sample, float scalar) const
273 : {
274 400 : return (sample.body_rate + ((sample.flow_rate * scalar) - sample.los_pred));
275 : }
276 :
277 : // calculate the scalar that minimises the residual for a single sample
278 : // returns true on success and populates the best_scalar argument
279 100 : bool AP_OpticalFlow_Calibrator::calc_sample_best_scalar(const sample_t& sample, float& best_scalar) const
280 : {
281 : // if sample's flow_rate is zero scalar has no effect
282 : // this should never happen because samples should have been checked before being added
283 100 : if (is_zero(sample.flow_rate)) {
284 0 : return false;
285 : }
286 100 : best_scalar = (sample.los_pred - sample.body_rate) / sample.flow_rate;
287 100 : return true;
288 : }
289 :
290 : // calculate mean squared residual for all samples of a single axis (0 or 1) given a scalar parameter
291 4 : float AP_OpticalFlow_Calibrator::calc_mean_squared_residuals(uint8_t axis, float scalar) const
292 : {
293 : // sanity check axis
294 4 : if (axis >= 2) {
295 0 : return AP_OPTICALFLOW_CAL_RMS_FAILED;
296 : }
297 :
298 : // calculate and sum residuals of each sample
299 4 : float sum = 0.0f;
300 4 : uint16_t num_samples = 0;
301 204 : for (uint8_t i = 0; i < _cal_data[axis].num_samples; i++) {
302 200 : sum += sq(calc_sample_residual(_cal_data[axis].samples[i], scalar));
303 200 : num_samples++;
304 : }
305 :
306 : // return a huge residual if no samples
307 4 : if (num_samples == 0) {
308 0 : return AP_OPTICALFLOW_CAL_RMS_FAILED;
309 : }
310 :
311 4 : sum /= num_samples;
312 4 : return sum;
313 : }
314 :
315 : #if HAL_LOGGING_ENABLED
316 : // log all samples
317 100 : void AP_OpticalFlow_Calibrator::log_sample(uint8_t axis, uint8_t sample_num, float flow_rate, float body_rate, float los_pred)
318 : {
319 : // @LoggerMessage: OFCA
320 : // @Description: Optical Flow Calibration sample
321 : // @Field: TimeUS: Time since system startup
322 : // @Field: Axis: Axis (X=0 Y=1)
323 : // @Field: Num: Sample number
324 : // @Field: FRate: Flow rate
325 : // @Field: BRate: Body rate
326 : // @Field: LPred: Los pred
327 :
328 100 : AP::logger().Write(
329 : "OFCA",
330 : "TimeUS,Axis,Num,FRate,BRate,LPred",
331 : "s#-EEE",
332 : "F00000",
333 : "QBBfff",
334 : AP_HAL::micros64(),
335 : (unsigned)axis,
336 : (unsigned)sample_num,
337 : (double)flow_rate,
338 : (double)body_rate,
339 : (double)los_pred);
340 100 : }
341 : #endif // HAL_LOGGING_ENABLED
342 :
343 : #endif // AP_OPTICALFLOW_CALIBRATOR_ENABLED
|
