Skip to content

Commit ddea6dc

Browse files
christianpetrichristianpetri
committed
AP_Compass: report specific mag calibration failure reasons
1 parent 8175f9f commit ddea6dc

4 files changed

Lines changed: 173 additions & 45 deletions

File tree

‎Tools/autotest/vehicle_test_suite.py‎

Lines changed: 79 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10198,17 +10198,26 @@ def do_test_mag_cal(mavproxy, params, compass_tnumber):
1019810198
tstart = self.get_sim_time()
1019910199
reached_pct = [0] * compass_tnumber
1020010200
report_get = [0] * compass_tnumber
10201+
# COMPASS_CAL_FIT=0.001 forces fitness > tolerance, so we expect
10202+
# MAG_CAL_FAILED_RESIDUALS_HIGH.
10203+
# TODO(mavlink-bump): drop nested fallback once new enum names are
10204+
# available in pymavlink.
10205+
MAG_CAL_FAILED_RESIDUALS_HIGH = getattr(
10206+
mavutil.mavlink,
10207+
'MAG_CAL_FAILED_RESIDUALS_HIGH',
10208+
getattr(mavutil.mavlink, 'MAG_CAL_BAD_FITNESS', 10)
10209+
)
1020110210
while True:
1020210211
if self.get_sim_time_cached() - tstart > timeout:
1020310212
raise NotAchievedException("Cannot receive enough MAG_CAL_PROGRESS")
1020410213
m = self.assert_receive_message(["MAG_CAL_PROGRESS", "MAG_CAL_REPORT"], timeout=10)
1020510214
if m.get_type() == "MAG_CAL_REPORT":
1020610215
if report_get[m.compass_id] == 0:
1020710216
self.progress("Report: %s" % str(m))
10208-
if m.cal_status == mavutil.mavlink.MAG_CAL_FAILED:
10217+
if m.cal_status == MAG_CAL_FAILED_RESIDUALS_HIGH:
1020910218
report_get[m.compass_id] = 1
1021010219
else:
10211-
raise NotAchievedException("Mag calibration didn't failed")
10220+
raise NotAchievedException("Expected MAG_CAL_FAILED_RESIDUALS_HIGH (10), got %u" % m.cal_status)
1021210221
if all(ele >= 1 for ele in report_get):
1021310222
self.progress("All Mag report failure")
1021410223
break
@@ -10227,6 +10236,74 @@ def do_test_mag_cal(mavproxy, params, compass_tnumber):
1022710236
self.check_zeros_mag_orient()
1022810237
self.set_parameter("COMPASS_CAL_FIT", old_cal_fit, add_to_context=False)
1022910238

10239+
#################################################
10240+
if compass_tnumber > 1 and target_mask == 0:
10241+
self.start_subtest("Try magcal with one bad compass and ensure others continue")
10242+
self.progress("Compass mask is %s" % "{0:b}".format(target_mask))
10243+
10244+
old_sim_mag1_ofs_x = self.get_parameter("SIM_MAG1_OFS_X")
10245+
old_sim_mag1_ofs_y = self.get_parameter("SIM_MAG1_OFS_Y")
10246+
old_sim_mag1_ofs_z = self.get_parameter("SIM_MAG1_OFS_Z")
10247+
10248+
self.set_parameters({
10249+
"SIM_MAG1_OFS_X": 2000,
10250+
"SIM_MAG1_OFS_Y": 2000,
10251+
"SIM_MAG1_OFS_Z": 2000,
10252+
}, add_to_context=False)
10253+
10254+
try:
10255+
reset_pos_and_start_magcal(mavproxy, target_mask)
10256+
report_status = [None] * compass_tnumber
10257+
tstart = self.get_sim_time()
10258+
while True:
10259+
if self.get_sim_time_cached() - tstart > timeout:
10260+
raise NotAchievedException("Cannot receive enough MAG_CAL_REPORT in selective-failure test")
10261+
m = self.mav.recv_match(type=["MAG_CAL_PROGRESS", "MAG_CAL_REPORT"], blocking=True, timeout=1)
10262+
if m is None:
10263+
continue
10264+
if m.get_type() != "MAG_CAL_REPORT":
10265+
continue
10266+
10267+
report_status[m.compass_id] = m.cal_status
10268+
self.progress("Selective-failure report compass %u status %u" %
10269+
(m.compass_id, m.cal_status))
10270+
if all(status is not None for status in report_status):
10271+
break
10272+
10273+
# SIM_MAG1_OFS_X/Y/Z=2000 exceeds COMPASS_OFFS_MAX, so the
10274+
# calibrator sets FAILED_OFFSETS (8) instead of FAILED (5).
10275+
# TODO(mavlink-bump): drop nested fallback once new enum
10276+
# names are available in pymavlink.
10277+
MAG_CAL_FAILED_OFFSETS = getattr(
10278+
mavutil.mavlink,
10279+
'MAG_CAL_FAILED_OFFSETS',
10280+
getattr(mavutil.mavlink, 'MAG_CAL_BAD_OFFSETS', 8)
10281+
)
10282+
if report_status[0] != MAG_CAL_FAILED_OFFSETS:
10283+
raise NotAchievedException(
10284+
"Expected compass 0 to report MAG_CAL_FAILED_OFFSETS (8), got %u" %
10285+
report_status[0]
10286+
)
10287+
10288+
other_succeeded = False
10289+
for status in report_status[1:]:
10290+
if status == mavutil.mavlink.MAG_CAL_SUCCESS:
10291+
other_succeeded = True
10292+
break
10293+
10294+
if not other_succeeded:
10295+
raise NotAchievedException("Expected at least one non-degraded compass calibration success")
10296+
10297+
finally:
10298+
self.set_parameters({
10299+
"SIM_MAG1_OFS_X": old_sim_mag1_ofs_x,
10300+
"SIM_MAG1_OFS_Y": old_sim_mag1_ofs_y,
10301+
"SIM_MAG1_OFS_Z": old_sim_mag1_ofs_z,
10302+
}, add_to_context=False)
10303+
10304+
self.check_zero_mag_parameters(params)
10305+
self.check_zeros_mag_orient()
10306+
1023010307
#################################################
1023110308
self.start_subtest("Try magcal and wait success")
1023210309
self.progress("Compass mask is %s" % "{0:b}".format(target_mask))

‎libraries/AP_Compass/AP_Compass_Calibration.cpp‎

Lines changed: 14 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ void Compass::cal_update()
2020
bool running = false;
2121
uint8_t num_compass = 0;
2222
uint8_t num_compass_successful = 0;
23-
23+
2424
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
2525
if (_calibrator[i] == nullptr) {
2626
continue;
@@ -54,13 +54,14 @@ void Compass::cal_update()
5454
if (is_calibrating()) {
5555
_cal_has_run = true;
5656
return;
57-
}
57+
}
5858
if (!_cal_has_run) {
5959
// calibration hasn't started yet
6060
return;
6161
}
6262
if (num_compass != num_compass_successful) {
63-
// only reboot if all successful
63+
// A failed compass (FAILED_OFFSETS, FAILED_RESIDUALS_HIGH, etc.) means the vehicle
64+
// may not be airworthy — do not auto-reboot in that case.
6465
return;
6566
}
6667
if (!_compass_cal_autoreboot) {
@@ -328,8 +329,11 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
328329
continue;
329330
case CompassCalibrator::Status::SUCCESS:
330331
case CompassCalibrator::Status::FAILED:
331-
case CompassCalibrator::Status::BAD_ORIENTATION:
332-
case CompassCalibrator::Status::BAD_RADIUS:
332+
case CompassCalibrator::Status::FAILED_ORIENTATION:
333+
case CompassCalibrator::Status::FAILED_RADIUS:
334+
case CompassCalibrator::Status::FAILED_OFFSETS:
335+
case CompassCalibrator::Status::FAILED_DIAG_SCALING:
336+
case CompassCalibrator::Status::FAILED_RESIDUALS_HIGH:
333337
// ensure we don't try to send with no space available
334338
if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_REPORT)) {
335339
return false;
@@ -368,8 +372,11 @@ bool Compass::is_calibrating() const
368372
case CompassCalibrator::Status::NOT_STARTED:
369373
case CompassCalibrator::Status::SUCCESS:
370374
case CompassCalibrator::Status::FAILED:
371-
case CompassCalibrator::Status::BAD_ORIENTATION:
372-
case CompassCalibrator::Status::BAD_RADIUS:
375+
case CompassCalibrator::Status::FAILED_ORIENTATION:
376+
case CompassCalibrator::Status::FAILED_RADIUS:
377+
case CompassCalibrator::Status::FAILED_OFFSETS:
378+
case CompassCalibrator::Status::FAILED_DIAG_SCALING:
379+
case CompassCalibrator::Status::FAILED_RESIDUALS_HIGH:
373380
// this backend isn't calibrating,
374381
// but maybe the next one is:
375382
continue;

‎libraries/AP_Compass/CompassCalibrator.cpp‎

Lines changed: 71 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -138,8 +138,11 @@ bool CompassCalibrator::failed() {
138138
WITH_SEMAPHORE(state_sem);
139139
switch (cal_state.status) {
140140
case Status::FAILED:
141-
case Status::BAD_ORIENTATION:
142-
case Status::BAD_RADIUS:
141+
case Status::FAILED_ORIENTATION:
142+
case Status::FAILED_RADIUS:
143+
case Status::FAILED_OFFSETS:
144+
case Status::FAILED_DIAG_SCALING:
145+
case Status::FAILED_RESIDUALS_HIGH:
143146
return true;
144147
case Status::SUCCESS:
145148
case Status::NOT_STARTED:
@@ -181,7 +184,7 @@ void CompassCalibrator::update()
181184
{
182185
WITH_SEMAPHORE(state_sem);
183186
//update_settings
184-
if (!running()) {
187+
if (!_running() && !_retry_pending) {
185188
update_cal_settings();
186189
}
187190

@@ -193,6 +196,17 @@ void CompassCalibrator::update()
193196
//update report and status
194197
update_cal_status();
195198
update_cal_report();
199+
200+
// Delay the retry state transition until after cal_report reflects
201+
// the FAILED_* status for at least one GCS poll cycle (~1 ms), so that
202+
// the ground station can display the specific failure reason before
203+
// the next calibration attempt begins.
204+
if (_retry_pending) {
205+
_retry_pending = false;
206+
if (set_status(Status::WAITING_TO_START)) {
207+
_attempt++;
208+
}
209+
}
196210
}
197211

198212
// collect the minimum number of samples
@@ -319,8 +333,11 @@ void CompassCalibrator::update_cal_status()
319333
cal_state.completion_pct = 100.0f;
320334
break;
321335
case Status::FAILED:
322-
case Status::BAD_ORIENTATION:
323-
case Status::BAD_RADIUS:
336+
case Status::FAILED_ORIENTATION:
337+
case Status::FAILED_RADIUS:
338+
case Status::FAILED_OFFSETS:
339+
case Status::FAILED_DIAG_SCALING:
340+
case Status::FAILED_RESIDUALS_HIGH:
324341
cal_state.completion_pct = 0.0f;
325342
break;
326343
};
@@ -376,6 +393,7 @@ void CompassCalibrator::reset_state()
376393
_params.diag = Vector3f(1.0f,1.0f,1.0f);
377394
_params.offdiag.zero();
378395
_params.scale_factor = 0;
396+
_retry_pending = false;
379397

380398
memset(_completion_mask, 0, sizeof(_completion_mask));
381399
initialize_fit();
@@ -443,24 +461,33 @@ bool CompassCalibrator::set_status(CompassCalibrator::Status status)
443461
}
444462

445463
_status = Status::SUCCESS;
464+
_retry_pending = false;
446465
return true;
447466

448467
case Status::FAILED:
449-
if (_status == Status::BAD_ORIENTATION ||
450-
_status == Status::BAD_RADIUS) {
451-
// don't overwrite bad orientation status
468+
if (_status == Status::FAILED_ORIENTATION ||
469+
_status == Status::FAILED_RADIUS ||
470+
_status == Status::FAILED_OFFSETS ||
471+
_status == Status::FAILED_DIAG_SCALING ||
472+
_status == Status::FAILED_RESIDUALS_HIGH) {
473+
// don't overwrite specific failure status
452474
return false;
453475
}
454476
FALLTHROUGH;
455477

456-
case Status::BAD_ORIENTATION:
457-
case Status::BAD_RADIUS:
478+
case Status::FAILED_ORIENTATION:
479+
case Status::FAILED_RADIUS:
480+
case Status::FAILED_OFFSETS:
481+
case Status::FAILED_DIAG_SCALING:
482+
case Status::FAILED_RESIDUALS_HIGH:
458483
if (_status == Status::NOT_STARTED) {
459484
return false;
460485
}
461486

462-
if (_retry && set_status(Status::WAITING_TO_START)) {
463-
_attempt++;
487+
_status = status;
488+
489+
if (_retry) {
490+
_retry_pending = true;
464491
return true;
465492
}
466493

@@ -469,30 +496,43 @@ bool CompassCalibrator::set_status(CompassCalibrator::Status status)
469496
_sample_buffer = nullptr;
470497
}
471498

472-
_status = status;
473499
return true;
474500
};
475501

476502
// compiler guarantees we don't get here
477503
return false;
478504
}
479505

480-
bool CompassCalibrator::fit_acceptable() const
506+
bool CompassCalibrator::fit_acceptable()
481507
{
482-
if (!isnan(_fitness) &&
483-
_params.radius > FIELD_RADIUS_MIN && _params.radius < FIELD_RADIUS_MAX &&
484-
fabsf(_params.offset.x) < _offset_max &&
485-
fabsf(_params.offset.y) < _offset_max &&
486-
fabsf(_params.offset.z) < _offset_max &&
487-
_params.diag.x > 0.2f && _params.diag.x < 5.0f &&
488-
_params.diag.y > 0.2f && _params.diag.y < 5.0f &&
489-
_params.diag.z > 0.2f && _params.diag.z < 5.0f &&
490-
fabsf(_params.offdiag.x) < 1.0f && //absolute of sine/cosine output cannot be greater than 1
491-
fabsf(_params.offdiag.y) < 1.0f &&
492-
fabsf(_params.offdiag.z) < 1.0f ) {
493-
return _fitness <= sq(_tolerance);
494-
}
495-
return false;
508+
if (isnan(_fitness)) {
509+
set_status(Status::FAILED_RESIDUALS_HIGH);
510+
return false;
511+
}
512+
if (_params.radius <= FIELD_RADIUS_MIN || _params.radius >= FIELD_RADIUS_MAX) {
513+
set_status(Status::FAILED_RADIUS);
514+
return false;
515+
}
516+
if (fabsf(_params.offset.x) >= _offset_max ||
517+
fabsf(_params.offset.y) >= _offset_max ||
518+
fabsf(_params.offset.z) >= _offset_max) {
519+
set_status(Status::FAILED_OFFSETS);
520+
return false;
521+
}
522+
if (_params.diag.x <= 0.2f || _params.diag.x >= 5.0f ||
523+
_params.diag.y <= 0.2f || _params.diag.y >= 5.0f ||
524+
_params.diag.z <= 0.2f || _params.diag.z >= 5.0f ||
525+
fabsf(_params.offdiag.x) >= 1.0f ||
526+
fabsf(_params.offdiag.y) >= 1.0f ||
527+
fabsf(_params.offdiag.z) >= 1.0f) {
528+
set_status(Status::FAILED_DIAG_SCALING);
529+
return false;
530+
}
531+
if (_fitness > sq(_tolerance)) {
532+
set_status(Status::FAILED_RESIDUALS_HIGH);
533+
return false;
534+
}
535+
return true;
496536
}
497537

498538
void CompassCalibrator::thin_samples()
@@ -1025,7 +1065,7 @@ bool CompassCalibrator::calculate_orientation(void)
10251065
}
10261066

10271067
if (!pass) {
1028-
set_status(Status::BAD_ORIENTATION);
1068+
set_status(Status::FAILED_ORIENTATION);
10291069
return false;
10301070
}
10311071

@@ -1039,7 +1079,7 @@ bool CompassCalibrator::calculate_orientation(void)
10391079
// for reporting purposes
10401080
_orientation = besti;
10411081
_orientation_solution = besti;
1042-
set_status(Status::BAD_ORIENTATION);
1082+
set_status(Status::FAILED_ORIENTATION);
10431083
return false;
10441084
}
10451085

@@ -1098,7 +1138,7 @@ bool CompassCalibrator::fix_radius(void)
10981138
_compass_idx,
10991139
_params.radius,
11001140
expected_radius);
1101-
set_status(Status::BAD_RADIUS);
1141+
set_status(Status::FAILED_RADIUS);
11021142
return false;
11031143
}
11041144

‎libraries/AP_Compass/CompassCalibrator.h‎

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@ class CompassCalibrator {
2727
// running is true if actively calculating offsets, diagonals or offdiagonals
2828
bool running();
2929

30-
// failed is true if either of the failure states are hit
30+
// failed is true if any terminal failure state is hit
3131
bool failed();
3232

3333

@@ -43,8 +43,11 @@ class CompassCalibrator {
4343
RUNNING_STEP_TWO = 3,
4444
SUCCESS = 4,
4545
FAILED = 5,
46-
BAD_ORIENTATION = 6,
47-
BAD_RADIUS = 7,
46+
FAILED_ORIENTATION = 6,
47+
FAILED_RADIUS = 7,
48+
FAILED_OFFSETS = 8,
49+
FAILED_DIAG_SCALING = 9,
50+
FAILED_RESIDUALS_HIGH = 10,
4851
};
4952

5053
// get completion mask for mavlink reporting (a bitmask of faces/directions for which we have compass samples)
@@ -156,8 +159,8 @@ class CompassCalibrator {
156159
bool accept_sample(const Vector3f &sample, uint16_t skip_index = UINT16_MAX);
157160
bool accept_sample(const CompassSample &sample, uint16_t skip_index = UINT16_MAX);
158161

159-
// returns true if fit is acceptable
160-
bool fit_acceptable() const;
162+
// returns true if fit is acceptable, sets specific BAD_* status on failure
163+
bool fit_acceptable();
161164

162165
// clear sample buffer and reset offsets and scaling to their defaults
163166
void reset_state();
@@ -248,6 +251,7 @@ class CompassCalibrator {
248251

249252
Status _requested_status;
250253
bool _status_set_requested;
254+
bool _retry_pending = false;
251255

252256
bool _new_sample;
253257

0 commit comments

Comments
 (0)