Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added support for fixed yaw mag calibration #12863

Open
wants to merge 3 commits into
base: master
from
Open
Changes from all commits
Commits
File filter...
Filter file types
Jump to…
Jump to file or symbol
Failed to load files and symbols.

Always

Just for now

@@ -321,6 +321,12 @@ friend class AP_Compass_Backend;

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
@@ -346,6 +352,12 @@ friend class AP_Compass_Backend;
// 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];
@@ -1,6 +1,8 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Notify/AP_Notify.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_GPS/AP_GPS.h>

#include "AP_Compass.h"

@@ -371,4 +373,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;
This conversation was marked as resolved by tridge

This comment has been minimized.

Copy link
@amilcarlucas

amilcarlucas Nov 21, 2019

Contributor

should this be * 1.0e-7f ?

This comment has been minimized.

Copy link
@tridge

tridge Nov 22, 2019

Author Contributor

it doesn't matter, as we tell the compiler to treat all float constants as float

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
@@ -437,6 +437,8 @@ class GCS_MAVLINK

void handle_optical_flow(const mavlink_message_t &msg);

MAV_RESULT handle_fixed_mag_cal_yaw(const mavlink_command_long_t &packet);

// vehicle-overridable message send function
virtual bool try_send_message(enum ap_message id);
virtual void send_global_position_int();
@@ -3004,6 +3004,19 @@ void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg)
optflow->handle_msg(msg);
}


/*
handle MAV_CMD_FIXED_MAG_CAL_YAW
*/
MAV_RESULT GCS_MAVLINK::handle_fixed_mag_cal_yaw(const mavlink_command_long_t &packet)
{
Compass &compass = AP::compass();
This conversation was marked as resolved by rmackay9

This comment has been minimized.

Copy link
@rmackay9

rmackay9 Dec 2, 2019

Contributor

shouldn't we check if this is nullptr?

This comment has been minimized.

Copy link
@tridge

tridge Dec 5, 2019

Author Contributor

it's a reference, impossible to be null

return compass.mag_cal_fixed_yaw(packet.param1,
uint8_t(packet.param2),
packet.param3,
packet.param4);
}

/*
handle messages which don't require vehicle specific data
*/
@@ -3725,6 +3738,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t

return MAV_RESULT_UNSUPPORTED;

case MAV_CMD_FIXED_MAG_CAL_YAW:

This comment has been minimized.

Copy link
@rmackay9

rmackay9 Dec 2, 2019

Contributor

I guess this command could also arrive as part of a COMMAND_INT?

result = handle_fixed_mag_cal_yaw(packet);
break;

default:
result = MAV_RESULT_UNSUPPORTED;
break;
ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.