From dc85169cb1582d136c4632f5078da21f2578b775 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 5 Oct 2022 13:07:18 +1100 Subject: [PATCH] AP_Compass: send report if we are in the BAD_RADIUS state --- .../AP_Compass/AP_Compass_Calibration.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index 2578e8c101..f52c69c2f7 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -291,10 +291,18 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link) continue; } const CompassCalibrator::Report cal_report = _calibrator[compass_id]->get_report(); - if (cal_report.status == CompassCalibrator::Status::SUCCESS || - cal_report.status == CompassCalibrator::Status::FAILED || - cal_report.status == CompassCalibrator::Status::BAD_ORIENTATION) { - + switch (cal_report.status) { + case CompassCalibrator::Status::NOT_STARTED: + case CompassCalibrator::Status::WAITING_TO_START: + case CompassCalibrator::Status::RUNNING_STEP_ONE: + case CompassCalibrator::Status::RUNNING_STEP_TWO: + // calibration has not finished ergo no report + next_cal_report_idx[chan] = compass_id; + continue; + case CompassCalibrator::Status::SUCCESS: + case CompassCalibrator::Status::FAILED: + case CompassCalibrator::Status::BAD_ORIENTATION: + case CompassCalibrator::Status::BAD_RADIUS: // ensure we don't try to send with no space available if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_REPORT)) { return false; @@ -317,8 +325,6 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link) cal_report.orientation, cal_report.scale_factor ); - } else { - next_cal_report_idx[chan] = compass_id; } } return true;