From 196ba0e858b44a6f306fbec4f20773cca40b5a93 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 18 Jul 2018 10:09:21 +1000 Subject: [PATCH] AP_Compass: make COMPASS_ROT_AUTO take 3 values 0 for disabled, 1 for check only, 2 for check and fix --- libraries/AP_Compass/AP_Compass.cpp | 8 +-- .../AP_Compass/AP_Compass_Calibration.cpp | 4 +- libraries/AP_Compass/CompassCalibrator.cpp | 57 ++++++++++++------- libraries/AP_Compass/CompassCalibrator.h | 8 ++- 4 files changed, 49 insertions(+), 28 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 743e55bc8d..3870a5bd4d 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -445,10 +445,10 @@ const AP_Param::GroupInfo Compass::var_info[] = { AP_GROUPINFO("FLTR_RNG", 34, Compass, _filter_range, HAL_COMPASS_FILTER_DEFAULT), // @Param: ROT_AUTO - // @DisplayName: Automatically set orientation - // @Description: When enabled this will automatically set the orientation of external compasses on successful completion of compass calibration - // @Values: 0:Disabled,1:Enabled - AP_GROUPINFO("ROT_AUTO", 35, Compass, _rotate_auto, 1), + // @DisplayName: Automatically check orientation + // @Description: When enabled this will automatically check the orientation of compasses on successful completion of compass calibration. If set to 2 then external compasses will have their orientation automatically corrected. + // @Values: 0:Disabled,1:CheckOnly,2:CheckAndFix + AP_GROUPINFO("ROT_AUTO", 35, Compass, _rotate_auto, 2), AP_GROUPEND }; diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index b1d49e1a02..e2624b3e2b 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -62,7 +62,7 @@ Compass::_start_calibration(uint8_t i, bool retry, float delay) _calibrator[i].set_tolerance(_calibration_threshold*2); } if (_rotate_auto) { - _calibrator[i].set_orientation(_state[i].external?(enum Rotation)_state[i].orientation.get():ROTATION_NONE, _state[i].external); + _calibrator[i].set_orientation(_state[i].external?(enum Rotation)_state[i].orientation.get():ROTATION_NONE, _state[i].external, _rotate_auto>=2); } _cal_saved[i] = false; _calibrator[i].start(retry, delay, get_offsets_max(), i); @@ -150,7 +150,7 @@ Compass::_accept_calibration(uint8_t i) set_and_save_diagonals(i,diag); set_and_save_offdiagonals(i,offdiag); - if (_state[i].external && _rotate_auto) { + if (_state[i].external && _rotate_auto >= 2) { _state[i].orientation.set_and_save_ifchanged(cal.get_orientation()); } diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 22eb95d618..413a903b9a 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -320,7 +320,7 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) { return true; case COMPASS_CAL_FAILED: - if (status == COMPASS_CAL_BAD_ORIENTATION) { + if (_status == COMPASS_CAL_BAD_ORIENTATION) { // don't overwrite bad orientation status return false; } @@ -731,7 +731,15 @@ Matrix3f CompassCalibrator::AttitudeSample::get_rotmat(void) { } /* - calculate the implied earth field for a compass sample and compass rotation + calculate the implied earth field for a compass sample and compass + rotation. This is used to check for consistency between + samples. + + If the orientation is correct then when rotated the same (or + similar) earth field should be given for all samples. + + Note that this earth field uses an arbitrary north reference, so it + may not match the true earth field. */ Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Rotation r) { @@ -743,6 +751,7 @@ Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Ro // rotate to body frame for this rotation v.rotate(r); + // apply offsets, rotating them for the orientation we are testing Vector3f rot_offsets = _params.offset; rot_offsets.rotate_inverse(_orientation); @@ -750,23 +759,28 @@ Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Ro v += rot_offsets; + // rotate the sample from body frame back to earth frame Matrix3f rot = sample.att.get_rotmat(); Vector3f efield = rot * v; + + // earth field is the mag sample in earth frame return efield; } /* - calculate compass orientation using the attitude estimate associated with each sample + calculate compass orientation using the attitude estimate associated + with each sample, and fix orientation on external compasses if + the feature is enabled */ bool CompassCalibrator::calculate_orientation(void) { - if (!_auto_orientation) { + if (!_check_orientation) { // we are not checking orientation return true; } - float sum_error[ROTATION_MAX] {}; + float variance[ROTATION_MAX] {}; for (enum Rotation r = ROTATION_NONE; r 4; + pass = _orientation_confidence > variance_threshold; } if (!pass) { 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, "Mag(%u) good orientation: %u %.1f\n", _compass_idx, besti, _orientation_confidence); - } else if (!_is_external) { + } else if (!_is_external || !_fix_orientation) { 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, "Mag(%u) new orientation: %u was %u %.1f\n", _compass_idx, besti, _orientation, _orientation_confidence); @@ -836,8 +853,10 @@ bool CompassCalibrator::calculate_orientation(void) return true; } - if (!_is_external) { - // we can't change the orientation on internal compasses + if (!_is_external || !_fix_orientation) { + // we won't change the orientation, but we set _orientation + // for reporting purposes + _orientation = besti; set_status(COMPASS_CAL_BAD_ORIENTATION); return false; } diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index 28a3ecaee9..dadf00d564 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -35,11 +35,12 @@ public: bool running() const; - void set_orientation(enum Rotation orientation, bool is_external) { - _auto_orientation = true; + void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation) { + _check_orientation = true; _orientation = orientation; _orig_orientation = orientation; _is_external = is_external; + _fix_orientation = fix_orientation; } void set_tolerance(float tolerance) { _tolerance = tolerance; } @@ -97,7 +98,8 @@ private: enum Rotation _orientation; enum Rotation _orig_orientation; bool _is_external; - bool _auto_orientation; + bool _check_orientation; + bool _fix_orientation; uint8_t _compass_idx; enum compass_cal_status_t _status;