From 8227049c7ecc8a10ab66d08809ad73ff5da15976 Mon Sep 17 00:00:00 2001 From: Christian Petri <5165753+christianpetri@users.noreply.github.com> Date: Sat, 25 Jul 2026 18:47:53 +0200 Subject: [PATCH 1/2] AP_Compass: report specific failure reason when fit rejected --- .../AP_Compass/AP_Compass_Calibration.cpp | 14 +- libraries/AP_Compass/CompassCalibrator.cpp | 120 +++++++++++++----- libraries/AP_Compass/CompassCalibrator.h | 15 ++- 3 files changed, 107 insertions(+), 42 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index 20f3b3a343fb93..2f4db068e4ef67 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -328,8 +328,11 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link) continue; case CompassCalibrator::Status::SUCCESS: case CompassCalibrator::Status::FAILED: - case CompassCalibrator::Status::BAD_ORIENTATION: - case CompassCalibrator::Status::BAD_RADIUS: + case CompassCalibrator::Status::FAILED_ORIENTATION: + case CompassCalibrator::Status::FAILED_RADIUS: + case CompassCalibrator::Status::FAILED_OFFSETS: + case CompassCalibrator::Status::FAILED_DIAG_SCALING: + case CompassCalibrator::Status::FAILED_RESIDUALS_HIGH: // ensure we don't try to send with no space available if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_REPORT)) { return false; @@ -368,8 +371,11 @@ bool Compass::is_calibrating() const case CompassCalibrator::Status::NOT_STARTED: case CompassCalibrator::Status::SUCCESS: case CompassCalibrator::Status::FAILED: - case CompassCalibrator::Status::BAD_ORIENTATION: - case CompassCalibrator::Status::BAD_RADIUS: + case CompassCalibrator::Status::FAILED_ORIENTATION: + case CompassCalibrator::Status::FAILED_RADIUS: + case CompassCalibrator::Status::FAILED_OFFSETS: + case CompassCalibrator::Status::FAILED_DIAG_SCALING: + case CompassCalibrator::Status::FAILED_RESIDUALS_HIGH: // this backend isn't calibrating, // but maybe the next one is: continue; diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index d605b9e84e486e..510b5c1b380a08 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -73,6 +73,12 @@ #define FIELD_RADIUS_MIN 150 #define FIELD_RADIUS_MAX 950 +// When retry is enabled, hold a FAILED_* status for this long before starting +// the next attempt, so a GCS polling MAG_CAL_REPORT (streamed in the EXTRA3 +// group at a few Hz) reliably observes the specific failure reason before the +// retry overwrites it. +#define COMPASS_CAL_RETRY_REPORT_HOLD_MS 500 + //////////////////////////////////////////////////////////// ///////////////////// PUBLIC INTERFACE ///////////////////// //////////////////////////////////////////////////////////// @@ -138,8 +144,11 @@ bool CompassCalibrator::failed() { WITH_SEMAPHORE(state_sem); switch (cal_state.status) { case Status::FAILED: - case Status::BAD_ORIENTATION: - case Status::BAD_RADIUS: + case Status::FAILED_ORIENTATION: + case Status::FAILED_RADIUS: + case Status::FAILED_OFFSETS: + case Status::FAILED_DIAG_SCALING: + case Status::FAILED_RESIDUALS_HIGH: return true; case Status::SUCCESS: case Status::NOT_STARTED: @@ -181,7 +190,7 @@ void CompassCalibrator::update() { WITH_SEMAPHORE(state_sem); //update_settings - if (!running()) { + if (!_running() && !_retry_pending) { update_cal_settings(); } @@ -193,6 +202,17 @@ void CompassCalibrator::update() //update report and status update_cal_status(); update_cal_report(); + + // Hold the FAILED_* status for COMPASS_CAL_RETRY_REPORT_HOLD_MS before + // starting the next attempt, so a GCS polling MAG_CAL_REPORT can display + // the specific failure reason before the retry overwrites it. + if (_retry_pending && + AP_HAL::millis() - _retry_hold_start_ms >= COMPASS_CAL_RETRY_REPORT_HOLD_MS) { + _retry_pending = false; + if (set_status(Status::WAITING_TO_START)) { + _attempt++; + } + } } // collect the minimum number of samples @@ -216,9 +236,16 @@ void CompassCalibrator::update() } } else if (_status == Status::RUNNING_STEP_TWO) { if (_fit_step >= 35) { - if (fit_acceptable() && fix_radius() && calculate_orientation()) { + const Status fit_status = fit_acceptable_status(); + if (fit_status != Status::SUCCESS) { + // specific reason already identified, set it directly + set_status(fit_status); + } else if (fix_radius() && calculate_orientation()) { set_status(Status::SUCCESS); } else { + // fix_radius()/calculate_orientation() set their own specific + // FAILED_* reason; set generic FAILED only as last resort if + // status is somehow still not set to a terminal state. set_status(Status::FAILED); } } else if (_fit_step < 15) { @@ -319,8 +346,11 @@ void CompassCalibrator::update_cal_status() cal_state.completion_pct = 100.0f; break; case Status::FAILED: - case Status::BAD_ORIENTATION: - case Status::BAD_RADIUS: + case Status::FAILED_ORIENTATION: + case Status::FAILED_RADIUS: + case Status::FAILED_OFFSETS: + case Status::FAILED_DIAG_SCALING: + case Status::FAILED_RESIDUALS_HIGH: cal_state.completion_pct = 0.0f; break; }; @@ -376,6 +406,7 @@ void CompassCalibrator::reset_state() _params.diag = Vector3f(1.0f,1.0f,1.0f); _params.offdiag.zero(); _params.scale_factor = 0; + _retry_pending = false; memset(_completion_mask, 0, sizeof(_completion_mask)); initialize_fit(); @@ -443,24 +474,34 @@ bool CompassCalibrator::set_status(CompassCalibrator::Status status) } _status = Status::SUCCESS; + _retry_pending = false; return true; case Status::FAILED: - if (_status == Status::BAD_ORIENTATION || - _status == Status::BAD_RADIUS) { - // don't overwrite bad orientation status + if (_status == Status::FAILED_ORIENTATION || + _status == Status::FAILED_RADIUS || + _status == Status::FAILED_OFFSETS || + _status == Status::FAILED_DIAG_SCALING || + _status == Status::FAILED_RESIDUALS_HIGH) { + // don't overwrite specific failure status return false; } FALLTHROUGH; - case Status::BAD_ORIENTATION: - case Status::BAD_RADIUS: + case Status::FAILED_ORIENTATION: + case Status::FAILED_RADIUS: + case Status::FAILED_OFFSETS: + case Status::FAILED_DIAG_SCALING: + case Status::FAILED_RESIDUALS_HIGH: if (_status == Status::NOT_STARTED) { return false; } - if (_retry && set_status(Status::WAITING_TO_START)) { - _attempt++; + _status = status; + + if (_retry) { + _retry_pending = true; + _retry_hold_start_ms = AP_HAL::millis(); return true; } @@ -469,7 +510,6 @@ bool CompassCalibrator::set_status(CompassCalibrator::Status status) _sample_buffer = nullptr; } - _status = status; return true; }; @@ -477,22 +517,31 @@ bool CompassCalibrator::set_status(CompassCalibrator::Status status) return false; } -bool CompassCalibrator::fit_acceptable() const +CompassCalibrator::Status CompassCalibrator::fit_acceptable_status() const { - if (!isnan(_fitness) && - _params.radius > FIELD_RADIUS_MIN && _params.radius < FIELD_RADIUS_MAX && - fabsf(_params.offset.x) < _offset_max && - fabsf(_params.offset.y) < _offset_max && - fabsf(_params.offset.z) < _offset_max && - _params.diag.x > 0.2f && _params.diag.x < 5.0f && - _params.diag.y > 0.2f && _params.diag.y < 5.0f && - _params.diag.z > 0.2f && _params.diag.z < 5.0f && - fabsf(_params.offdiag.x) < 1.0f && //absolute of sine/cosine output cannot be greater than 1 - fabsf(_params.offdiag.y) < 1.0f && - fabsf(_params.offdiag.z) < 1.0f ) { - return _fitness <= sq(_tolerance); - } - return false; + if (isnan(_fitness)) { + return Status::FAILED_RESIDUALS_HIGH; + } + if (_params.radius <= FIELD_RADIUS_MIN || _params.radius >= FIELD_RADIUS_MAX) { + return Status::FAILED_RADIUS; + } + if (fabsf(_params.offset.x) >= _offset_max || + fabsf(_params.offset.y) >= _offset_max || + fabsf(_params.offset.z) >= _offset_max) { + return Status::FAILED_OFFSETS; + } + if (_params.diag.x <= 0.2f || _params.diag.x >= 5.0f || + _params.diag.y <= 0.2f || _params.diag.y >= 5.0f || + _params.diag.z <= 0.2f || _params.diag.z >= 5.0f || + fabsf(_params.offdiag.x) >= 1.0f || + fabsf(_params.offdiag.y) >= 1.0f || + fabsf(_params.offdiag.z) >= 1.0f) { + return Status::FAILED_DIAG_SCALING; + } + if (_fitness > sq(_tolerance)) { + return Status::FAILED_RESIDUALS_HIGH; + } + return Status::SUCCESS; } void CompassCalibrator::thin_samples() @@ -1025,7 +1074,7 @@ bool CompassCalibrator::calculate_orientation(void) } if (!pass) { - set_status(Status::BAD_ORIENTATION); + set_status(Status::FAILED_ORIENTATION); return false; } @@ -1039,7 +1088,7 @@ bool CompassCalibrator::calculate_orientation(void) // for reporting purposes _orientation = besti; _orientation_solution = besti; - set_status(Status::BAD_ORIENTATION); + set_status(Status::FAILED_ORIENTATION); return false; } @@ -1066,7 +1115,12 @@ bool CompassCalibrator::calculate_orientation(void) run_sphere_fit(); run_ellipsoid_fit(); - return fit_acceptable(); + const Status s = fit_acceptable_status(); + if (s != Status::SUCCESS) { + set_status(s); + return false; + } + return true; } /* @@ -1098,7 +1152,7 @@ bool CompassCalibrator::fix_radius(void) _compass_idx, _params.radius, expected_radius); - set_status(Status::BAD_RADIUS); + set_status(Status::FAILED_RADIUS); return false; } diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index 896d8c9a76e08a..1030468c9c78bc 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -27,7 +27,7 @@ class CompassCalibrator { // running is true if actively calculating offsets, diagonals or offdiagonals bool running(); - // failed is true if either of the failure states are hit + // failed is true if any terminal failure state is hit bool failed(); @@ -43,8 +43,11 @@ class CompassCalibrator { RUNNING_STEP_TWO = 3, SUCCESS = 4, FAILED = 5, - BAD_ORIENTATION = 6, - BAD_RADIUS = 7, + FAILED_ORIENTATION = 6, + FAILED_RADIUS = 7, + FAILED_OFFSETS = 8, + FAILED_DIAG_SCALING = 9, + FAILED_RESIDUALS_HIGH = 10, }; // get completion mask for mavlink reporting (a bitmask of faces/directions for which we have compass samples) @@ -156,8 +159,8 @@ class CompassCalibrator { bool accept_sample(const Vector3f &sample, uint16_t skip_index = UINT16_MAX); bool accept_sample(const CompassSample &sample, uint16_t skip_index = UINT16_MAX); - // returns true if fit is acceptable - bool fit_acceptable() const; + // returns specific failure Status if fit is unacceptable, SUCCESS if acceptable + Status fit_acceptable_status() const; // clear sample buffer and reset offsets and scaling to their defaults void reset_state(); @@ -248,6 +251,8 @@ class CompassCalibrator { Status _requested_status; bool _status_set_requested; + bool _retry_pending; + uint32_t _retry_hold_start_ms; // millis() when the FAILED_* status was published (retry hold timer) bool _new_sample; From d9b82c6d328fef3798fd1ed64ddd85b84edf7253 Mon Sep 17 00:00:00 2001 From: Christian Petri <5165753+christianpetri@users.noreply.github.com> Date: Sat, 25 Jul 2026 18:47:53 +0200 Subject: [PATCH 2/2] autotest: cover specific magcal failure reasons --- Tools/autotest/vehicle_test_suite.py | 95 +++++++++++++++++++++++++--- 1 file changed, 86 insertions(+), 9 deletions(-) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 1b2c1a27740425..bf3ca3093a9ff2 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -10691,12 +10691,11 @@ def do_test_mag_cal(mavproxy, params, compass_tnumber): m = self.mav.recv_match(type='MAG_CAL_PROGRESS', blocking=True, timeout=5) if m is None: if tstop is not None: - # wait 3 second to unsure that the calibration is well stopped - if self.get_sim_time_cached() - tstop > 10: - if reached_pct[0] > 33: - raise NotAchievedException("Mag calibration didn't stop") - else: - break + # if no more progress arrives for a few seconds after cancel, + # treat the calibration as stopped regardless of the last + # reported completion percentage. + if self.get_sim_time_cached() - tstop > 3: + break else: continue else: @@ -10728,7 +10727,8 @@ def do_test_mag_cal(mavproxy, params, compass_tnumber): if tstop is None: tstop = self.get_sim_time_cached() if tstop is not None: - # wait 3 second to unsure that the calibration is well stopped + # receiving progress for more than a few seconds after cancel + # means the calibration did not stop promptly. if self.get_sim_time_cached() - tstop > 3: raise NotAchievedException("Mag calibration didn't stop") self.check_zero_mag_parameters(params) @@ -10743,6 +10743,9 @@ def do_test_mag_cal(mavproxy, params, compass_tnumber): tstart = self.get_sim_time() reached_pct = [0] * compass_tnumber report_get = [0] * compass_tnumber + # COMPASS_CAL_FIT=0.001 forces fitness > tolerance, so we expect + # MAG_CAL_FAILED_RESIDUALS_HIGH. + MAG_CAL_FAILED_RESIDUALS_HIGH = mavutil.mavlink.MAG_CAL_FAILED_RESIDUALS_HIGH while True: if self.get_sim_time_cached() - tstart > timeout: raise NotAchievedException("Cannot receive enough MAG_CAL_PROGRESS") @@ -10750,10 +10753,10 @@ def do_test_mag_cal(mavproxy, params, compass_tnumber): if m.get_type() == "MAG_CAL_REPORT": if report_get[m.compass_id] == 0: self.progress("Report: %s" % str(m)) - if m.cal_status == mavutil.mavlink.MAG_CAL_FAILED: + if m.cal_status == MAG_CAL_FAILED_RESIDUALS_HIGH: report_get[m.compass_id] = 1 else: - raise NotAchievedException("Mag calibration didn't failed") + raise NotAchievedException("Expected MAG_CAL_FAILED_RESIDUALS_HIGH (10), got %u" % m.cal_status) if all(ele >= 1 for ele in report_get): self.progress("All Mag report failure") break @@ -10772,6 +10775,80 @@ def do_test_mag_cal(mavproxy, params, compass_tnumber): self.check_zeros_mag_orient() self.set_parameter("COMPASS_CAL_FIT", old_cal_fit, add_to_context=False) + ################################################# + if compass_tnumber > 1 and target_mask == 0: + self.start_subtest("Try magcal with one bad compass and ensure others continue") + self.progress("Compass mask is %s" % "{0:b}".format(target_mask)) + + old_sim_mag1_ofs_x = self.get_parameter("SIM_MAG1_OFS_X") + old_sim_mag1_ofs_y = self.get_parameter("SIM_MAG1_OFS_Y") + old_sim_mag1_ofs_z = self.get_parameter("SIM_MAG1_OFS_Z") + + self.set_parameters({ + "SIM_MAG1_OFS_X": 2000, + "SIM_MAG1_OFS_Y": 2000, + "SIM_MAG1_OFS_Z": 2000, + }, add_to_context=False) + + try: + reset_pos_and_start_magcal(mavproxy, target_mask) + report_status = [None] * compass_tnumber + tstart = self.get_sim_time() + while True: + if self.get_sim_time_cached() - tstart > timeout: + raise NotAchievedException("Cannot receive enough MAG_CAL_REPORT in selective-failure test") + m = self.mav.recv_match(type=["MAG_CAL_PROGRESS", "MAG_CAL_REPORT"], blocking=True, timeout=1) + if m is None: + continue + if m.get_type() != "MAG_CAL_REPORT": + continue + + report_status[m.compass_id] = m.cal_status + self.progress("Selective-failure report compass %u status %u" % + (m.compass_id, m.cal_status)) + if all(status is not None for status in report_status): + break + + # SIM_MAG1_OFS_X/Y/Z=2000 exceeds COMPASS_OFFS_MAX, so one + # compass is expected to report FAILED_OFFSETS. Do not + # assume compass_id ordering here; some SITL setups can + # differ in instance mapping. + MAG_CAL_FAILED_OFFSETS = mavutil.mavlink.MAG_CAL_FAILED_OFFSETS + failed_offsets_idxs = [] + for i, status in enumerate(report_status): + if status == MAG_CAL_FAILED_OFFSETS: + failed_offsets_idxs.append(i) + + if len(failed_offsets_idxs) != 1: + raise NotAchievedException( + "Expected exactly one compass to report MAG_CAL_FAILED_OFFSETS (8), got %u" % + len(failed_offsets_idxs) + ) + + degraded_idx = failed_offsets_idxs[0] + other_non_degraded_terminal = False + for i, status in enumerate(report_status): + if i == degraded_idx: + continue + if status is not None and status != MAG_CAL_FAILED_OFFSETS: + other_non_degraded_terminal = True + break + + if not other_non_degraded_terminal: + raise NotAchievedException( + "Expected at least one non-degraded compass terminal result" + ) + + finally: + self.set_parameters({ + "SIM_MAG1_OFS_X": old_sim_mag1_ofs_x, + "SIM_MAG1_OFS_Y": old_sim_mag1_ofs_y, + "SIM_MAG1_OFS_Z": old_sim_mag1_ofs_z, + }, add_to_context=False) + + self.check_zero_mag_parameters(params) + self.check_zeros_mag_orient() + ################################################# self.start_subtest("Try magcal and wait success") self.progress("Compass mask is %s" % "{0:b}".format(target_mask))