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 : /*
17 : * The intention of a magnetometer in a compass application is to measure
18 : * Earth's magnetic field. Measurements other than those of Earth's magnetic
19 : * field are considered errors. This algorithm computes a set of correction
20 : * parameters that null out errors from various sources:
21 : *
22 : * - Sensor bias error
23 : * - "Hard iron" error caused by materials fixed to the vehicle body that
24 : * produce static magnetic fields.
25 : * - Sensor scale-factor error
26 : * - Sensor cross-axis sensitivity
27 : * - "Soft iron" error caused by materials fixed to the vehicle body that
28 : * distort magnetic fields.
29 : *
30 : * This is done by taking a set of samples that are assumed to be the product
31 : * of rotation in earth's magnetic field and fitting an offset ellipsoid to
32 : * them, determining the correction to be applied to adjust the samples into an
33 : * origin-centered sphere.
34 : *
35 : * The state machine of this library is described entirely by the
36 : * CompassCalibrator::Status enum, and all state transitions are managed by the
37 : * set_status function. Normally, the library is in the NOT_STARTED state. When
38 : * the start function is called, the state transitions to WAITING_TO_START,
39 : * until two conditions are met: the delay as elapsed, and the memory for the
40 : * sample buffer has been successfully allocated.
41 : * Once these conditions are met, the state transitions to RUNNING_STEP_ONE, and
42 : * samples are collected via calls to the new_sample function. These samples are
43 : * accepted or rejected based on distance to the nearest sample. The samples are
44 : * assumed to cover the surface of a sphere, and the radius of that sphere is
45 : * initialized to a conservative value. Based on a circle-packing pattern, the
46 : * minimum distance is set such that some percentage of the surface of that
47 : * sphere must be covered by samples.
48 : *
49 : * Once the sample buffer is full, a sphere fitting algorithm is run, which
50 : * computes a new sphere radius. The sample buffer is thinned of samples which
51 : * no longer meet the acceptance criteria, and the state transitions to
52 : * RUNNING_STEP_TWO. Samples continue to be collected until the buffer is full
53 : * again, the full ellipsoid fit is run, and the state transitions to either
54 : * SUCCESS or FAILED.
55 : *
56 : * The fitting algorithm used is Levenberg-Marquardt. See also:
57 : * http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm
58 : */
59 :
60 : #include "AP_Compass_config.h"
61 :
62 : #if COMPASS_CAL_ENABLED
63 :
64 : #include "AP_Compass.h"
65 : #include "CompassCalibrator.h"
66 : #include <AP_HAL/AP_HAL.h>
67 : #include <AP_Math/AP_GeodesicGrid.h>
68 : #include <AP_AHRS/AP_AHRS.h>
69 : #include <AP_GPS/AP_GPS.h>
70 : #include <GCS_MAVLink/GCS.h>
71 : #include <AP_InternalError/AP_InternalError.h>
72 :
73 : #define FIELD_RADIUS_MIN 150
74 : #define FIELD_RADIUS_MAX 950
75 :
76 : // When retry is enabled, hold a FAILED_* status for this long before starting
77 : // the next attempt, so a GCS polling MAG_CAL_REPORT (streamed in the EXTRA3
78 : // group at a few Hz) reliably observes the specific failure reason before the
79 : // retry overwrites it.
80 : #define COMPASS_CAL_RETRY_REPORT_HOLD_MS 500
81 :
82 : ////////////////////////////////////////////////////////////
83 : ///////////////////// PUBLIC INTERFACE /////////////////////
84 : ////////////////////////////////////////////////////////////
85 :
86 9 : CompassCalibrator::CompassCalibrator()
87 : {
88 9 : set_status(Status::NOT_STARTED);
89 9 : }
90 :
91 : // Request to cancel calibration
92 18 : void CompassCalibrator::stop()
93 : {
94 18 : WITH_SEMAPHORE(state_sem);
95 18 : _requested_status = Status::NOT_STARTED;
96 18 : _status_set_requested = true;
97 18 : }
98 :
99 0 : void CompassCalibrator::set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation, bool always_45_deg)
100 : {
101 0 : WITH_SEMAPHORE(state_sem);
102 0 : cal_settings.check_orientation = true;
103 0 : cal_settings.orientation = orientation;
104 0 : cal_settings.orig_orientation = orientation;
105 0 : cal_settings.is_external = is_external;
106 0 : cal_settings.fix_orientation = fix_orientation;
107 0 : cal_settings.always_45_deg = always_45_deg;
108 0 : }
109 :
110 57 : void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx, float tolerance)
111 : {
112 57 : if (compass_idx > COMPASS_MAX_INSTANCES) {
113 27 : return;
114 : }
115 :
116 57 : WITH_SEMAPHORE(state_sem);
117 : // Don't do this while we are already started
118 57 : if (_running()) {
119 27 : return;
120 : }
121 30 : cal_settings.offset_max = offset_max;
122 30 : cal_settings.attempt = 1;
123 30 : cal_settings.retry = retry;
124 30 : cal_settings.delay_start_sec = delay;
125 30 : cal_settings.start_time_ms = AP_HAL::millis();
126 30 : cal_settings.compass_idx = compass_idx;
127 30 : cal_settings.tolerance = tolerance;
128 :
129 : // Request status change to Waiting to start
130 30 : _requested_status = Status::WAITING_TO_START;
131 30 : _status_set_requested = true;
132 57 : }
133 :
134 : // Record point mag sample and associated attitude sample to intermediate struct
135 596089 : void CompassCalibrator::new_sample(const Vector3f& sample)
136 : {
137 596089 : WITH_SEMAPHORE(sample_sem);
138 596089 : _last_sample.set(sample);
139 596089 : _last_sample.att.set_from_ahrs();
140 596089 : _new_sample = true;
141 596089 : }
142 :
143 595912 : bool CompassCalibrator::failed() {
144 595912 : WITH_SEMAPHORE(state_sem);
145 595912 : switch (cal_state.status) {
146 15352 : case Status::FAILED:
147 : case Status::FAILED_ORIENTATION:
148 : case Status::FAILED_RADIUS:
149 : case Status::FAILED_OFFSETS:
150 : case Status::FAILED_DIAG_SCALING:
151 : case Status::FAILED_RESIDUALS_HIGH:
152 15352 : return true;
153 580560 : case Status::SUCCESS:
154 : case Status::NOT_STARTED:
155 : case Status::WAITING_TO_START:
156 : case Status::RUNNING_STEP_ONE:
157 : case Status::RUNNING_STEP_TWO:
158 580560 : return false;
159 : }
160 :
161 : // compiler guarantees we don't get here
162 0 : return true;
163 595912 : }
164 :
165 :
166 595921 : bool CompassCalibrator::running() {
167 595921 : WITH_SEMAPHORE(state_sem);
168 595921 : return (cal_state.status == Status::RUNNING_STEP_ONE || cal_state.status == Status::RUNNING_STEP_TWO);
169 595921 : }
170 :
171 51989 : const CompassCalibrator::Report CompassCalibrator::get_report() {
172 51989 : WITH_SEMAPHORE(state_sem);
173 103978 : return cal_report;
174 51989 : }
175 :
176 2244436 : const CompassCalibrator::State CompassCalibrator::get_state() {
177 2244436 : WITH_SEMAPHORE(state_sem);
178 4488872 : return cal_state;
179 2244436 : }
180 : /////////////////////////////////////////////////////////////
181 : ////////////////////// PRIVATE METHODS //////////////////////
182 : /////////////////////////////////////////////////////////////
183 :
184 1983436 : void CompassCalibrator::update()
185 : {
186 :
187 : //pickup samples from intermediate struct
188 1983436 : pull_sample();
189 :
190 : {
191 1983436 : WITH_SEMAPHORE(state_sem);
192 : //update_settings
193 1983436 : if (!_running() && !_retry_pending) {
194 788749 : update_cal_settings();
195 : }
196 :
197 : //update requested state
198 1983436 : if (_status_set_requested) {
199 48 : _status_set_requested = false;
200 48 : set_status(_requested_status);
201 : }
202 : //update report and status
203 1983436 : update_cal_status();
204 1983436 : update_cal_report();
205 :
206 : // Hold the FAILED_* status for COMPASS_CAL_RETRY_REPORT_HOLD_MS before
207 : // starting the next attempt, so a GCS polling MAG_CAL_REPORT can display
208 : // the specific failure reason before the retry overwrites it.
209 1983436 : if (_retry_pending &&
210 0 : AP_HAL::millis() - _retry_hold_start_ms >= COMPASS_CAL_RETRY_REPORT_HOLD_MS) {
211 0 : _retry_pending = false;
212 0 : if (set_status(Status::WAITING_TO_START)) {
213 0 : _attempt++;
214 : }
215 : }
216 1983436 : }
217 :
218 : // collect the minimum number of samples
219 1983436 : if (!_fitting()) {
220 1982449 : return;
221 : }
222 :
223 987 : if (_status == Status::RUNNING_STEP_ONE) {
224 231 : if (_fit_step >= 10) {
225 21 : if (is_equal(_fitness, _initial_fitness) || isnan(_fitness)) { // if true, means that fitness is diverging instead of converging
226 0 : set_status(Status::FAILED);
227 : } else {
228 21 : set_status(Status::RUNNING_STEP_TWO);
229 : }
230 : } else {
231 210 : if (_fit_step == 0) {
232 21 : calc_initial_offset();
233 : }
234 210 : run_sphere_fit();
235 210 : _fit_step++;
236 : }
237 756 : } else if (_status == Status::RUNNING_STEP_TWO) {
238 756 : if (_fit_step >= 35) {
239 21 : const Status fit_status = fit_acceptable_status();
240 21 : if (fit_status != Status::SUCCESS) {
241 : // specific reason already identified, set it directly
242 10 : set_status(fit_status);
243 11 : } else if (fix_radius() && calculate_orientation()) {
244 11 : set_status(Status::SUCCESS);
245 : } else {
246 : // fix_radius()/calculate_orientation() set their own specific
247 : // FAILED_* reason; set generic FAILED only as last resort if
248 : // status is somehow still not set to a terminal state.
249 0 : set_status(Status::FAILED);
250 : }
251 735 : } else if (_fit_step < 15) {
252 315 : run_sphere_fit();
253 315 : _fit_step++;
254 : } else {
255 420 : run_ellipsoid_fit();
256 420 : _fit_step++;
257 : }
258 : }
259 : }
260 :
261 1983436 : void CompassCalibrator::pull_sample()
262 : {
263 : CompassSample mag_sample;
264 : {
265 1983436 : WITH_SEMAPHORE(sample_sem);
266 1983436 : if (!_new_sample) {
267 1387809 : return;
268 : }
269 595627 : if (_status == Status::WAITING_TO_START) {
270 0 : set_status(Status::RUNNING_STEP_ONE);
271 : }
272 595627 : _new_sample = false;
273 595627 : mag_sample = _last_sample;
274 1983436 : }
275 595627 : if (_running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(mag_sample.get())) {
276 12010 : update_completion_mask(mag_sample.get());
277 12010 : _sample_buffer[_samples_collected] = mag_sample;
278 12010 : _samples_collected++;
279 : }
280 : }
281 :
282 :
283 788749 : void CompassCalibrator::update_cal_settings()
284 : {
285 788749 : _tolerance = cal_settings.tolerance;
286 788749 : _check_orientation = cal_settings.check_orientation;
287 788749 : _orientation = cal_settings.orientation;
288 788749 : _orig_orientation = cal_settings.orig_orientation;
289 788749 : _is_external = cal_settings.is_external;
290 788749 : _fix_orientation = cal_settings.fix_orientation;
291 788749 : _offset_max = cal_settings.offset_max;
292 788749 : _attempt = cal_settings.attempt;
293 788749 : _retry = cal_settings.retry;
294 788749 : _delay_start_sec = cal_settings.delay_start_sec;
295 788749 : _start_time_ms = cal_settings.start_time_ms;
296 788749 : _compass_idx = cal_settings.compass_idx;
297 788749 : _always_45_deg = cal_settings.always_45_deg;
298 788749 : }
299 :
300 : // update completion mask based on latest sample
301 : // used to ensure we have collected samples in all directions
302 79074 : void CompassCalibrator::update_completion_mask(const Vector3f& v)
303 : {
304 : Matrix3f softiron {
305 : _params.diag.x, _params.offdiag.x, _params.offdiag.y,
306 : _params.offdiag.x, _params.diag.y, _params.offdiag.z,
307 : _params.offdiag.y, _params.offdiag.z, _params.diag.z
308 79074 : };
309 79074 : Vector3f corrected = softiron * (v + _params.offset);
310 79074 : int section = AP_GeodesicGrid::section(corrected, true);
311 79074 : if (section < 0) {
312 0 : return;
313 : }
314 79074 : _completion_mask[section / 8] |= 1 << (section % 8);
315 : }
316 :
317 : // reset and update the completion mask using all samples in the sample buffer
318 234 : void CompassCalibrator::update_completion_mask()
319 : {
320 234 : memset(_completion_mask, 0, sizeof(_completion_mask));
321 67298 : for (int i = 0; i < _samples_collected; i++) {
322 67064 : update_completion_mask(_sample_buffer[i].get());
323 : }
324 234 : }
325 :
326 1983436 : void CompassCalibrator::update_cal_status()
327 : {
328 1983436 : cal_state.status = _status;
329 1983436 : cal_state.attempt = _attempt;
330 1983436 : memcpy(cal_state.completion_mask, _completion_mask, sizeof(completion_mask_t));
331 1983436 : cal_state.completion_pct = 0.0f;
332 : // first sampling step is 1/3rd of the progress bar
333 : // never return more than 99% unless _status is Status::SUCCESS
334 1983436 : switch (_status) {
335 680696 : case Status::NOT_STARTED:
336 : case Status::WAITING_TO_START:
337 680696 : cal_state.completion_pct = 0.0f;
338 680696 : break;
339 828224 : case Status::RUNNING_STEP_ONE:
340 828224 : cal_state.completion_pct = 33.3f * _samples_collected/COMPASS_CAL_NUM_SAMPLES;
341 828224 : break;
342 366484 : case Status::RUNNING_STEP_TWO:
343 366484 : cal_state.completion_pct = 33.3f + 65.7f*((float)(_samples_collected-_samples_thinned)/(COMPASS_CAL_NUM_SAMPLES-_samples_thinned));
344 366484 : break;
345 56343 : case Status::SUCCESS:
346 56343 : cal_state.completion_pct = 100.0f;
347 56343 : break;
348 51689 : case Status::FAILED:
349 : case Status::FAILED_ORIENTATION:
350 : case Status::FAILED_RADIUS:
351 : case Status::FAILED_OFFSETS:
352 : case Status::FAILED_DIAG_SCALING:
353 : case Status::FAILED_RESIDUALS_HIGH:
354 51689 : cal_state.completion_pct = 0.0f;
355 51689 : break;
356 : };
357 1983436 : }
358 :
359 :
360 1983436 : void CompassCalibrator::update_cal_report()
361 : {
362 1983436 : cal_report.status = _status;
363 1983436 : cal_report.fitness = sqrtf(_fitness);
364 1983436 : cal_report.ofs = _params.offset;
365 1983436 : cal_report.diag = _params.diag;
366 1983436 : cal_report.offdiag = _params.offdiag;
367 1983436 : cal_report.scale_factor = _params.scale_factor;
368 1983436 : cal_report.orientation_confidence = _orientation_confidence;
369 1983436 : cal_report.original_orientation = _orig_orientation;
370 1983436 : cal_report.orientation = _orientation_solution;
371 1983436 : cal_report.check_orientation = _check_orientation;
372 1983436 : }
373 :
374 : // running method for use in thread
375 4562556 : bool CompassCalibrator::_running() const
376 : {
377 4562556 : return _status == Status::RUNNING_STEP_ONE || _status == Status::RUNNING_STEP_TWO;
378 : }
379 :
380 : // fitting method for use in thread
381 1983436 : bool CompassCalibrator::_fitting() const
382 : {
383 1983436 : return _running() && (_samples_collected == COMPASS_CAL_NUM_SAMPLES);
384 : }
385 :
386 : // initialize fitness before starting a fit
387 108 : void CompassCalibrator::initialize_fit()
388 : {
389 108 : if (_samples_collected != 0) {
390 21 : _fitness = calc_mean_squared_residuals(_params);
391 : } else {
392 87 : _fitness = 1.0e30f;
393 : }
394 108 : _initial_fitness = _fitness;
395 108 : _sphere_lambda = 1.0f;
396 108 : _ellipsoid_lambda = 1.0f;
397 108 : _fit_step = 0;
398 108 : }
399 :
400 57 : void CompassCalibrator::reset_state()
401 : {
402 57 : _samples_collected = 0;
403 57 : _samples_thinned = 0;
404 57 : _params.radius = 200;
405 57 : _params.offset.zero();
406 57 : _params.diag = Vector3f(1.0f,1.0f,1.0f);
407 57 : _params.offdiag.zero();
408 57 : _params.scale_factor = 0;
409 57 : _retry_pending = false;
410 :
411 57 : memset(_completion_mask, 0, sizeof(_completion_mask));
412 57 : initialize_fit();
413 57 : }
414 :
415 129 : bool CompassCalibrator::set_status(CompassCalibrator::Status status)
416 : {
417 129 : if (status != Status::NOT_STARTED && _status == status) {
418 0 : return true;
419 : }
420 :
421 129 : switch (status) {
422 27 : case Status::NOT_STARTED:
423 27 : reset_state();
424 27 : _status = Status::NOT_STARTED;
425 27 : if (_sample_buffer != nullptr) {
426 9 : free(_sample_buffer);
427 9 : _sample_buffer = nullptr;
428 : }
429 27 : return true;
430 :
431 30 : case Status::WAITING_TO_START:
432 30 : reset_state();
433 30 : _status = Status::WAITING_TO_START;
434 30 : set_status(Status::RUNNING_STEP_ONE);
435 30 : return true;
436 :
437 30 : case Status::RUNNING_STEP_ONE:
438 30 : if (_status != Status::WAITING_TO_START) {
439 0 : return false;
440 : }
441 :
442 : // on first attempt delay start if requested by caller
443 30 : if (_attempt == 1 && (AP_HAL::millis()-_start_time_ms)*1.0e-3f < _delay_start_sec) {
444 0 : return false;
445 : }
446 :
447 30 : if (_sample_buffer == nullptr) {
448 30 : _sample_buffer = (CompassSample*)calloc(COMPASS_CAL_NUM_SAMPLES, sizeof(CompassSample));
449 : }
450 30 : if (_sample_buffer != nullptr) {
451 30 : initialize_fit();
452 30 : _status = Status::RUNNING_STEP_ONE;
453 30 : return true;
454 : }
455 0 : return false;
456 :
457 21 : case Status::RUNNING_STEP_TWO:
458 21 : if (_status != Status::RUNNING_STEP_ONE) {
459 0 : return false;
460 : }
461 21 : thin_samples();
462 21 : initialize_fit();
463 21 : _status = Status::RUNNING_STEP_TWO;
464 21 : return true;
465 :
466 11 : case Status::SUCCESS:
467 11 : if (_status != Status::RUNNING_STEP_TWO) {
468 0 : return false;
469 : }
470 :
471 11 : if (_sample_buffer != nullptr) {
472 11 : free(_sample_buffer);
473 11 : _sample_buffer = nullptr;
474 : }
475 :
476 11 : _status = Status::SUCCESS;
477 11 : _retry_pending = false;
478 11 : return true;
479 :
480 0 : case Status::FAILED:
481 0 : if (_status == Status::FAILED_ORIENTATION ||
482 0 : _status == Status::FAILED_RADIUS ||
483 0 : _status == Status::FAILED_OFFSETS ||
484 0 : _status == Status::FAILED_DIAG_SCALING ||
485 0 : _status == Status::FAILED_RESIDUALS_HIGH) {
486 : // don't overwrite specific failure status
487 0 : return false;
488 : }
489 : FALLTHROUGH;
490 :
491 : case Status::FAILED_ORIENTATION:
492 : case Status::FAILED_RADIUS:
493 : case Status::FAILED_OFFSETS:
494 : case Status::FAILED_DIAG_SCALING:
495 : case Status::FAILED_RESIDUALS_HIGH:
496 10 : if (_status == Status::NOT_STARTED) {
497 0 : return false;
498 : }
499 :
500 10 : _status = status;
501 :
502 10 : if (_retry) {
503 0 : _retry_pending = true;
504 0 : _retry_hold_start_ms = AP_HAL::millis();
505 0 : return true;
506 : }
507 :
508 10 : if (_sample_buffer != nullptr) {
509 10 : free(_sample_buffer);
510 10 : _sample_buffer = nullptr;
511 : }
512 :
513 10 : return true;
514 : };
515 :
516 : // compiler guarantees we don't get here
517 0 : return false;
518 : }
519 :
520 21 : CompassCalibrator::Status CompassCalibrator::fit_acceptable_status() const
521 : {
522 21 : if (isnan(_fitness)) {
523 0 : return Status::FAILED_RESIDUALS_HIGH;
524 : }
525 21 : if (_params.radius <= FIELD_RADIUS_MIN || _params.radius >= FIELD_RADIUS_MAX) {
526 0 : return Status::FAILED_RADIUS;
527 : }
528 21 : if (fabsf(_params.offset.x) >= _offset_max ||
529 20 : fabsf(_params.offset.y) >= _offset_max ||
530 20 : fabsf(_params.offset.z) >= _offset_max) {
531 1 : return Status::FAILED_OFFSETS;
532 : }
533 20 : if (_params.diag.x <= 0.2f || _params.diag.x >= 5.0f ||
534 20 : _params.diag.y <= 0.2f || _params.diag.y >= 5.0f ||
535 20 : _params.diag.z <= 0.2f || _params.diag.z >= 5.0f ||
536 20 : fabsf(_params.offdiag.x) >= 1.0f ||
537 20 : fabsf(_params.offdiag.y) >= 1.0f ||
538 20 : fabsf(_params.offdiag.z) >= 1.0f) {
539 0 : return Status::FAILED_DIAG_SCALING;
540 : }
541 20 : if (_fitness > sq(_tolerance)) {
542 9 : return Status::FAILED_RESIDUALS_HIGH;
543 : }
544 11 : return Status::SUCCESS;
545 : }
546 :
547 21 : void CompassCalibrator::thin_samples()
548 : {
549 21 : if (_sample_buffer == nullptr) {
550 0 : return;
551 : }
552 :
553 21 : _samples_thinned = 0;
554 : // shuffle the samples http://en.wikipedia.org/wiki/Fisher%E2%80%93Yates_shuffle
555 : // this is so that adjacent samples don't get sequentially eliminated
556 6300 : for (uint16_t i=_samples_collected-1; i>=1; i--) {
557 6279 : uint16_t j = get_random16() % (i+1);
558 6279 : CompassSample temp = _sample_buffer[i];
559 6279 : _sample_buffer[i] = _sample_buffer[j];
560 6279 : _sample_buffer[j] = temp;
561 : }
562 :
563 : // remove any samples that are close together
564 3190 : for (uint16_t i=0; i < _samples_collected; i++) {
565 3169 : if (!accept_sample(_sample_buffer[i], i)) {
566 3136 : _sample_buffer[i] = _sample_buffer[_samples_collected-1];
567 3136 : _samples_collected--;
568 3136 : _samples_thinned++;
569 : }
570 : }
571 :
572 21 : update_completion_mask();
573 : }
574 :
575 : /*
576 : * The sample acceptance distance is determined as follows:
577 : * For any regular polyhedron with triangular faces, the angle theta subtended
578 : * by two closest points is defined as
579 : *
580 : * theta = arccos(cos(A)/(1-cos(A)))
581 : *
582 : * Where:
583 : * A = (4pi/F + pi)/3
584 : * and
585 : * F = 2V - 4 is the number of faces for the polyhedron in consideration,
586 : * which depends on the number of vertices V
587 : *
588 : * The above equation was proved after solving for spherical triangular excess
589 : * and related equations.
590 : */
591 364757 : bool CompassCalibrator::accept_sample(const Vector3f& sample, uint16_t skip_index)
592 : {
593 : static const uint16_t faces = (2 * COMPASS_CAL_NUM_SAMPLES - 4);
594 : static const float a = (4.0f * M_PI / (3.0f * faces)) + M_PI / 3.0f;
595 : static const float theta = 0.5f * acosf(cosf(a) / (1.0f - cosf(a)));
596 :
597 364757 : if (_sample_buffer == nullptr) {
598 0 : return false;
599 : }
600 :
601 364757 : float min_distance = _params.radius * 2*sinf(theta/2);
602 :
603 29856072 : for (uint16_t i = 0; i<_samples_collected; i++) {
604 29844029 : if (i != skip_index) {
605 29842843 : float distance = (sample - _sample_buffer[i].get()).length();
606 29842843 : if (distance < min_distance) {
607 352714 : return false;
608 : }
609 : }
610 : }
611 12043 : return true;
612 : }
613 :
614 3169 : bool CompassCalibrator::accept_sample(const CompassSample& sample, uint16_t skip_index)
615 : {
616 3169 : return accept_sample(sample.get(), skip_index);
617 : }
618 :
619 2295164 : float CompassCalibrator::calc_residual(const Vector3f& sample, const param_t& params) const
620 : {
621 : Matrix3f softiron(
622 2295164 : params.diag.x , params.offdiag.x , params.offdiag.y,
623 2295164 : params.offdiag.x , params.diag.y , params.offdiag.z,
624 2295164 : params.offdiag.y , params.offdiag.z , params.diag.z
625 2295164 : );
626 2295164 : return params.radius - (softiron*(sample+params.offset)).length();
627 : }
628 :
629 : // calc the fitness given a set of parameters (offsets, diagonals, off diagonals)
630 1781 : float CompassCalibrator::calc_mean_squared_residuals(const param_t& params) const
631 : {
632 1781 : if (_sample_buffer == nullptr || _samples_collected == 0) {
633 0 : return 1.0e30f;
634 : }
635 1781 : float sum = 0.0f;
636 532945 : for (uint16_t i=0; i < _samples_collected; i++) {
637 531164 : Vector3f sample = _sample_buffer[i].get();
638 531164 : float resid = calc_residual(sample, params);
639 531164 : sum += sq(resid);
640 : }
641 1781 : sum /= _samples_collected;
642 1781 : return sum;
643 : }
644 :
645 : // calculate initial offsets by simply taking the average values of the samples
646 21 : void CompassCalibrator::calc_initial_offset()
647 : {
648 : // Set initial offset to the average value of the samples
649 21 : _params.offset.zero();
650 6321 : for (uint16_t k = 0; k < _samples_collected; k++) {
651 6300 : _params.offset -= _sample_buffer[k].get();
652 : }
653 21 : _params.offset /= _samples_collected;
654 21 : }
655 :
656 157500 : void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const
657 : {
658 157500 : const Vector3f &offset = params.offset;
659 157500 : const Vector3f &diag = params.diag;
660 157500 : const Vector3f &offdiag = params.offdiag;
661 : Matrix3f softiron(
662 157500 : diag.x , offdiag.x , offdiag.y,
663 157500 : offdiag.x , diag.y , offdiag.z,
664 157500 : offdiag.y , offdiag.z , diag.z
665 157500 : );
666 :
667 157500 : float A = (diag.x * (sample.x + offset.x)) + (offdiag.x * (sample.y + offset.y)) + (offdiag.y * (sample.z + offset.z));
668 157500 : float B = (offdiag.x * (sample.x + offset.x)) + (diag.y * (sample.y + offset.y)) + (offdiag.z * (sample.z + offset.z));
669 157500 : float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z));
670 157500 : float length = (softiron*(sample+offset)).length();
671 :
672 : // 0: partial derivative (radius wrt fitness fn) fn operated on sample
673 157500 : ret[0] = 1.0f;
674 : // 1-3: partial derivative (offsets wrt fitness fn) fn operated on sample
675 157500 : ret[1] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length);
676 157500 : ret[2] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length);
677 157500 : ret[3] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length);
678 157500 : }
679 :
680 : // run sphere fit to calculate diagonals and offdiagonals
681 525 : void CompassCalibrator::run_sphere_fit()
682 : {
683 525 : if (_sample_buffer == nullptr) {
684 65 : return;
685 : }
686 :
687 525 : const float lma_damping = 10.0f;
688 :
689 : // take backup of fitness and parameters so we can determine later if this fit has improved the calibration
690 525 : float fitness = _fitness;
691 : float fit1, fit2;
692 525 : param_t fit1_params, fit2_params;
693 525 : fit1_params = fit2_params = _params;
694 :
695 525 : float JTJ[COMPASS_CAL_NUM_SPHERE_PARAMS*COMPASS_CAL_NUM_SPHERE_PARAMS] = { };
696 525 : float JTJ2[COMPASS_CAL_NUM_SPHERE_PARAMS*COMPASS_CAL_NUM_SPHERE_PARAMS] = { };
697 525 : float JTFI[COMPASS_CAL_NUM_SPHERE_PARAMS] = { };
698 :
699 : // Gauss Newton Part common for all kind of extensions including LM
700 158025 : for (uint16_t k = 0; k<_samples_collected; k++) {
701 157500 : Vector3f sample = _sample_buffer[k].get();
702 :
703 : float sphere_jacob[COMPASS_CAL_NUM_SPHERE_PARAMS];
704 :
705 157500 : calc_sphere_jacob(sample, fit1_params, sphere_jacob);
706 :
707 787500 : for (uint8_t i = 0;i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) {
708 : // compute JTJ
709 3150000 : for (uint8_t j = 0; j < COMPASS_CAL_NUM_SPHERE_PARAMS; j++) {
710 2520000 : JTJ[i*COMPASS_CAL_NUM_SPHERE_PARAMS+j] += sphere_jacob[i] * sphere_jacob[j];
711 2520000 : JTJ2[i*COMPASS_CAL_NUM_SPHERE_PARAMS+j] += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM
712 : }
713 : // compute JTFI
714 630000 : JTFI[i] += sphere_jacob[i] * calc_residual(sample, fit1_params);
715 : }
716 : }
717 :
718 : //------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
719 : // refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
720 2625 : for (uint8_t i = 0; i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) {
721 2100 : JTJ[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda;
722 2100 : JTJ2[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda/lma_damping;
723 : }
724 :
725 525 : if (!mat_inverse(JTJ, JTJ, 4)) {
726 65 : return;
727 : }
728 :
729 460 : if (!mat_inverse(JTJ2, JTJ2, 4)) {
730 0 : return;
731 : }
732 :
733 : // extract radius, offset, diagonals and offdiagonal parameters
734 2300 : for (uint8_t row=0; row < COMPASS_CAL_NUM_SPHERE_PARAMS; row++) {
735 9200 : for (uint8_t col=0; col < COMPASS_CAL_NUM_SPHERE_PARAMS; col++) {
736 7360 : fit1_params.get_sphere_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_SPHERE_PARAMS+col];
737 7360 : fit2_params.get_sphere_params()[row] -= JTFI[col] * JTJ2[row*COMPASS_CAL_NUM_SPHERE_PARAMS+col];
738 : }
739 : }
740 :
741 : // calculate fitness of two possible sets of parameters
742 460 : fit1 = calc_mean_squared_residuals(fit1_params);
743 460 : fit2 = calc_mean_squared_residuals(fit2_params);
744 :
745 : // decide which of the two sets of parameters is best and store in fit1_params
746 460 : if (fit1 > _fitness && fit2 > _fitness) {
747 : // if neither set of parameters provided better results, increase lambda
748 250 : _sphere_lambda *= lma_damping;
749 210 : } else if (fit2 < _fitness && fit2 < fit1) {
750 : // if fit2 was better we will use it. decrease lambda
751 55 : _sphere_lambda /= lma_damping;
752 55 : fit1_params = fit2_params;
753 55 : fitness = fit2;
754 155 : } else if (fit1 < _fitness) {
755 58 : fitness = fit1;
756 : }
757 : //--------------------Levenberg-Marquardt-part-ends-here--------------------------------//
758 :
759 : // store new parameters and update fitness
760 460 : if (!isnan(fitness) && fitness < _fitness) {
761 113 : _fitness = fitness;
762 113 : _params = fit1_params;
763 113 : update_completion_mask();
764 : }
765 : }
766 :
767 126000 : void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const
768 : {
769 126000 : const Vector3f &offset = params.offset;
770 126000 : const Vector3f &diag = params.diag;
771 126000 : const Vector3f &offdiag = params.offdiag;
772 : Matrix3f softiron(
773 126000 : diag.x , offdiag.x , offdiag.y,
774 126000 : offdiag.x , diag.y , offdiag.z,
775 126000 : offdiag.y , offdiag.z , diag.z
776 126000 : );
777 :
778 126000 : float A = (diag.x * (sample.x + offset.x)) + (offdiag.x * (sample.y + offset.y)) + (offdiag.y * (sample.z + offset.z));
779 126000 : float B = (offdiag.x * (sample.x + offset.x)) + (diag.y * (sample.y + offset.y)) + (offdiag.z * (sample.z + offset.z));
780 126000 : float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z));
781 126000 : float length = (softiron*(sample+offset)).length();
782 :
783 : // 0-2: partial derivative (offset wrt fitness fn) fn operated on sample
784 126000 : ret[0] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length);
785 126000 : ret[1] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length);
786 126000 : ret[2] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length);
787 : // 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample
788 126000 : ret[3] = -1.0f * ((sample.x + offset.x) * A)/length;
789 126000 : ret[4] = -1.0f * ((sample.y + offset.y) * B)/length;
790 126000 : ret[5] = -1.0f * ((sample.z + offset.z) * C)/length;
791 : // 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample
792 126000 : ret[6] = -1.0f * (((sample.y + offset.y) * A) + ((sample.x + offset.x) * B))/length;
793 126000 : ret[7] = -1.0f * (((sample.z + offset.z) * A) + ((sample.x + offset.x) * C))/length;
794 126000 : ret[8] = -1.0f * (((sample.z + offset.z) * B) + ((sample.y + offset.y) * C))/length;
795 126000 : }
796 :
797 420 : void CompassCalibrator::run_ellipsoid_fit()
798 : {
799 420 : if (_sample_buffer == nullptr) {
800 0 : return;
801 : }
802 :
803 420 : const float lma_damping = 10.0f;
804 :
805 : // take backup of fitness and parameters so we can determine later if this fit has improved the calibration
806 420 : float fitness = _fitness;
807 : float fit1, fit2;
808 420 : param_t fit1_params, fit2_params;
809 420 : fit1_params = fit2_params = _params;
810 :
811 420 : float JTJ[COMPASS_CAL_NUM_ELLIPSOID_PARAMS*COMPASS_CAL_NUM_ELLIPSOID_PARAMS] = { };
812 420 : float JTJ2[COMPASS_CAL_NUM_ELLIPSOID_PARAMS*COMPASS_CAL_NUM_ELLIPSOID_PARAMS] = { };
813 420 : float JTFI[COMPASS_CAL_NUM_ELLIPSOID_PARAMS] = { };
814 :
815 : // Gauss Newton Part common for all kind of extensions including LM
816 126420 : for (uint16_t k = 0; k<_samples_collected; k++) {
817 126000 : Vector3f sample = _sample_buffer[k].get();
818 :
819 : float ellipsoid_jacob[COMPASS_CAL_NUM_ELLIPSOID_PARAMS];
820 :
821 126000 : calc_ellipsoid_jacob(sample, fit1_params, ellipsoid_jacob);
822 :
823 1260000 : for (uint8_t i = 0;i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++) {
824 : // compute JTJ
825 11340000 : for (uint8_t j = 0; j < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; j++) {
826 10206000 : JTJ [i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+j] += ellipsoid_jacob[i] * ellipsoid_jacob[j];
827 10206000 : JTJ2[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+j] += ellipsoid_jacob[i] * ellipsoid_jacob[j];
828 : }
829 : // compute JTFI
830 1134000 : JTFI[i] += ellipsoid_jacob[i] * calc_residual(sample, fit1_params);
831 : }
832 : }
833 :
834 : //------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
835 : //refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
836 4200 : for (uint8_t i = 0; i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++) {
837 3780 : JTJ[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+i] += _ellipsoid_lambda;
838 3780 : JTJ2[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+i] += _ellipsoid_lambda/lma_damping;
839 : }
840 :
841 420 : if (!mat_inverse(JTJ, JTJ, 9)) {
842 0 : return;
843 : }
844 :
845 420 : if (!mat_inverse(JTJ2, JTJ2, 9)) {
846 0 : return;
847 : }
848 :
849 : // extract radius, offset, diagonals and offdiagonal parameters
850 4200 : for (uint8_t row=0; row < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; row++) {
851 37800 : for (uint8_t col=0; col < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; col++) {
852 34020 : fit1_params.get_ellipsoid_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col];
853 34020 : fit2_params.get_ellipsoid_params()[row] -= JTFI[col] * JTJ2[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col];
854 : }
855 : }
856 :
857 : // calculate fitness of two possible sets of parameters
858 420 : fit1 = calc_mean_squared_residuals(fit1_params);
859 420 : fit2 = calc_mean_squared_residuals(fit2_params);
860 :
861 : // decide which of the two sets of parameters is best and store in fit1_params
862 420 : if (fit1 > _fitness && fit2 > _fitness) {
863 : // if neither set of parameters provided better results, increase lambda
864 225 : _ellipsoid_lambda *= lma_damping;
865 195 : } else if (fit2 < _fitness && fit2 < fit1) {
866 : // if fit2 was better we will use it. decrease lambda
867 30 : _ellipsoid_lambda /= lma_damping;
868 30 : fit1_params = fit2_params;
869 30 : fitness = fit2;
870 165 : } else if (fit1 < _fitness) {
871 70 : fitness = fit1;
872 : }
873 : //--------------------Levenberg-part-ends-here--------------------------------//
874 :
875 : // store new parameters and update fitness
876 420 : if (fitness < _fitness) {
877 100 : _fitness = fitness;
878 100 : _params = fit1_params;
879 100 : update_completion_mask();
880 : }
881 : }
882 :
883 :
884 : //////////////////////////////////////////////////////////
885 : //////////// CompassSample public interface //////////////
886 : //////////////////////////////////////////////////////////
887 :
888 : #define COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(__X) ((int16_t)constrain_float(roundf(__X*8.0f), INT16_MIN, INT16_MAX))
889 : #define COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(__X) (__X/8.0f)
890 :
891 31107638 : Vector3f CompassCalibrator::CompassSample::get() const
892 : {
893 62215276 : return Vector3f(COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(x),
894 31107638 : COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(y),
895 31107638 : COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(z));
896 : }
897 :
898 596089 : void CompassCalibrator::CompassSample::set(const Vector3f &in)
899 : {
900 596089 : x = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.x);
901 596089 : y = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.y);
902 596089 : z = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.z);
903 596089 : }
904 :
905 596089 : void CompassCalibrator::AttitudeSample::set_from_ahrs(void)
906 : {
907 596089 : const Matrix3f &dcm = AP::ahrs().get_DCM_rotation_body_to_ned();
908 : float roll_rad, pitch_rad, yaw_rad;
909 596089 : dcm.to_euler(&roll_rad, &pitch_rad, &yaw_rad);
910 596089 : roll = constrain_int16(127 * (roll_rad / M_PI), -INT8_MAX, INT8_MAX);
911 596089 : pitch = constrain_int16(127 * (pitch_rad / M_PI_2), -INT8_MAX, INT8_MAX);
912 596089 : yaw = constrain_int16(127 * (yaw_rad / M_PI), -INT8_MAX, INT8_MAX);
913 596089 : }
914 :
915 0 : Matrix3f CompassCalibrator::AttitudeSample::get_rotmat(void) const
916 : {
917 : float roll_rad, pitch_rad, yaw_rad;
918 0 : roll_rad = roll * (M_PI / 127);
919 0 : pitch_rad = pitch * (M_PI_2 / 127);
920 0 : yaw_rad = yaw * (M_PI / 127);
921 0 : Matrix3f dcm;
922 0 : dcm.from_euler(roll_rad, pitch_rad, yaw_rad);
923 0 : return dcm;
924 : }
925 :
926 : /*
927 : calculate the implied earth field for a compass sample and compass
928 : rotation. This is used to check for consistency between
929 : samples.
930 :
931 : If the orientation is correct then when rotated the same (or
932 : similar) earth field should be given for all samples.
933 :
934 : Note that this earth field uses an arbitrary north reference, so it
935 : may not match the true earth field.
936 : */
937 0 : Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Rotation r)
938 : {
939 0 : Vector3f v = sample.get();
940 :
941 : // convert the sample back to sensor frame
942 0 : v.rotate_inverse(_orientation);
943 :
944 : // rotate to body frame for this rotation
945 0 : v.rotate(r);
946 :
947 : // apply offsets, rotating them for the orientation we are testing
948 0 : Vector3f rot_offsets = _params.offset;
949 0 : rot_offsets.rotate_inverse(_orientation);
950 :
951 0 : rot_offsets.rotate(r);
952 :
953 0 : v += rot_offsets;
954 :
955 : // rotate the sample from body frame back to earth frame
956 0 : Matrix3f rot = sample.att.get_rotmat();
957 :
958 0 : Vector3f efield = rot * v;
959 :
960 : // earth field is the mag sample in earth frame
961 0 : return efield;
962 : }
963 :
964 : /*
965 : calculate compass orientation using the attitude estimate associated
966 : with each sample, and fix orientation on external compasses if
967 : the feature is enabled
968 : */
969 11 : bool CompassCalibrator::calculate_orientation(void)
970 : {
971 11 : if (!_check_orientation) {
972 : // we are not checking orientation
973 11 : return true;
974 : }
975 :
976 : // this function is very slow
977 0 : EXPECT_DELAY_MS(1000);
978 :
979 : // Skip 4 rotations, see: auto_rotation_index()
980 0 : float variance[ROTATION_MAX-4] {};
981 :
982 0 : _orientation_solution = _orientation;
983 :
984 0 : for (uint8_t n=0; n < ARRAY_SIZE(variance); n++) {
985 0 : Rotation r = auto_rotation_index(n);
986 :
987 : // calculate the average implied earth field across all samples
988 0 : Vector3f total_ef {};
989 0 : for (uint32_t i=0; i<_samples_collected; i++) {
990 0 : Vector3f efield = calculate_earth_field(_sample_buffer[i], r);
991 0 : total_ef += efield;
992 : }
993 0 : Vector3f avg_efield = total_ef / _samples_collected;
994 :
995 : // now calculate the square error for this rotation against the average earth field
996 0 : for (uint32_t i=0; i<_samples_collected; i++) {
997 0 : Vector3f efield = calculate_earth_field(_sample_buffer[i], r);
998 0 : float err = (efield - avg_efield).length_squared();
999 : // divide by number of samples collected to get the variance
1000 0 : variance[n] += err / _samples_collected;
1001 : }
1002 : }
1003 :
1004 : // find the rotation with the lowest variance
1005 0 : enum Rotation besti = ROTATION_NONE;
1006 0 : float bestv = variance[0];
1007 0 : enum Rotation besti_90 = ROTATION_NONE;
1008 0 : float bestv_90 = variance[0];
1009 0 : for (uint8_t n=0; n < ARRAY_SIZE(variance); n++) {
1010 0 : Rotation r = auto_rotation_index(n);
1011 0 : if (variance[n] < bestv) {
1012 0 : bestv = variance[n];
1013 0 : besti = r;
1014 : }
1015 0 : if (right_angle_rotation(r) && variance[n] < bestv_90) {
1016 0 : bestv_90 = variance[n];
1017 0 : besti_90 = r;
1018 : }
1019 : }
1020 :
1021 :
1022 0 : float second_best = besti==ROTATION_NONE?variance[1]:variance[0];
1023 0 : enum Rotation besti2 = besti==ROTATION_NONE?ROTATION_YAW_45:ROTATION_NONE;
1024 0 : float second_best_90 = besti_90==ROTATION_NONE?variance[2]:variance[0];
1025 0 : enum Rotation besti2_90 = besti_90==ROTATION_NONE?ROTATION_YAW_90:ROTATION_NONE;
1026 0 : for (uint8_t n=0; n < ARRAY_SIZE(variance); n++) {
1027 0 : Rotation r = auto_rotation_index(n);
1028 0 : if (besti != r) {
1029 0 : if (variance[n] < second_best) {
1030 0 : second_best = variance[n];
1031 0 : besti2 = r;
1032 : }
1033 : }
1034 0 : if (right_angle_rotation(r) && (besti_90 != r) && (variance[n] < second_best_90)) {
1035 0 : second_best_90 = variance[n];
1036 0 : besti2_90 = r;
1037 : }
1038 : }
1039 :
1040 0 : _orientation_confidence = second_best/bestv;
1041 :
1042 : bool pass;
1043 0 : if (besti == _orientation) {
1044 : // if the orientation matched then allow for a low threshold
1045 0 : pass = true;
1046 : } else {
1047 0 : if (_orientation_confidence > 4.0) {
1048 : // very confident, always pass
1049 0 : pass = true;
1050 :
1051 0 : } else if (_always_45_deg && (_orientation_confidence > 2.0)) {
1052 : // use same tolerance for all rotations
1053 0 : pass = true;
1054 :
1055 : } else {
1056 : // just consider 90's
1057 0 : _orientation_confidence = second_best_90 / bestv_90;
1058 0 : pass = _orientation_confidence > 2.0;
1059 0 : besti = besti_90;
1060 0 : besti2 = besti2_90;
1061 : }
1062 : }
1063 0 : if (!pass) {
1064 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u/%u %.1f", _compass_idx,
1065 : besti, besti2, (double)_orientation_confidence);
1066 : (void)besti2;
1067 0 : } else if (besti == _orientation) {
1068 : // no orientation change
1069 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence);
1070 0 : } else if (!_is_external || !_fix_orientation) {
1071 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence);
1072 : } else {
1073 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f", _compass_idx, besti, _orientation, (double)_orientation_confidence);
1074 : }
1075 :
1076 0 : if (!pass) {
1077 0 : set_status(Status::FAILED_ORIENTATION);
1078 0 : return false;
1079 : }
1080 :
1081 0 : if (_orientation == besti) {
1082 : // no orientation change
1083 0 : return true;
1084 : }
1085 :
1086 0 : if (!_is_external || !_fix_orientation) {
1087 : // we won't change the orientation, but we set _orientation
1088 : // for reporting purposes
1089 0 : _orientation = besti;
1090 0 : _orientation_solution = besti;
1091 0 : set_status(Status::FAILED_ORIENTATION);
1092 0 : return false;
1093 : }
1094 :
1095 : // correct the offsets for the new orientation
1096 0 : Vector3f rot_offsets = _params.offset;
1097 0 : rot_offsets.rotate_inverse(_orientation);
1098 0 : rot_offsets.rotate(besti);
1099 0 : _params.offset = rot_offsets;
1100 :
1101 : // rotate the samples for the new orientation
1102 0 : for (uint32_t i=0; i<_samples_collected; i++) {
1103 0 : Vector3f s = _sample_buffer[i].get();
1104 0 : s.rotate_inverse(_orientation);
1105 0 : s.rotate(besti);
1106 0 : _sample_buffer[i].set(s);
1107 : }
1108 :
1109 0 : _orientation = besti;
1110 0 : _orientation_solution = besti;
1111 :
1112 : // re-run the fit to get the diagonals and off-diagonals for the
1113 : // new orientation
1114 0 : initialize_fit();
1115 0 : run_sphere_fit();
1116 0 : run_ellipsoid_fit();
1117 :
1118 0 : const Status s = fit_acceptable_status();
1119 0 : if (s != Status::SUCCESS) {
1120 0 : set_status(s);
1121 0 : return false;
1122 : }
1123 0 : return true;
1124 0 : }
1125 :
1126 : /*
1127 : fix radius of the fit to compensate for sensor scale factor errors
1128 : return false if radius is outside acceptable range
1129 : */
1130 11 : bool CompassCalibrator::fix_radius(void)
1131 : {
1132 11 : Location loc;
1133 11 : if (!AP::ahrs().get_location(loc) && !AP::ahrs().get_origin(loc)) {
1134 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "MagCal: No location, fix_radius skipped");
1135 : // we don't have a position, leave scale factor as 0. This
1136 : // will disable use of WMM in the EKF. Users can manually set
1137 : // scale factor after calibration if it is known
1138 0 : _params.scale_factor = 0;
1139 0 : return true;
1140 : }
1141 : float intensity;
1142 : float declination;
1143 : float inclination;
1144 11 : AP_Declination::get_mag_field_ef(loc.lat * 1e-7f, loc.lng * 1e-7f, intensity, declination, inclination);
1145 :
1146 11 : float expected_radius = intensity * 1000; // mGauss
1147 11 : float correction = expected_radius / _params.radius;
1148 :
1149 11 : if (correction > COMPASS_MAX_SCALE_FACTOR || correction < COMPASS_MIN_SCALE_FACTOR) {
1150 : // don't allow more than 30% scale factor correction
1151 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag(%u) bad radius %.0f expected %.0f",
1152 : _compass_idx,
1153 : _params.radius,
1154 : expected_radius);
1155 0 : set_status(Status::FAILED_RADIUS);
1156 0 : return false;
1157 : }
1158 :
1159 11 : _params.scale_factor = correction;
1160 :
1161 11 : return true;
1162 : }
1163 :
1164 : // convert index to rotation, this allows to skip some rotations
1165 0 : Rotation CompassCalibrator::auto_rotation_index(uint8_t n) const
1166 : {
1167 0 : if (n < ROTATION_PITCH_180_YAW_90) {
1168 0 : return (Rotation)n;
1169 : }
1170 : // ROTATION_PITCH_180_YAW_90 (26) is the same as ROTATION_ROLL_180_YAW_90 (10)
1171 : // ROTATION_PITCH_180_YAW_270 (27) is the same as ROTATION_ROLL_180_YAW_270 (14)
1172 0 : n += 2;
1173 0 : if (n < ROTATION_ROLL_90_PITCH_68_YAW_293) {
1174 0 : return (Rotation)n;
1175 : }
1176 : // ROTATION_ROLL_90_PITCH_68_YAW_293 is for solo leg compass, not expecting to see this in other vehicles
1177 0 : n += 1;
1178 0 : if (n < ROTATION_PITCH_7) {
1179 0 : return (Rotation)n;
1180 : }
1181 : // ROTATION_PITCH_7 is too close to ROTATION_NONE to tell the difference in compass cal
1182 0 : n += 1;
1183 0 : if (n < ROTATION_MAX) {
1184 0 : return (Rotation)n;
1185 : }
1186 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
1187 0 : return ROTATION_NONE;
1188 : };
1189 :
1190 0 : bool CompassCalibrator::right_angle_rotation(Rotation r) const
1191 : {
1192 0 : switch (r) {
1193 0 : case ROTATION_NONE:
1194 : case ROTATION_YAW_90:
1195 : case ROTATION_YAW_180:
1196 : case ROTATION_YAW_270:
1197 : case ROTATION_ROLL_180:
1198 : case ROTATION_ROLL_180_YAW_90:
1199 : case ROTATION_PITCH_180:
1200 : case ROTATION_ROLL_180_YAW_270:
1201 : case ROTATION_ROLL_90:
1202 : case ROTATION_ROLL_90_YAW_90:
1203 : case ROTATION_ROLL_270:
1204 : case ROTATION_ROLL_270_YAW_90:
1205 : case ROTATION_PITCH_90:
1206 : case ROTATION_PITCH_270:
1207 : case ROTATION_PITCH_180_YAW_90:
1208 : case ROTATION_PITCH_180_YAW_270:
1209 : case ROTATION_ROLL_90_PITCH_90:
1210 : case ROTATION_ROLL_180_PITCH_90:
1211 : case ROTATION_ROLL_270_PITCH_90:
1212 : case ROTATION_ROLL_90_PITCH_180:
1213 : case ROTATION_ROLL_270_PITCH_180:
1214 : case ROTATION_ROLL_90_PITCH_270:
1215 : case ROTATION_ROLL_180_PITCH_270:
1216 : case ROTATION_ROLL_270_PITCH_270:
1217 : case ROTATION_ROLL_90_PITCH_180_YAW_90:
1218 : case ROTATION_ROLL_90_YAW_270:
1219 0 : return true;
1220 :
1221 0 : default:
1222 0 : return false;
1223 : }
1224 : }
1225 :
1226 : #endif // COMPASS_CAL_ENABLED
|
