mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: add calibration interface
This commit is contained in:
parent
28ee63c855
commit
cc36401b7c
|
@ -0,0 +1,232 @@
|
||||||
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
|
#include <AP_HAL.h>
|
||||||
|
#include "Compass.h"
|
||||||
|
#include <AP_Notify.h>
|
||||||
|
|
||||||
|
void
|
||||||
|
Compass::compass_cal_update()
|
||||||
|
{
|
||||||
|
AP_Notify::flags.compass_cal_running = 0;
|
||||||
|
|
||||||
|
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
|
_calibrator[i].run_fit_chunk();
|
||||||
|
|
||||||
|
if(_calibrator[i].check_for_timeout()) {
|
||||||
|
cancel_calibration_all();
|
||||||
|
}
|
||||||
|
|
||||||
|
if(_calibrator[i].running()) {
|
||||||
|
AP_Notify::flags.compass_cal_running = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool
|
||||||
|
Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay)
|
||||||
|
{
|
||||||
|
if(healthy(i)) {
|
||||||
|
if(!is_calibrating() && delay > 0.5f) {
|
||||||
|
AP_Notify::events.initiated_compass_cal = 1;
|
||||||
|
}
|
||||||
|
if(i == get_primary()) {
|
||||||
|
_calibrator[i].set_tolerance(5);
|
||||||
|
} else {
|
||||||
|
_calibrator[i].set_tolerance(10);
|
||||||
|
}
|
||||||
|
_calibrator[i].start(retry, autosave, delay);
|
||||||
|
return true;
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool
|
||||||
|
Compass::start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay)
|
||||||
|
{
|
||||||
|
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
|
if((1<<i) & mask) {
|
||||||
|
if(!start_calibration(i,retry,autosave,delay)) {
|
||||||
|
cancel_calibration_mask(mask);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool
|
||||||
|
Compass::start_calibration_all(bool retry, bool autosave, float delay)
|
||||||
|
{
|
||||||
|
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
|
if(healthy(i) && use_for_yaw(i)) {
|
||||||
|
if(!start_calibration(i,retry,autosave,delay)) {
|
||||||
|
cancel_calibration_all();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
Compass::cancel_calibration(uint8_t i)
|
||||||
|
{
|
||||||
|
_calibrator[i].clear();
|
||||||
|
AP_Notify::events.compass_cal_canceled = 1;
|
||||||
|
AP_Notify::events.initiated_compass_cal = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
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)
|
||||||
|
{
|
||||||
|
CompassCalibrator& cal = _calibrator[i];
|
||||||
|
uint8_t cal_status = cal.get_status();
|
||||||
|
|
||||||
|
if(cal_status == COMPASS_CAL_SUCCESS) {
|
||||||
|
Vector3f ofs, diag, offdiag;
|
||||||
|
cal.get_calibration(ofs, diag, offdiag);
|
||||||
|
cal.clear();
|
||||||
|
|
||||||
|
set_and_save_offsets(i, ofs);
|
||||||
|
set_and_save_diagonals(i,diag);
|
||||||
|
set_and_save_offdiagonals(i,offdiag);
|
||||||
|
|
||||||
|
if(!is_calibrating()) {
|
||||||
|
AP_Notify::events.compass_cal_saved = 1;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool
|
||||||
|
Compass::accept_calibration_mask(uint8_t mask)
|
||||||
|
{
|
||||||
|
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
|
if((1<<i) & mask) {
|
||||||
|
CompassCalibrator& cal = _calibrator[i];
|
||||||
|
uint8_t cal_status = cal.get_status();
|
||||||
|
if(cal_status != COMPASS_CAL_SUCCESS && cal_status != COMPASS_CAL_NOT_STARTED) {
|
||||||
|
// a compass failed or is still in progress
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool success = true;
|
||||||
|
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
|
if((1<<i) & mask) {
|
||||||
|
if(!accept_calibration(i)) {
|
||||||
|
success = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return success;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool
|
||||||
|
Compass::accept_calibration_all()
|
||||||
|
{
|
||||||
|
return accept_calibration_mask(0xFF);
|
||||||
|
}
|
||||||
|
|
||||||
|
void
|
||||||
|
Compass::send_mag_cal_progress(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
uint8_t cal_mask = get_cal_mask();
|
||||||
|
|
||||||
|
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
|
CompassCalibrator& cal = _calibrator[i];
|
||||||
|
|
||||||
|
uint8_t& compass_id = i;
|
||||||
|
uint8_t cal_status = cal.get_status();
|
||||||
|
|
||||||
|
if(cal_status == COMPASS_CAL_WAITING_TO_START ||
|
||||||
|
cal_status == COMPASS_CAL_RUNNING_STEP_ONE ||
|
||||||
|
cal_status == COMPASS_CAL_RUNNING_STEP_TWO) {
|
||||||
|
uint8_t completion_pct = cal.get_completion_percent();
|
||||||
|
uint8_t completion_mask[10];
|
||||||
|
Vector3f direction(0.0f,0.0f,0.0f);
|
||||||
|
uint8_t attempt = cal.get_attempt();
|
||||||
|
|
||||||
|
memset(completion_mask, 0, sizeof(completion_mask));
|
||||||
|
|
||||||
|
mavlink_msg_mag_cal_progress_send(
|
||||||
|
chan,
|
||||||
|
compass_id, cal_mask,
|
||||||
|
cal_status, attempt, completion_pct, completion_mask,
|
||||||
|
direction.x, direction.y, direction.z
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Compass::send_mag_cal_report(mavlink_channel_t chan)
|
||||||
|
{
|
||||||
|
uint8_t cal_mask = get_cal_mask();
|
||||||
|
|
||||||
|
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
|
CompassCalibrator& cal = _calibrator[i];
|
||||||
|
|
||||||
|
uint8_t& compass_id = i;
|
||||||
|
uint8_t cal_status = cal.get_status();
|
||||||
|
|
||||||
|
if(cal_status == COMPASS_CAL_SUCCESS ||
|
||||||
|
cal_status == COMPASS_CAL_FAILED) {
|
||||||
|
float fitness = cal.get_fitness();
|
||||||
|
Vector3f ofs, diag, offdiag;
|
||||||
|
cal.get_calibration(ofs, diag, offdiag);
|
||||||
|
uint8_t autosaved = cal.get_autosave();
|
||||||
|
|
||||||
|
mavlink_msg_mag_cal_report_send(
|
||||||
|
chan,
|
||||||
|
compass_id, cal_mask,
|
||||||
|
cal_status, autosaved,
|
||||||
|
fitness,
|
||||||
|
ofs.x, ofs.y, ofs.z,
|
||||||
|
diag.x, diag.y, diag.z,
|
||||||
|
offdiag.x, offdiag.y, offdiag.z
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(cal_status == COMPASS_CAL_SUCCESS && cal.get_autosave()) {
|
||||||
|
accept_calibration(i);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool
|
||||||
|
Compass::is_calibrating() const
|
||||||
|
{
|
||||||
|
return get_cal_mask();
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t
|
||||||
|
Compass::get_cal_mask() const
|
||||||
|
{
|
||||||
|
uint8_t cal_mask = 0;
|
||||||
|
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
|
if(_calibrator[i].get_status() != COMPASS_CAL_NOT_STARTED) {
|
||||||
|
cal_mask |= 1 << i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return cal_mask;
|
||||||
|
}
|
|
@ -486,6 +486,18 @@ Compass::read(void)
|
||||||
return healthy();
|
return healthy();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
uint8_t
|
||||||
|
Compass::get_healthy_mask() const
|
||||||
|
{
|
||||||
|
uint8_t healthy_mask = 0;
|
||||||
|
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
|
if(healthy(i)) {
|
||||||
|
healthy_mask |= 1 << i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return healthy_mask;
|
||||||
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
Compass::set_offsets(uint8_t i, const Vector3f &offsets)
|
Compass::set_offsets(uint8_t i, const Vector3f &offsets)
|
||||||
{
|
{
|
||||||
|
|
|
@ -3,11 +3,13 @@
|
||||||
#define Compass_h
|
#define Compass_h
|
||||||
|
|
||||||
#include <inttypes.h>
|
#include <inttypes.h>
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_Common.h>
|
||||||
#include <AP_Param/AP_Param.h>
|
#include <AP_Param.h>
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math.h>
|
||||||
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
|
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL.h>
|
||||||
|
#include <GCS_MAVLink.h>
|
||||||
|
#include "CompassCalibrator.h"
|
||||||
#include "AP_Compass_Backend.h"
|
#include "AP_Compass_Backend.h"
|
||||||
|
|
||||||
// compass product id
|
// compass product id
|
||||||
|
@ -109,8 +111,6 @@ public:
|
||||||
const Vector3f &get_field(void) const { return get_field(get_primary()); }
|
const Vector3f &get_field(void) const { return get_field(get_primary()); }
|
||||||
|
|
||||||
// raw/unfiltered measurement interface
|
// raw/unfiltered measurement interface
|
||||||
|
|
||||||
|
|
||||||
uint32_t raw_meas_time_us(uint8_t i) const { return _state[i].raw_meas_time_us; }
|
uint32_t raw_meas_time_us(uint8_t i) const { return _state[i].raw_meas_time_us; }
|
||||||
uint32_t raw_meas_time_us() const { return _state[get_primary()].raw_meas_time_us; }
|
uint32_t raw_meas_time_us() const { return _state[get_primary()].raw_meas_time_us; }
|
||||||
uint32_t unfiltered_meas_time_us(uint8_t i) const { return _state[i].raw_meas_time_us; }
|
uint32_t unfiltered_meas_time_us(uint8_t i) const { return _state[i].raw_meas_time_us; }
|
||||||
|
@ -128,9 +128,31 @@ public:
|
||||||
const Vector3f &get_unfiltered_field(uint8_t i) const { return _state[i].unfiltered_field; }
|
const Vector3f &get_unfiltered_field(uint8_t i) const { return _state[i].unfiltered_field; }
|
||||||
const Vector3f &get_unfiltered_field(void) const { return get_unfiltered_field(get_primary()); }
|
const Vector3f &get_unfiltered_field(void) const { return get_unfiltered_field(get_primary()); }
|
||||||
|
|
||||||
|
// compass calibrator interface
|
||||||
|
void compass_cal_update();
|
||||||
|
|
||||||
|
bool start_calibration(uint8_t i, bool retry=false, bool autosave=false, float delay_sec=0.0f);
|
||||||
|
bool start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f);
|
||||||
|
bool start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f);
|
||||||
|
|
||||||
|
void cancel_calibration(uint8_t i);
|
||||||
|
void cancel_calibration_all();
|
||||||
|
void cancel_calibration_mask(uint8_t mask);
|
||||||
|
|
||||||
|
bool accept_calibration(uint8_t i);
|
||||||
|
bool accept_calibration_all();
|
||||||
|
bool accept_calibration_mask(uint8_t mask);
|
||||||
|
|
||||||
|
uint8_t get_cal_mask() const;
|
||||||
|
bool is_calibrating() const;
|
||||||
|
|
||||||
|
void send_mag_cal_progress(mavlink_channel_t chan);
|
||||||
|
void send_mag_cal_report(mavlink_channel_t chan);
|
||||||
|
|
||||||
/// Return the health of a compass
|
/// Return the health of a compass
|
||||||
bool healthy(uint8_t i) const { return _state[i].healthy; }
|
bool healthy(uint8_t i) const { return _state[i].healthy; }
|
||||||
bool healthy(void) const { return healthy(get_primary()); }
|
bool healthy(void) const { return healthy(get_primary()); }
|
||||||
|
uint8_t get_healthy_mask() const;
|
||||||
|
|
||||||
/// Returns the current offset values
|
/// Returns the current offset values
|
||||||
///
|
///
|
||||||
|
@ -353,6 +375,8 @@ private:
|
||||||
Vector3f unfiltered_field;
|
Vector3f unfiltered_field;
|
||||||
} _state[COMPASS_MAX_INSTANCES];
|
} _state[COMPASS_MAX_INSTANCES];
|
||||||
|
|
||||||
|
CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];
|
||||||
|
|
||||||
// if we want HIL only
|
// if we want HIL only
|
||||||
bool _hil_mode:1;
|
bool _hil_mode:1;
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue