Skip to content

AP_Compass: report specific failure reason when fit rejected #32757

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
christianpetri wants to merge 1 commit into
base: master
Choose a base branch
from
+193 −51
Open

AP_Compass: report specific failure reason when fit rejected #32757

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
95 changes: 86 additions & 9 deletions Tools/autotest/vehicle_test_suite.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10368,12 +10368,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:
Expand Down Expand Up @@ -10405,7 +10404,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)
Expand All @@ -10420,17 +10420,20 @@ 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")
m = self.assert_receive_message(["MAG_CAL_PROGRESS", "MAG_CAL_REPORT"], timeout=10)
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
Expand All @@ -10449,6 +10452,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))
Expand Down
14 changes: 10 additions & 4 deletions libraries/AP_Compass/AP_Compass_Calibration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
120 changes: 87 additions & 33 deletions libraries/AP_Compass/CompassCalibrator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 /////////////////////
////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -181,7 +190,7 @@ void CompassCalibrator::update()
{
WITH_SEMAPHORE(state_sem);
//update_settings
if (!running()) {
if (!_running() && !_retry_pending) {
update_cal_settings();
}

Expand All @@ -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
Expand All @@ -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) {
Expand Down Expand Up @@ -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;
};
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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;
}

Expand All @@ -469,30 +510,38 @@ bool CompassCalibrator::set_status(CompassCalibrator::Status status)
_sample_buffer = nullptr;
}

_status = status;
return true;
};

// compiler guarantees we don't get here
return false;
}

bool CompassCalibrator::fit_acceptable() const
Comment thread
christianpetri marked this conversation as resolved.
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()
Expand Down Expand Up @@ -1025,7 +1074,7 @@ bool CompassCalibrator::calculate_orientation(void)
}

if (!pass) {
set_status(Status::BAD_ORIENTATION);
set_status(Status::FAILED_ORIENTATION);
return false;
}

Expand All @@ -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;
}

Expand All @@ -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;
}

/*
Expand Down Expand Up @@ -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;
}

Expand Down
Loading
Loading