Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 : #include <AP_Notify/AP_Notify.h>
3 : #include <AP_GPS/AP_GPS.h>
4 : #include <GCS_MAVLink/GCS.h>
5 : #include <AP_AHRS/AP_AHRS.h>
6 : #include <AP_InternalError/AP_InternalError.h>
7 :
8 : #include "AP_Compass.h"
9 :
10 : const extern AP_HAL::HAL& hal;
11 :
12 : #if COMPASS_CAL_ENABLED
13 :
14 12757017 : void Compass::cal_update()
15 : {
16 12757017 : if (hal.util->get_soft_armed()) {
17 8554910 : return;
18 : }
19 :
20 4202107 : bool running = false;
21 4202107 : uint8_t num_compass = 0;
22 4202107 : uint8_t num_compass_successful = 0;
23 :
24 16808428 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
25 12606321 : if (_calibrator[i] == nullptr) {
26 12010409 : continue;
27 : }
28 595912 : num_compass++;
29 595912 : if (_calibrator[i]->failed()) {
30 15352 : AP_Notify::events.compass_cal_failed = 1;
31 : }
32 :
33 595912 : if (_calibrator[i]->running()) {
34 362807 : running = true;
35 362807 : continue;
36 : }
37 233105 : if (_calibrator[i]->get_state().status != CompassCalibrator::Status::SUCCESS) {
38 216469 : continue;
39 : }
40 16636 : num_compass_successful++;
41 16636 : if (!_cal_autosave) {
42 : // user requested to not automatically save the calibration values
43 16636 : continue;
44 : }
45 0 : if (_cal_saved[i]) {
46 : // user already manually applied changes via MAV_CMD_DO_ACCEPT_MAG_CAL
47 0 : continue;
48 : }
49 0 : _accept_calibration(uint8_t(i));
50 : }
51 :
52 4202107 : AP_Notify::flags.compass_cal_running = running;
53 :
54 4202107 : if (is_calibrating()) {
55 141322 : _cal_has_run = true;
56 141322 : return;
57 : }
58 4060785 : if (!_cal_has_run) {
59 : // calibration hasn't started yet
60 3958735 : return;
61 : }
62 102050 : if (num_compass != num_compass_successful) {
63 : // only reboot if all successful
64 95760 : return;
65 : }
66 6290 : if (!_compass_cal_autoreboot) {
67 : // user did not request autoreboot
68 6290 : return;
69 : }
70 0 : hal.scheduler->delay(1000);
71 0 : hal.scheduler->reboot();
72 : }
73 :
74 57 : bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
75 : {
76 57 : if (!healthy(i)) {
77 0 : return false;
78 : }
79 57 : if (!use_for_yaw(i)) {
80 0 : return false;
81 : }
82 57 : Priority prio = Priority(i);
83 :
84 : #if COMPASS_MAX_INSTANCES > 1
85 57 : if (_priority_did_list[prio] != _priority_did_stored_list[prio]) {
86 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Compass cal requires reboot after priority change");
87 0 : return false;
88 : }
89 : #endif
90 :
91 57 : if (_calibrator[prio] == nullptr) {
92 9 : _calibrator[prio] = NEW_NOTHROW CompassCalibrator();
93 9 : if (_calibrator[prio] == nullptr) {
94 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Compass cal object not initialised");
95 0 : return false;
96 : }
97 : }
98 :
99 57 : if (option_set(Option::CAL_REQUIRE_GPS)) {
100 0 : if (AP::gps().status() < AP_GPS_FixType::FIX_2D) {
101 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Compass cal requires GPS lock");
102 0 : return false;
103 : }
104 : }
105 57 : if (!is_calibrating()) {
106 25 : AP_Notify::events.initiated_compass_cal = 1;
107 : }
108 :
109 57 : if (_rotate_auto) {
110 0 : enum Rotation r = _get_state(prio).external?(enum Rotation)_get_state(prio).orientation.get():ROTATION_NONE;
111 0 : if (r < ROTATION_MAX) {
112 0 : _calibrator[prio]->set_orientation(r, _get_state(prio).external, _rotate_auto>=2, _rotate_auto>=3);
113 : }
114 : }
115 57 : _cal_saved[prio] = false;
116 57 : if (i == 0 && _get_state(prio).external != 0) {
117 25 : _calibrator[prio]->start(retry, delay, get_offsets_max(), i, _calibration_threshold);
118 : } else {
119 : // internal compasses or secondary compasses get twice the
120 : // threshold. This is because internal compasses tend to be a
121 : // lot noisier
122 32 : _calibrator[prio]->start(retry, delay, get_offsets_max(), i, _calibration_threshold*2);
123 : }
124 57 : if (!_cal_thread_started) {
125 4 : _cal_requires_reboot = true;
126 4 : if (!hal.scheduler->thread_create(FUNCTOR_BIND(this, &Compass::_update_calibration_trampoline, void), "compasscal", 2048, AP_HAL::Scheduler::PRIORITY_IO, 0)) {
127 0 : GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "CompassCalibrator: Cannot start compass thread.");
128 0 : return false;
129 : }
130 4 : _cal_thread_started = true;
131 : }
132 :
133 : // disable compass learning both for calibration and after completion
134 57 : _learn.set_and_save(LearnType::NONE);
135 :
136 57 : return true;
137 : }
138 :
139 811124 : void Compass::_update_calibration_trampoline() {
140 : while(true) {
141 3244496 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
142 2433372 : if (_calibrator[i] == nullptr) {
143 449936 : continue;
144 : }
145 1983436 : _calibrator[i]->update();
146 : }
147 811124 : hal.scheduler->delay(1);
148 811120 : }
149 : }
150 :
151 18 : bool Compass::_start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay, bool autoreboot)
152 : {
153 18 : _cal_autosave = autosave;
154 18 : _compass_cal_autoreboot = autoreboot;
155 :
156 18 : bool at_least_one_started = false;
157 72 : for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
158 54 : if ((1<<i) & mask) {
159 36 : if (!_start_calibration(i,retry,delay)) {
160 0 : _cancel_calibration_mask(mask);
161 0 : return false;
162 : }
163 36 : at_least_one_started = true;
164 : }
165 : }
166 18 : return at_least_one_started;
167 : }
168 :
169 7 : bool Compass::start_calibration_all(bool retry, bool autosave, float delay, bool autoreboot)
170 : {
171 7 : _cal_autosave = autosave;
172 7 : _compass_cal_autoreboot = autoreboot;
173 :
174 7 : bool at_least_one_started = false;
175 28 : for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
176 : // ignore any compasses that fail to start calibrating
177 : // start all should only calibrate compasses that are being used
178 21 : if (_start_calibration(i,retry,delay)) {
179 21 : at_least_one_started = true;
180 : }
181 : }
182 7 : return at_least_one_started;
183 : }
184 :
185 9 : void Compass::_cancel_calibration(uint8_t i)
186 : {
187 9 : AP_Notify::events.initiated_compass_cal = 0;
188 9 : Priority prio = Priority(i);
189 9 : if (_calibrator[prio] == nullptr) {
190 0 : return;
191 : }
192 9 : if (_calibrator[prio]->running() || _calibrator[prio]->get_state().status == CompassCalibrator::Status::WAITING_TO_START) {
193 9 : AP_Notify::events.compass_cal_canceled = 1;
194 : }
195 9 : _cal_saved[prio] = false;
196 9 : _calibrator[prio]->stop();
197 : }
198 :
199 4 : void Compass::_cancel_calibration_mask(uint8_t mask)
200 : {
201 16 : for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
202 12 : if ((1<<i) & mask) {
203 9 : _cancel_calibration(i);
204 : }
205 : }
206 4 : }
207 :
208 1 : void Compass::cancel_calibration_all()
209 : {
210 1 : _cancel_calibration_mask(0xFF);
211 1 : }
212 :
213 9 : bool Compass::_accept_calibration(uint8_t i)
214 : {
215 9 : Priority prio = Priority(i);
216 :
217 9 : CompassCalibrator* cal = _calibrator[prio];
218 9 : if (cal == nullptr) {
219 0 : return false;
220 : }
221 9 : const CompassCalibrator::Report cal_report = cal->get_report();
222 :
223 9 : if (_cal_saved[prio] || cal_report.status == CompassCalibrator::Status::NOT_STARTED) {
224 0 : return true;
225 9 : } else if (cal_report.status == CompassCalibrator::Status::SUCCESS) {
226 9 : _cal_saved[prio] = true;
227 :
228 9 : Vector3f ofs(cal_report.ofs), diag(cal_report.diag), offdiag(cal_report.offdiag);
229 9 : float scale_factor = cal_report.scale_factor;
230 :
231 9 : set_and_save_offsets(i, ofs);
232 : #if AP_COMPASS_DIAGONALS_ENABLED
233 9 : set_and_save_diagonals(i,diag);
234 9 : set_and_save_offdiagonals(i,offdiag);
235 : #endif
236 9 : set_and_save_scale_factor(i,scale_factor);
237 :
238 9 : if (cal_report.check_orientation && _get_state(prio).external && _rotate_auto >= 2) {
239 0 : set_and_save_orientation(i, cal_report.orientation);
240 : }
241 :
242 9 : if (!is_calibrating()) {
243 9 : AP_Notify::events.compass_cal_saved = 1;
244 : }
245 9 : return true;
246 : } else {
247 0 : return false;
248 : }
249 : }
250 :
251 4 : bool Compass::_accept_calibration_mask(uint8_t mask)
252 : {
253 4 : bool success = true;
254 16 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
255 12 : if (_calibrator[i] == nullptr) {
256 3 : continue;
257 : }
258 9 : if ((1<<uint8_t(i)) & mask) {
259 9 : if (!_accept_calibration(uint8_t(i))) {
260 0 : success = false;
261 : }
262 9 : _calibrator[i]->stop();
263 : }
264 : }
265 :
266 4 : return success;
267 : }
268 :
269 : #if HAL_GCS_ENABLED
270 1117755 : bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
271 : {
272 1117755 : const mavlink_channel_t chan = link.get_chan();
273 :
274 4471020 : for (uint8_t i = 0; i < COMPASS_MAX_INSTANCES; i++) {
275 3353265 : const Priority compass_id = (next_cal_progress_idx[chan] + 1) % COMPASS_MAX_INSTANCES;
276 :
277 3353265 : auto& calibrator = _calibrator[compass_id];
278 3353265 : if (calibrator == nullptr) {
279 3301285 : next_cal_progress_idx[chan] = compass_id;
280 3301285 : continue;
281 : }
282 51980 : const CompassCalibrator::State cal_state = calibrator->get_state();
283 :
284 51980 : if (cal_state.status == CompassCalibrator::Status::WAITING_TO_START ||
285 51980 : cal_state.status == CompassCalibrator::Status::RUNNING_STEP_ONE ||
286 34838 : cal_state.status == CompassCalibrator::Status::RUNNING_STEP_TWO) {
287 : // ensure we don't try to send with no space available
288 28661 : if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_PROGRESS)) {
289 0 : return false;
290 : }
291 :
292 28661 : next_cal_progress_idx[chan] = compass_id;
293 :
294 28661 : mavlink_msg_mag_cal_progress_send(
295 : link.get_chan(),
296 28661 : uint8_t(compass_id),
297 28661 : _get_cal_mask(),
298 28661 : (uint8_t)cal_state.status, cal_state.attempt, cal_state.completion_pct, cal_state.completion_mask,
299 : 0.0f, 0.0f, 0.0f
300 : );
301 : } else {
302 23319 : next_cal_progress_idx[chan] = compass_id;
303 : }
304 : }
305 :
306 1117755 : return true;
307 : }
308 :
309 1117755 : bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
310 : {
311 1117755 : const mavlink_channel_t chan = link.get_chan();
312 :
313 4471020 : for (uint8_t i = 0; i < COMPASS_MAX_INSTANCES; i++) {
314 3353265 : const Priority compass_id = (next_cal_report_idx[chan] + 1) % COMPASS_MAX_INSTANCES;
315 :
316 3353265 : if (_calibrator[compass_id] == nullptr) {
317 3301285 : next_cal_report_idx[chan] = compass_id;
318 3350054 : continue;
319 : }
320 51980 : const CompassCalibrator::Report cal_report = _calibrator[compass_id]->get_report();
321 51980 : switch (cal_report.status) {
322 48769 : case CompassCalibrator::Status::NOT_STARTED:
323 : case CompassCalibrator::Status::WAITING_TO_START:
324 : case CompassCalibrator::Status::RUNNING_STEP_ONE:
325 : case CompassCalibrator::Status::RUNNING_STEP_TWO:
326 : // calibration has not finished ergo no report
327 48769 : next_cal_report_idx[chan] = compass_id;
328 48769 : continue;
329 3211 : case CompassCalibrator::Status::SUCCESS:
330 : case CompassCalibrator::Status::FAILED:
331 : case CompassCalibrator::Status::FAILED_ORIENTATION:
332 : case CompassCalibrator::Status::FAILED_RADIUS:
333 : case CompassCalibrator::Status::FAILED_OFFSETS:
334 : case CompassCalibrator::Status::FAILED_DIAG_SCALING:
335 : case CompassCalibrator::Status::FAILED_RESIDUALS_HIGH:
336 : // ensure we don't try to send with no space available
337 3211 : if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_REPORT)) {
338 0 : return false;
339 : }
340 :
341 3211 : next_cal_report_idx[chan] = compass_id;
342 :
343 3211 : mavlink_msg_mag_cal_report_send(
344 : link.get_chan(),
345 3211 : uint8_t(compass_id),
346 3211 : _get_cal_mask(),
347 3211 : (uint8_t)cal_report.status,
348 3211 : _cal_saved[compass_id],
349 3211 : cal_report.fitness,
350 3211 : cal_report.ofs.x, cal_report.ofs.y, cal_report.ofs.z,
351 3211 : cal_report.diag.x, cal_report.diag.y, cal_report.diag.z,
352 3211 : cal_report.offdiag.x, cal_report.offdiag.y, cal_report.offdiag.z,
353 3211 : cal_report.orientation_confidence,
354 3211 : cal_report.original_orientation,
355 3211 : cal_report.orientation,
356 3211 : cal_report.scale_factor
357 : );
358 : }
359 : }
360 1117755 : return true;
361 : }
362 : #endif
363 :
364 46029384 : bool Compass::is_calibrating() const
365 : {
366 181996134 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
367 136674797 : if (_calibrator[i] == nullptr) {
368 134801750 : continue;
369 : }
370 1873047 : switch(_calibrator[i]->get_state().status) {
371 1165000 : case CompassCalibrator::Status::NOT_STARTED:
372 : case CompassCalibrator::Status::SUCCESS:
373 : case CompassCalibrator::Status::FAILED:
374 : case CompassCalibrator::Status::FAILED_ORIENTATION:
375 : case CompassCalibrator::Status::FAILED_RADIUS:
376 : case CompassCalibrator::Status::FAILED_OFFSETS:
377 : case CompassCalibrator::Status::FAILED_DIAG_SCALING:
378 : case CompassCalibrator::Status::FAILED_RESIDUALS_HIGH:
379 : // this backend isn't calibrating,
380 : // but maybe the next one is:
381 1165000 : continue;
382 708047 : case CompassCalibrator::Status::WAITING_TO_START:
383 : case CompassCalibrator::Status::RUNNING_STEP_ONE:
384 : case CompassCalibrator::Status::RUNNING_STEP_TWO:
385 708047 : return true;
386 : }
387 : }
388 45321337 : return false;
389 : }
390 :
391 31872 : uint8_t Compass::_get_cal_mask()
392 : {
393 31872 : uint8_t cal_mask = 0;
394 127488 : for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
395 95616 : if (_calibrator[i] == nullptr) {
396 9312 : continue;
397 : }
398 86304 : if (_calibrator[i]->get_state().status != CompassCalibrator::Status::NOT_STARTED) {
399 86304 : cal_mask |= 1 << uint8_t(i);
400 : }
401 : }
402 31872 : return cal_mask;
403 : }
404 :
405 : /*
406 : handle an incoming MAG_CAL command
407 : */
408 37 : MAV_RESULT Compass::handle_mag_cal_command(const mavlink_command_int_t &packet)
409 : {
410 37 : MAV_RESULT result = MAV_RESULT_FAILED;
411 :
412 37 : switch (packet.command) {
413 29 : case MAV_CMD_DO_START_MAG_CAL: {
414 29 : result = MAV_RESULT_ACCEPTED;
415 29 : if (hal.util->get_soft_armed()) {
416 4 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Disarm to allow compass calibration");
417 4 : result = MAV_RESULT_FAILED;
418 4 : break;
419 : }
420 25 : if (packet.param1 < 0 || packet.param1 > 255) {
421 0 : result = MAV_RESULT_FAILED;
422 0 : break;
423 : }
424 :
425 25 : uint8_t mag_mask = packet.param1;
426 25 : bool retry = !is_zero(packet.param2);
427 25 : bool autosave = !is_zero(packet.param3);
428 25 : float delay = packet.param4;
429 25 : bool autoreboot = packet.x != 0;
430 :
431 25 : if (mag_mask == 0) { // 0 means all
432 7 : _reset_compass_id();
433 7 : if (!start_calibration_all(retry, autosave, delay, autoreboot)) {
434 0 : result = MAV_RESULT_FAILED;
435 : }
436 : } else {
437 18 : if (!_start_calibration_mask(mag_mask, retry, autosave, delay, autoreboot)) {
438 0 : result = MAV_RESULT_FAILED;
439 : }
440 : }
441 :
442 25 : if (result != MAV_RESULT_ACCEPTED) {
443 0 : GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Compass calibration failed to start");
444 : }
445 :
446 25 : break;
447 : }
448 :
449 4 : case MAV_CMD_DO_ACCEPT_MAG_CAL: {
450 4 : result = MAV_RESULT_ACCEPTED;
451 4 : if (packet.param1 < 0 || packet.param1 > 255) {
452 0 : result = MAV_RESULT_FAILED;
453 0 : break;
454 : }
455 :
456 4 : uint8_t mag_mask = packet.param1;
457 :
458 4 : if (mag_mask == 0) { // 0 means all
459 1 : mag_mask = 0xFF;
460 : }
461 :
462 4 : if (!_accept_calibration_mask(mag_mask)) {
463 0 : result = MAV_RESULT_FAILED;
464 : }
465 4 : break;
466 : }
467 :
468 4 : case MAV_CMD_DO_CANCEL_MAG_CAL: {
469 4 : result = MAV_RESULT_ACCEPTED;
470 4 : if (packet.param1 < 0 || packet.param1 > 255) {
471 0 : result = MAV_RESULT_FAILED;
472 0 : break;
473 : }
474 :
475 4 : uint8_t mag_mask = packet.param1;
476 :
477 4 : if (mag_mask == 0) { // 0 means all
478 1 : cancel_calibration_all();
479 1 : break;
480 : }
481 :
482 3 : _cancel_calibration_mask(mag_mask);
483 3 : break;
484 : }
485 : }
486 :
487 37 : return result;
488 : }
489 :
490 : #endif // COMPASS_CAL_ENABLED
491 :
492 : #if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
493 : /*
494 : get mag field with the effects of offsets, diagonals and
495 : off-diagonals removed
496 : */
497 9 : bool Compass::get_uncorrected_field(uint8_t instance, Vector3f &field) const
498 : {
499 : // get corrected field
500 9 : field = get_field(instance);
501 :
502 : #if AP_COMPASS_DIAGONALS_ENABLED
503 : // form elliptical correction matrix and invert it. This is
504 : // needed to remove the effects of the elliptical correction
505 : // when calculating new offsets
506 9 : const Vector3f &diagonals = get_diagonals(instance);
507 9 : if (!diagonals.is_zero()) {
508 9 : const Vector3f &offdiagonals = get_offdiagonals(instance);
509 : Matrix3f mat {
510 9 : diagonals.x, offdiagonals.x, offdiagonals.y,
511 9 : offdiagonals.x, diagonals.y, offdiagonals.z,
512 9 : offdiagonals.y, offdiagonals.z, diagonals.z
513 9 : };
514 9 : if (!mat.invert()) {
515 0 : return false;
516 : }
517 :
518 : // remove impact of diagonals and off-diagonals
519 9 : field = mat * field;
520 : }
521 : #endif
522 :
523 : // remove impact of offsets
524 9 : field -= get_offsets(instance);
525 :
526 9 : return true;
527 : }
528 :
529 : /*
530 : fast compass calibration given vehicle position and yaw. This
531 : results in zero diagonal and off-diagonal elements, so is only
532 : suitable for vehicles where the field is close to spherical. It is
533 : useful for large vehicles where moving the vehicle to calibrate it
534 : is difficult.
535 :
536 : The offsets of the selected compasses are set to values to bring
537 : them into consistency with the WMM tables at the given latitude and
538 : longitude. If compass_mask is zero then all enabled compasses are
539 : calibrated.
540 :
541 : This assumes that the compass is correctly scaled in milliGauss
542 : */
543 3 : bool Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
544 : float lat_deg, float lon_deg, bool force_use)
545 : {
546 3 : _reset_compass_id();
547 3 : if (is_zero(lat_deg) && is_zero(lon_deg)) {
548 3 : Location loc;
549 : // get AHRS position. If unavailable then try GPS location
550 3 : if (!AP::ahrs().get_location(loc)) {
551 0 : if (AP::gps().status() < AP_GPS_FixType::FIX_3D) {
552 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag: no position available");
553 0 : return false;
554 : }
555 0 : loc = AP::gps().location();
556 : }
557 3 : lat_deg = loc.lat * 1.0e-7;
558 3 : lon_deg = loc.lng * 1.0e-7;
559 : }
560 :
561 : // get the magnetic field intensity and orientation
562 : float intensity;
563 : float declination;
564 : float inclination;
565 3 : if (!AP_Declination::get_mag_field_ef(lat_deg, lon_deg, intensity, declination, inclination)) {
566 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag: WMM table error");
567 0 : return false;
568 : }
569 :
570 : // create a field vector and rotate to the required orientation
571 3 : Vector3f field(1e3f * intensity, 0.0f, 0.0f);
572 3 : Matrix3f R;
573 3 : R.from_euler(0.0f, -radians(inclination), radians(declination));
574 3 : field = R * field;
575 :
576 3 : Matrix3f dcm;
577 3 : dcm.from_euler(AP::ahrs().get_roll_rad(), AP::ahrs().get_pitch_rad(), radians(yaw_deg));
578 :
579 : // Rotate into body frame using provided yaw
580 3 : field = dcm.transposed() * field;
581 :
582 12 : for (uint8_t i=0; i<get_count(); i++) {
583 9 : if (compass_mask != 0 && ((1U<<i) & compass_mask) == 0) {
584 : // skip this compass
585 0 : continue;
586 : }
587 9 : if (_use_for_yaw[Priority(i)] == 0 || (!force_use && !use_for_yaw(i))) {
588 0 : continue;
589 : }
590 9 : if (!healthy(i)) {
591 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag[%u]: unhealthy", i);
592 0 : return false;
593 : }
594 :
595 9 : Vector3f measurement;
596 9 : if (!get_uncorrected_field(i, measurement)) {
597 0 : GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag[%u]: bad uncorrected field", i);
598 0 : return false;
599 : }
600 :
601 9 : Vector3f offsets = field - measurement;
602 9 : set_and_save_offsets(i, offsets);
603 : #if AP_COMPASS_DIAGONALS_ENABLED
604 9 : Vector3f one{1,1,1};
605 9 : set_and_save_diagonals(i, one);
606 9 : Vector3f zero{0,0,0};
607 9 : set_and_save_offdiagonals(i, zero);
608 : #endif
609 : }
610 :
611 3 : return true;
612 : }
613 :
614 : #endif // AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
|
