From d15a4ad92a3d190a91cd1f428cb6861268900000 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 17 Jul 2018 16:11:28 +1000 Subject: [PATCH] AP_Compass: improved error reporting, check all compasses this uses extensions to the MAG_CAL_REPORT message to convey failures of orientation checking. It also checks all compasses, external or internal. It only tries to fix the orientation if it is external --- .../AP_Compass/AP_Compass_Calibration.cpp | 17 +++++--- libraries/AP_Compass/CompassCalibrator.cpp | 40 ++++++++++++++----- libraries/AP_Compass/CompassCalibrator.h | 11 ++++- 3 files changed, 50 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index ec07e7c79a..b1d49e1a02 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -61,11 +61,11 @@ Compass::_start_calibration(uint8_t i, bool retry, float delay) // lot noisier _calibrator[i].set_tolerance(_calibration_threshold*2); } - if (_state[i].external && _rotate_auto) { - _calibrator[i].set_orientation((enum Rotation)_state[i].orientation.get(), _state[i].external); + if (_rotate_auto) { + _calibrator[i].set_orientation(_state[i].external?(enum Rotation)_state[i].orientation.get():ROTATION_NONE, _state[i].external); } _cal_saved[i] = false; - _calibrator[i].start(retry, delay, get_offsets_max()); + _calibrator[i].start(retry, delay, get_offsets_max(), i); // disable compass learning both for calibration and after completion _learn.set_and_save(0); @@ -222,8 +222,9 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan) } uint8_t cal_status = _calibrator[compass_id].get_status(); - if ((cal_status == COMPASS_CAL_SUCCESS || - cal_status == COMPASS_CAL_FAILED)) { + if (cal_status == COMPASS_CAL_SUCCESS || + cal_status == COMPASS_CAL_FAILED || + cal_status == COMPASS_CAL_BAD_ORIENTATION) { float fitness = _calibrator[compass_id].get_fitness(); Vector3f ofs, diag, offdiag; _calibrator[compass_id].get_calibration(ofs, diag, offdiag); @@ -236,7 +237,10 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan) fitness, ofs.x, ofs.y, ofs.z, diag.x, diag.y, diag.z, - offdiag.x, offdiag.y, offdiag.z + offdiag.x, offdiag.y, offdiag.z, + _calibrator[compass_id].get_orientation_confidence(), + _calibrator[compass_id].get_original_orientation(), + _calibrator[compass_id].get_orientation() ); } } @@ -250,6 +254,7 @@ Compass::is_calibrating() const case COMPASS_CAL_NOT_STARTED: case COMPASS_CAL_SUCCESS: case COMPASS_CAL_FAILED: + case COMPASS_CAL_BAD_ORIENTATION: break; default: return true; diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 9e6e23f8a0..22eb95d618 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -80,7 +80,8 @@ void CompassCalibrator::clear() { set_status(COMPASS_CAL_NOT_STARTED); } -void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max) { +void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx) +{ if(running()) { return; } @@ -89,6 +90,7 @@ void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max) { _retry = retry; _delay_start_sec = delay; _start_time_ms = AP_HAL::millis(); + _compass_idx = compass_idx; set_status(COMPASS_CAL_WAITING_TO_START); } @@ -116,6 +118,7 @@ float CompassCalibrator::get_completion_percent() const { case COMPASS_CAL_SUCCESS: return 100.0f; case COMPASS_CAL_FAILED: + case COMPASS_CAL_BAD_ORIENTATION: default: return 0.0f; }; @@ -317,6 +320,13 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) { return true; case COMPASS_CAL_FAILED: + if (status == COMPASS_CAL_BAD_ORIENTATION) { + // don't overwrite bad orientation status + return false; + } + FALLTHROUGH; + + case COMPASS_CAL_BAD_ORIENTATION: if(_status == COMPASS_CAL_NOT_STARTED) { return false; } @@ -331,9 +341,9 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) { _sample_buffer = nullptr; } - _status = COMPASS_CAL_FAILED; + _status = status; return true; - + default: return false; }; @@ -752,7 +762,7 @@ Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Ro bool CompassCalibrator::calculate_orientation(void) { if (!_auto_orientation) { - // only calculate orientation for external compasses + // we are not checking orientation return true; } @@ -795,25 +805,29 @@ bool CompassCalibrator::calculate_orientation(void) } } } + + _orientation_confidence = second_best/bestv; + bool pass; if (besti == _orientation) { // if the orientation matched then allow for a low threshold pass = true; } else { - pass = (second_best / bestv) > 4; + pass = _orientation_confidence > 4; } if (!pass) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "Bad orientation: %u %.1f\n", besti, second_best/bestv); + gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence); } else if (besti == _orientation) { // no orientation change - gcs().send_text(MAV_SEVERITY_INFO, "Good orientation: %u %.1f\n", besti, second_best/bestv); + gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence); + } else if (!_is_external) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence); } else { - gcs().send_text(MAV_SEVERITY_INFO, "New orientation: %u was %u %.1f\n", besti, _orientation, second_best/bestv); + gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f\n", _compass_idx, besti, _orientation, _orientation_confidence); } if (!pass) { - // give an indication of orientation failure by showing a very high fitness - _fitness += 1000; + set_status(COMPASS_CAL_BAD_ORIENTATION); return false; } @@ -821,6 +835,12 @@ bool CompassCalibrator::calculate_orientation(void) // no orientation change return true; } + + if (!_is_external) { + // we can't change the orientation on internal compasses + set_status(COMPASS_CAL_BAD_ORIENTATION); + return false; + } // correct the offsets for the new orientation Vector3f rot_offsets = _params.offset; diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index c922d5b9c0..28a3ecaee9 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -15,7 +15,8 @@ enum compass_cal_status_t { COMPASS_CAL_RUNNING_STEP_ONE=2, COMPASS_CAL_RUNNING_STEP_TWO=3, COMPASS_CAL_SUCCESS=4, - COMPASS_CAL_FAILED=5 + COMPASS_CAL_FAILED=5, + COMPASS_CAL_BAD_ORIENTATION=6, }; class CompassCalibrator { @@ -24,7 +25,7 @@ public: CompassCalibrator(); - void start(bool retry, float delay, uint16_t offset_max); + void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx); void clear(); void update(bool &failure); @@ -37,6 +38,7 @@ public: void set_orientation(enum Rotation orientation, bool is_external) { _auto_orientation = true; _orientation = orientation; + _orig_orientation = orientation; _is_external = is_external; } @@ -44,11 +46,13 @@ public: void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals); enum Rotation get_orientation(void) { return _orientation; } + enum Rotation get_original_orientation(void) { return _orig_orientation; } float get_completion_percent() const; completion_mask_t& get_completion_mask(); enum compass_cal_status_t get_status() const { return _status; } float get_fitness() const { return sqrtf(_fitness); } + float get_orientation_confidence() const { return _orientation_confidence; } uint8_t get_attempt() const { return _attempt; } private: @@ -91,8 +95,10 @@ private: }; enum Rotation _orientation; + enum Rotation _orig_orientation; bool _is_external; bool _auto_orientation; + uint8_t _compass_idx; enum compass_cal_status_t _status; @@ -119,6 +125,7 @@ private: float _ellipsoid_lambda; uint16_t _samples_collected; uint16_t _samples_thinned; + float _orientation_confidence; bool set_status(compass_cal_status_t status);