AP_Compass: added mag_cal_fixed_yaw()

this is a fast compass calibration that uses a yaw value provided by
the user.
This commit is contained in:
Andrew Tridgell 2019-11-21 14:53:43 +11:00
parent f7a8bcf87f
commit facedb5156
2 changed files with 129 additions and 0 deletions

View File

@ -325,6 +325,12 @@ public:
uint8_t get_filter_range() const { return uint8_t(_filter_range.get()); }
/*
fast compass calibration given vehicle position and yaw
*/
MAV_RESULT mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
float lat_deg, float lon_deg);
private:
static Compass *_singleton;
/// Register a new compas driver, allocating an instance number
@ -350,6 +356,12 @@ private:
// see if we already have probed a i2c driver by bus number and address
bool _have_i2c_driver(uint8_t bus_num, uint8_t address) const;
/*
get mag field with the effects of offsets, diagonals and
off-diagonals removed
*/
bool get_uncorrected_field(uint8_t instance, Vector3f &field);
#if COMPASS_CAL_ENABLED
//keep track of which calibrators have been saved
bool _cal_saved[COMPASS_MAX_INSTANCES];

View File

@ -2,6 +2,8 @@
#include <AP_Notify/AP_Notify.h>
#include <AP_GPS/AP_GPS.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_GPS/AP_GPS.h>
#include "AP_Compass.h"
@ -370,4 +372,119 @@ MAV_RESULT Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
return result;
}
/*
get mag field with the effects of offsets, diagonals and
off-diagonals removed
*/
bool Compass::get_uncorrected_field(uint8_t instance, Vector3f &field)
{
// form eliptical correction matrix and invert it. This is
// needed to remove the effects of the eliptical correction
// when calculating new offsets
const Vector3f &diagonals = get_diagonals(instance);
const Vector3f &offdiagonals = get_offdiagonals(instance);
Matrix3f mat {
diagonals.x, offdiagonals.x, offdiagonals.y,
offdiagonals.x, diagonals.y, offdiagonals.z,
offdiagonals.y, offdiagonals.z, diagonals.z
};
if (!mat.invert()) {
return false;
}
// get corrected field
field = get_field(instance);
// remove impact of diagonals and off-diagonals
field = mat * field;
// remove impact of offsets
field -= get_offsets(instance);
return true;
}
/*
fast compass calibration given vehicle position and yaw. This
results in zero diagonal and off-diagonal elements, so is only
suitable for vehicles where the field is close to spherical. It is
useful for large vehicles where moving the vehicle to calibrate it
is difficult.
The offsets of the selected compasses are set to values to bring
them into consistency with the WMM tables at the given latitude and
longitude. If compass_mask is zero then all enabled compasses are
calibrated.
This assumes that the compass is correctly scaled in milliGauss
*/
MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask,
float lat_deg, float lon_deg)
{
if (is_zero(lat_deg) && is_zero(lon_deg)) {
Location loc;
// get AHRS position. If unavailable then try GPS location
if (!AP::ahrs().get_position(loc)) {
if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) {
gcs().send_text(MAV_SEVERITY_ERROR, "Mag: no position available");
return MAV_RESULT_FAILED;
}
loc = AP::gps().location();
}
lat_deg = loc.lat * 1.0e-7;
lon_deg = loc.lng * 1.0e-7;
}
// get the magnetic field intensity and orientation
float intensity;
float declination;
float inclination;
if (!AP_Declination::get_mag_field_ef(lat_deg, lon_deg, intensity, declination, inclination)) {
gcs().send_text(MAV_SEVERITY_ERROR, "Mag: WMM table error");
return MAV_RESULT_FAILED;
}
// create a field vector and rotate to the required orientation
Vector3f field(1e3f * intensity, 0.0f, 0.0f);
Matrix3f R;
R.from_euler(0.0f, -ToRad(inclination), ToRad(declination));
field = R * field;
Matrix3f dcm;
dcm.from_euler(AP::ahrs().roll, AP::ahrs().pitch, radians(yaw_deg));
// Rotate into body frame using provided yaw
field = dcm.transposed() * field;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if (compass_mask != 0 && ((1U<<i) & compass_mask) == 0) {
// skip this compass
continue;
}
if (!use_for_yaw(i)) {
continue;
}
if (!healthy(i)) {
gcs().send_text(MAV_SEVERITY_ERROR, "Mag[%u]: unhealthy\n", i);
return MAV_RESULT_FAILED;
}
Vector3f measurement;
if (!get_uncorrected_field(i, measurement)) {
gcs().send_text(MAV_SEVERITY_ERROR, "Mag[%u]: bad uncorrected field", i);
return MAV_RESULT_FAILED;
}
Vector3f offsets = field - measurement;
set_and_save_offsets(i, offsets);
Vector3f one{1,1,1};
set_and_save_diagonals(i, one);
Vector3f zero{0,0,0};
set_and_save_offdiagonals(i, zero);
}
return MAV_RESULT_ACCEPTED;
}
#endif // COMPASS_CAL_ENABLED