Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: added estimation of compass scale factor

Browse Source 
this adds new COMPASS_SCALE, COMPASS_SCALE2 and COMPASS_SCALE3
parameters, which give the sensor scaling factor. It is used to
compensate for an incorrect scaling in a compass.

The 3D compass calibration process will set the correct value
automatically, otherwise users can set the value to a known value for
an existing compass
This commit is contained in:
Andrew Tridgell 2019-11-27 09:11:54 +11:00
parent 7b203f6816
commit 6fd0ca36aa
8 changed files with 134 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_Compass/AP_Compass.cpp
View File

@ -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;

5
libraries/AP_Compass/AP_Compass.h
View File

@ -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 &

6
libraries/AP_Compass/AP_Compass_Backend.cpp
View File

@ -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,

10
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -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
);
}
}

3
libraries/AP_Compass/AP_Compass_SITL.cpp
View File

@ -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);

54
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -61,8 +61,12 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_GeodesicGrid.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_GPS/AP_GPS.h>
#include <GCS_MAVLink/GCS.h>
#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;
}

10
libraries/AP_Compass/CompassCalibrator.h
View File

@ -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

8
libraries/AP_Compass/Compass_learn.cpp
View File

@ -19,6 +19,13 @@ CompassLearn::CompassLearn(Compass &_compass) :
compass(_compass)
{
gcs().send_text(MAV_SEVERITY_INFO, "CompassLearn: Initialised");
for (uint8_t i=0; i<compass.get_count(); i++) {
if (compass._state[i].use_for_yaw) {
// reset scale factors, we can't learn scale factors in
// flight
compass.set_and_save_scale_factor(i, 0.0);
}
}
}
/*
@ -133,6 +140,7 @@ void CompassLearn::update(void)
for (uint8_t i=0; i<compass.get_count(); i++) {
if (compass._state[i].use_for_yaw) {
compass.save_offsets(i);
compass.set_and_save_scale_factor(i, 0.0);
compass.set_use_for_yaw(i, true);
}
}