diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 16711215b0..2b471a7032 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -509,6 +509,31 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Values: 0:Disabled,1:Enabled AP_GROUPINFO("ENABLE", 39, Compass, _enabled, 1), + // @Param: SCALE + // @DisplayName: Compass1 scale factor + // @Description: Scaling factor for first compass to compensate for sensor scaling errors. If this is 0 then no scaling is done + // @User: Standard + // @Range: 0 1.3 + AP_GROUPINFO("SCALE", 40, Compass, _state[0].scale_factor, 0), + +#if HAL_COMPASS_MAX_SENSORS > 1 + // @Param: SCALE2 + // @DisplayName: Compass2 scale factor + // @Description: Scaling factor for 2nd compass to compensate for sensor scaling errors. If this is 0 then no scaling is done + // @User: Standard + // @Range: 0 1.3 + AP_GROUPINFO("SCALE2", 41, Compass, _state[1].scale_factor, 0), +#endif + +#if HAL_COMPASS_MAX_SENSORS > 2 + // @Param: SCALE3 + // @DisplayName: Compass3 scale factor + // @Description: Scaling factor for 3rd compass to compensate for sensor scaling errors. If this is 0 then no scaling is done + // @User: Standard + // @Range: 0 1.3 + AP_GROUPINFO("SCALE3", 42, Compass, _state[2].scale_factor, 0), +#endif + AP_GROUPEND }; @@ -1007,6 +1032,14 @@ Compass::set_and_save_offdiagonals(uint8_t i, const Vector3f &offdiagonals) } } +void +Compass::set_and_save_scale_factor(uint8_t i, float scale_factor) +{ + if (i < COMPASS_MAX_INSTANCES) { + _state[i].scale_factor.set_and_save(scale_factor); + } +} + void Compass::save_offsets(uint8_t i) { @@ -1309,6 +1342,19 @@ bool Compass::consistent() const return true; } +/* + return true if we have a valid scale factor + */ +bool Compass::have_scale_factor(uint8_t i) const +{ + if (i >= get_count() || + _state[i].scale_factor < COMPASS_MIN_SCALE_FACTOR || + _state[i].scale_factor > COMPASS_MAX_SCALE_FACTOR) { + return false; + } + return true; +} + // singleton instance Compass *Compass::_singleton; diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 7c8684ca08..8d2218b316 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -106,6 +106,7 @@ public: void set_and_save_offsets(uint8_t i, const Vector3f &offsets); void set_and_save_diagonals(uint8_t i, const Vector3f &diagonals); void set_and_save_offdiagonals(uint8_t i, const Vector3f &diagonals); + void set_and_save_scale_factor(uint8_t i, float scale_factor); /// Saves the current offset x/y/z values for one or all compasses /// @@ -124,6 +125,9 @@ public: const Vector3f &get_field(uint8_t i) const { return _state[i].field; } const Vector3f &get_field(void) const { return get_field(get_primary()); } + /// Return true if we have set a scale factor for a compass + bool have_scale_factor(uint8_t i) const; + // compass calibrator interface void cal_update(); @@ -431,6 +435,7 @@ private: AP_Vector3f offset; AP_Vector3f diagonals; AP_Vector3f offdiagonals; + AP_Float scale_factor; // device id detected at init. // saved to eeprom when offsets are saved allowing ram & diff --git a/libraries/AP_Compass/AP_Compass_Backend.cpp b/libraries/AP_Compass/AP_Compass_Backend.cpp index 8fbcbf4318..afec18b51f 100644 --- a/libraries/AP_Compass/AP_Compass_Backend.cpp +++ b/libraries/AP_Compass/AP_Compass_Backend.cpp @@ -59,6 +59,12 @@ void AP_Compass_Backend::correct_field(Vector3f &mag, uint8_t i) // add in the basic offsets mag += offsets; + // add in scale factor, use a wide sanity check. The calibrator + // uses a narrower check. + if (_compass.have_scale_factor(i)) { + mag *= state.scale_factor; + } + // apply eliptical correction Matrix3f mat( diagonals.x, offdiagonals.x, offdiagonals.y, diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index e22a769df0..add1babc62 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -145,11 +145,13 @@ bool Compass::_accept_calibration(uint8_t i) _cal_saved[i] = true; Vector3f ofs, diag, offdiag; - cal.get_calibration(ofs, diag, offdiag); + float scale_factor; + cal.get_calibration(ofs, diag, offdiag, scale_factor); set_and_save_offsets(i, ofs); set_and_save_diagonals(i,diag); set_and_save_offdiagonals(i,offdiag); + set_and_save_scale_factor(i,scale_factor); if (_state[i].external && _rotate_auto >= 2) { _state[i].orientation.set_and_save_ifchanged(cal.get_orientation()); @@ -234,7 +236,8 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link) 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); + float scale_factor; + _calibrator[compass_id].get_calibration(ofs, diag, offdiag, scale_factor); uint8_t autosaved = _cal_saved[compass_id]; mavlink_msg_mag_cal_report_send( @@ -247,7 +250,8 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link) offdiag.x, offdiag.y, offdiag.z, _calibrator[compass_id].get_orientation_confidence(), _calibrator[compass_id].get_original_orientation(), - _calibrator[compass_id].get_orientation() + _calibrator[compass_id].get_orientation(), + scale_factor ); } } diff --git a/libraries/AP_Compass/AP_Compass_SITL.cpp b/libraries/AP_Compass/AP_Compass_SITL.cpp index d4e2410f44..a9790b2e12 100644 --- a/libraries/AP_Compass/AP_Compass_SITL.cpp +++ b/libraries/AP_Compass/AP_Compass_SITL.cpp @@ -115,6 +115,9 @@ void AP_Compass_SITL::_timer() // rotate the first compass, allowing for testing of external compass rotation f.rotate_inverse((enum Rotation)_sitl->mag_orient.get()); f.rotate(get_board_orientation()); + + // scale the first compass to simulate sensor scale factor errors + f *= _sitl->mag_scaling; } accumulate_sample(f, _compass_instance[i], 10); diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 3ea266f509..fdd9995c1e 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -61,8 +61,12 @@ #include #include #include +#include #include +#define FIELD_RADIUS_MIN 150 +#define FIELD_RADIUS_MAX 950 + extern const AP_HAL::HAL& hal; //////////////////////////////////////////////////////////// @@ -104,7 +108,7 @@ void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint set_status(COMPASS_CAL_WAITING_TO_START); } -void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals) +void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals, float &scale_factor) { if (_status != COMPASS_CAL_SUCCESS) { return; @@ -113,6 +117,7 @@ void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, offsets = _params.offset; diagonals = _params.diag; offdiagonals = _params.offdiag; + scale_factor = _params.scale_factor; } float CompassCalibrator::get_completion_percent() const @@ -131,6 +136,7 @@ float CompassCalibrator::get_completion_percent() const return 100.0f; case COMPASS_CAL_FAILED: case COMPASS_CAL_BAD_ORIENTATION: + case COMPASS_CAL_BAD_RADIUS: default: return 0.0f; }; @@ -215,7 +221,7 @@ void CompassCalibrator::update(bool &failure) } } else if (_status == COMPASS_CAL_RUNNING_STEP_TWO) { if (_fit_step >= 35) { - if (fit_acceptable() && calculate_orientation()) { + if (fit_acceptable() && fix_radius() && calculate_orientation()) { set_status(COMPASS_CAL_SUCCESS); } else { set_status(COMPASS_CAL_FAILED); @@ -266,6 +272,7 @@ void CompassCalibrator::reset_state() _params.offset.zero(); _params.diag = Vector3f(1.0f,1.0f,1.0f); _params.offdiag.zero(); + _params.scale_factor = 0; memset(_completion_mask, 0, sizeof(_completion_mask)); initialize_fit(); @@ -336,13 +343,15 @@ 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 || + _status == COMPASS_CAL_BAD_RADIUS) { // don't overwrite bad orientation status return false; } FALLTHROUGH; case COMPASS_CAL_BAD_ORIENTATION: + case COMPASS_CAL_BAD_RADIUS: if (_status == COMPASS_CAL_NOT_STARTED) { return false; } @@ -368,7 +377,7 @@ bool CompassCalibrator::set_status(compass_cal_status_t status) bool CompassCalibrator::fit_acceptable() { if (!isnan(_fitness) && - _params.radius > 150 && _params.radius < 950 && //Earth's magnetic field strength range: 250-850mG + _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 && @@ -923,3 +932,40 @@ bool CompassCalibrator::calculate_orientation(void) return fit_acceptable(); } + +/* + fix radius of the fit to compensate for sensor scale factor errors + return false if radius is outside acceptable range + */ +bool CompassCalibrator::fix_radius(void) +{ + if (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) { + // we don't have a position, leave scale factor as 0. This + // will disable use of WMM in the EKF. Users can manually set + // scale factor after calibration if it is known + _params.scale_factor = 0; + return true; + } + const struct Location &loc = AP::gps().location(); + float intensity; + float declination; + float inclination; + AP_Declination::get_mag_field_ef(loc.lat * 1e-7f, loc.lng * 1e-7f, intensity, declination, inclination); + + float expected_radius = intensity * 1000; // mGauss + float correction = expected_radius / _params.radius; + + if (correction > COMPASS_MAX_SCALE_FACTOR || correction < COMPASS_MIN_SCALE_FACTOR) { + // don't allow more than 30% scale factor correction + gcs().send_text(MAV_SEVERITY_ERROR, "Mag(%u) bad radius %.0f expected %.0f", + _compass_idx, + _params.radius, + expected_radius); + set_status(COMPASS_CAL_BAD_RADIUS); + return false; + } + + _params.scale_factor = correction; + + return true; +} diff --git a/libraries/AP_Compass/CompassCalibrator.h b/libraries/AP_Compass/CompassCalibrator.h index 45454fbe5e..b8c768bbbb 100644 --- a/libraries/AP_Compass/CompassCalibrator.h +++ b/libraries/AP_Compass/CompassCalibrator.h @@ -16,8 +16,12 @@ enum compass_cal_status_t { COMPASS_CAL_SUCCESS = 4, COMPASS_CAL_FAILED = 5, COMPASS_CAL_BAD_ORIENTATION = 6, + COMPASS_CAL_BAD_RADIUS = 7, }; +#define COMPASS_MIN_SCALE_FACTOR 0.3 +#define COMPASS_MAX_SCALE_FACTOR 1.6 + class CompassCalibrator { public: CompassCalibrator(); @@ -45,7 +49,7 @@ public: enum compass_cal_status_t get_status() const { return _status; } // get calibration outputs (offsets, diagonals, offdiagonals) and fitness - void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals); + void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals, float &scale_factor); float get_fitness() const { return sqrtf(_fitness); } // get corrected (and original) orientation @@ -80,6 +84,7 @@ private: Vector3f offset; // offsets Vector3f diag; // diagonal scaling Vector3f offdiag; // off diagonal scaling + float scale_factor; // scaling factor to compensate for radius error }; // compact class for approximate attitude, to save memory @@ -155,6 +160,9 @@ private: Vector3f calculate_earth_field(CompassSample &sample, enum Rotation r); bool calculate_orientation(); + // fix radius to compensate for sensor scaling errors + bool fix_radius(); + uint8_t _compass_idx; // index of the compass providing data enum compass_cal_status_t _status; // current state of calibrator uint32_t _last_sample_ms; // system time of last sample received for timeout diff --git a/libraries/AP_Compass/Compass_learn.cpp b/libraries/AP_Compass/Compass_learn.cpp index d0b7122b7c..9498b17fb6 100644 --- a/libraries/AP_Compass/Compass_learn.cpp +++ b/libraries/AP_Compass/Compass_learn.cpp @@ -19,6 +19,13 @@ CompassLearn::CompassLearn(Compass &_compass) : compass(_compass) { gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Initialised"); + for (uint8_t i=0; i