Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_Compass/AP_Compass_Calibration.cpp

556 lines
17 KiB
C++
Raw Blame History

#include <AP_HAL/AP_HAL.h>
#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_InternalError/AP_InternalError.h>
#include "AP_Compass.h"
const extern AP_HAL::HAL& hal;
#if COMPASS_CAL_ENABLED
void Compass::cal_update()
{
if (hal.util->get_soft_armed()) {
return;
}
bool running = false;
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (_calibrator[i] == nullptr) {
continue;
}
if (_calibrator[i]->failed()) {
AP_Notify::events.compass_cal_failed = 1;
}
if (_calibrator[i]->running()) {
running = true;
} else if (_cal_autosave && !_cal_saved[i] && _calibrator[i]->get_state().status == CompassCalibrator::Status::SUCCESS) {
_accept_calibration(uint8_t(i));
}
}
AP_Notify::flags.compass_cal_running = running;
if (is_calibrating()) {
_cal_has_run = true;
return;
} else if (_cal_has_run && _auto_reboot()) {
hal.scheduler->delay(1000);
hal.scheduler->reboot(false);
}
}
bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
{
if (!healthy(i)) {
return false;
}
if (!use_for_yaw(i)) {
return false;
}
Priority prio = Priority(i);
#if COMPASS_MAX_INSTANCES > 1
if (_priority_did_list[prio] != _priority_did_stored_list[prio]) {
gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires reboot after priority change");
return false;
}
#endif
if (_calibrator[prio] == nullptr) {
_calibrator[prio] = new CompassCalibrator();
if (_calibrator[prio] == nullptr) {
gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal object not initialised");
return false;
}
}
if (_options.get() & uint16_t(Option::CAL_REQUIRE_GPS)) {
if (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) {
gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires GPS lock");
return false;
}
}
if (!is_calibrating()) {
AP_Notify::events.initiated_compass_cal = 1;
}
if (_rotate_auto) {
enum Rotation r = _get_state(prio).external?(enum Rotation)_get_state(prio).orientation.get():ROTATION_NONE;
if (r != ROTATION_CUSTOM) {
_calibrator[prio]->set_orientation(r, _get_state(prio).external, _rotate_auto>=2, _rotate_auto>=3);
}
}
_cal_saved[prio] = false;
if (i == 0 && _get_state(prio).external != 0) {
_calibrator[prio]->start(retry, delay, get_offsets_max(), i, _calibration_threshold);
} else {
// internal compasses or secondary compasses get twice the
// threshold. This is because internal compasses tend to be a
// lot noisier
_calibrator[prio]->start(retry, delay, get_offsets_max(), i, _calibration_threshold*2);
}
if (!_cal_thread_started) {
_cal_requires_reboot = true;
if (!hal.scheduler->thread_create(FUNCTOR_BIND(this, &Compass::_update_calibration_trampoline, void), "compasscal", 2048, AP_HAL::Scheduler::PRIORITY_IO, 0)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "CompassCalibrator: Cannot start compass thread.");
return false;
}
_cal_thread_started = true;
}
// disable compass learning both for calibration and after completion
_learn.set_and_save(0);
return true;
}
void Compass::_update_calibration_trampoline() {
while(true) {
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (_calibrator[i] == nullptr) {
continue;
}
_calibrator[i]->update();
}
hal.scheduler->delay(1);
}
}
bool Compass::_start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay, bool autoreboot)
{
_cal_autosave = autosave;
_compass_cal_autoreboot = autoreboot;
bool at_least_one_started = false;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if ((1<<i) & mask) {
if (!_start_calibration(i,retry,delay)) {
_cancel_calibration_mask(mask);
return false;
}
at_least_one_started = true;
}
}
return at_least_one_started;
}
bool Compass::start_calibration_all(bool retry, bool autosave, float delay, bool autoreboot)
{
_cal_autosave = autosave;
_compass_cal_autoreboot = autoreboot;
bool at_least_one_started = false;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
// ignore any compasses that fail to start calibrating
// start all should only calibrate compasses that are being used
if (_start_calibration(i,retry,delay)) {
at_least_one_started = true;
}
}
return at_least_one_started;
}
void Compass::_cancel_calibration(uint8_t i)
{
AP_Notify::events.initiated_compass_cal = 0;
Priority prio = Priority(i);
if (_calibrator[prio] == nullptr) {
return;
}
if (_calibrator[prio]->running() || _calibrator[prio]->get_state().status == CompassCalibrator::Status::WAITING_TO_START) {
AP_Notify::events.compass_cal_canceled = 1;
}
_cal_saved[prio] = false;
_calibrator[prio]->stop();
}
void Compass::_cancel_calibration_mask(uint8_t mask)
{
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if ((1<<i) & mask) {
_cancel_calibration(i);
}
}
}
void Compass::cancel_calibration_all()
{
_cancel_calibration_mask(0xFF);
}
bool Compass::_accept_calibration(uint8_t i)
{
Priority prio = Priority(i);
CompassCalibrator* cal = _calibrator[prio];
if (cal == nullptr) {
return false;
}
const CompassCalibrator::Report cal_report = cal->get_report();
if (_cal_saved[prio] || cal_report.status == CompassCalibrator::Status::NOT_STARTED) {
return true;
} else if (cal_report.status == CompassCalibrator::Status::SUCCESS) {
_cal_saved[prio] = true;
Vector3f ofs(cal_report.ofs), diag(cal_report.diag), offdiag(cal_report.offdiag);
float scale_factor = cal_report.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 (cal_report.check_orientation && _get_state(prio).external && _rotate_auto >= 2) {
set_and_save_orientation(i, cal_report.orientation);
}
if (!is_calibrating()) {
AP_Notify::events.compass_cal_saved = 1;
}
return true;
} else {
return false;
}
}
bool Compass::_accept_calibration_mask(uint8_t mask)
{
bool success = true;
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (_calibrator[i] == nullptr) {
continue;
}
if ((1<<uint8_t(i)) & mask) {
if (!_accept_calibration(uint8_t(i))) {
success = false;
}
_calibrator[i]->stop();
}
}
return success;
}
bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
{
const mavlink_channel_t chan = link.get_chan();
for (uint8_t i = 0; i < COMPASS_MAX_INSTANCES; i++) {
const Priority compass_id = (next_cal_progress_idx[chan] + 1) % COMPASS_MAX_INSTANCES;
auto& calibrator = _calibrator[compass_id];
if (calibrator == nullptr) {
next_cal_progress_idx[chan] = compass_id;
continue;
}
const CompassCalibrator::State cal_state = calibrator->get_state();
if (cal_state.status == CompassCalibrator::Status::WAITING_TO_START ||
cal_state.status == CompassCalibrator::Status::RUNNING_STEP_ONE ||
cal_state.status == CompassCalibrator::Status::RUNNING_STEP_TWO) {
// ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_PROGRESS)) {
return false;
}
next_cal_progress_idx[chan] = compass_id;
mavlink_msg_mag_cal_progress_send(
link.get_chan(),
uint8_t(compass_id),
_get_cal_mask(),
(uint8_t)cal_state.status, cal_state.attempt, cal_state.completion_pct, cal_state.completion_mask,
0.0f, 0.0f, 0.0f
);
} else {
next_cal_progress_idx[chan] = compass_id;
}
}
return true;
}
bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
{
const mavlink_channel_t chan = link.get_chan();
for (uint8_t i = 0; i < COMPASS_MAX_INSTANCES; i++) {
const Priority compass_id = (next_cal_report_idx[chan] + 1) % COMPASS_MAX_INSTANCES;
if (_calibrator[compass_id] == nullptr) {
next_cal_report_idx[chan] = compass_id;
continue;
}
const CompassCalibrator::Report cal_report = _calibrator[compass_id]->get_report();
if (cal_report.status == CompassCalibrator::Status::SUCCESS ||
cal_report.status == CompassCalibrator::Status::FAILED ||
cal_report.status == CompassCalibrator::Status::BAD_ORIENTATION) {
// ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_REPORT)) {
return false;
}
next_cal_report_idx[chan] = compass_id;
mavlink_msg_mag_cal_report_send(
link.get_chan(),
uint8_t(compass_id),
_get_cal_mask(),
(uint8_t)cal_report.status,
_cal_saved[compass_id],
cal_report.fitness,
cal_report.ofs.x, cal_report.ofs.y, cal_report.ofs.z,
cal_report.diag.x, cal_report.diag.y, cal_report.diag.z,
cal_report.offdiag.x, cal_report.offdiag.y, cal_report.offdiag.z,
cal_report.orientation_confidence,
cal_report.original_orientation,
cal_report.orientation,
cal_report.scale_factor
);
} else {
next_cal_report_idx[chan] = compass_id;
}
}
return true;
}
bool Compass::is_calibrating() const
{
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (_calibrator[i] == nullptr) {
continue;
}
switch(_calibrator[i]->get_state().status) {
case CompassCalibrator::Status::NOT_STARTED:
case CompassCalibrator::Status::SUCCESS:
case CompassCalibrator::Status::FAILED:
case CompassCalibrator::Status::BAD_ORIENTATION:
break;
default:
return true;
}
}
return false;
}
uint8_t Compass::_get_cal_mask()
{
uint8_t cal_mask = 0;
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (_calibrator[i] == nullptr) {
continue;
}
if (_calibrator[i]->get_state().status != CompassCalibrator::Status::NOT_STARTED) {
cal_mask |= 1 << uint8_t(i);
}
}
return cal_mask;
}
/*
handle an incoming MAG_CAL command
*/
MAV_RESULT Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
{
MAV_RESULT result = MAV_RESULT_FAILED;
switch (packet.command) {
case MAV_CMD_DO_START_MAG_CAL: {
result = MAV_RESULT_ACCEPTED;
if (hal.util->get_soft_armed()) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Disarm to allow compass calibration");
result = MAV_RESULT_FAILED;
break;
}
if (packet.param1 < 0 || packet.param1 > 255) {
result = MAV_RESULT_FAILED;
break;
}
uint8_t mag_mask = packet.param1;
bool retry = !is_zero(packet.param2);
bool autosave = !is_zero(packet.param3);
float delay = packet.param4;
bool autoreboot = !is_zero(packet.param5);
if (mag_mask == 0) { // 0 means all
_reset_compass_id();
if (!start_calibration_all(retry, autosave, delay, autoreboot)) {
result = MAV_RESULT_FAILED;
}
} else {
if (!_start_calibration_mask(mag_mask, retry, autosave, delay, autoreboot)) {
result = MAV_RESULT_FAILED;
}
}
break;
}
case MAV_CMD_DO_ACCEPT_MAG_CAL: {
result = MAV_RESULT_ACCEPTED;
if (packet.param1 < 0 || packet.param1 > 255) {
result = MAV_RESULT_FAILED;
break;
}
uint8_t mag_mask = packet.param1;
if (mag_mask == 0) { // 0 means all
mag_mask = 0xFF;
}
if (!_accept_calibration_mask(mag_mask)) {
result = MAV_RESULT_FAILED;
}
break;
}
case MAV_CMD_DO_CANCEL_MAG_CAL: {
result = MAV_RESULT_ACCEPTED;
if (packet.param1 < 0 || packet.param1 > 255) {
result = MAV_RESULT_FAILED;
break;
}
uint8_t mag_mask = packet.param1;
if (mag_mask == 0) { // 0 means all
cancel_calibration_all();
break;
}
_cancel_calibration_mask(mag_mask);
break;
}
}
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) const
{
// 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, bool force_use)
{
_reset_compass_id();
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 (!force_use && !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
Reference in New Issue View Git Blame Copy Permalink