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

Compass: Rework compass calibrator

Browse Source 
Summary of significant changes:
  -Autsave doesn't depend on STREAM_EXTRA3
  -Don't risk only saving one compass on copter if CAL_ALWAYS_REBOOT is set
  -Only calibrate compasses that are both health and marked for use (there was a inconsistency in handling the mask)
  -Fix incorrect failure reporting on DO_ACCEPT_MAG_CAL with a mask of 0 if a channel was specifically not started
  -Fix not starting the buzzer if the delay is set to 0 seconds
  -Always send MAG_CAL_REPORT until its acknowledged
  -Correct the field in MAG_CAL_REPORT for autosave to indicate if the compass had actually been saved, rather then being scheduled to be saved
  -Remmove unused public interfaces
This commit is contained in:
Michael du Breuil 2016-10-24 22:16:11 -07:00 committed by Randy Mackay
parent 8a5d9a87d6
commit 4d8053afc9
5 changed files with 90 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Compass/AP_Compass.cpp
View File

@ -383,7 +383,6 @@ Compass::Compass(void) :
for (uint8_t i=0; i<COMPASS_MAX_BACKEND; i++) {
_backends[i] = NULL;
_state[i].last_update_usec = 0;
_reports_sent[i] = 0;
}
// default device ids to zero. init() method will overwrite with the actual device ids

31
libraries/AP_Compass/AP_Compass.h
View File

@ -49,9 +49,6 @@
#define COMPASS_MAX_INSTANCES 3
#define COMPASS_MAX_BACKEND 3
//MAXIMUM COMPASS REPORTS
#define MAX_CAL_REPORTS 10
#define CONTINUOUS_REPORTS 0
#define AP_COMPASS_MAX_XYZ_ANG_DIFF radians(50.0f)
#define AP_COMPASS_MAX_XY_ANG_DIFF radians(30.0f)
#define AP_COMPASS_MAX_XY_LENGTH_DIFF 100.0f
@ -126,21 +123,11 @@ public:
// 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 autoreboot = false);
bool start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot = false);
bool start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false);
void start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot = false);
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);
bool compass_cal_requires_reboot() { return _cal_complete_requires_reboot; }
bool auto_reboot() { return _compass_cal_autoreboot; }
uint8_t get_cal_mask() const;
bool is_calibrating() const;
/*
@ -316,8 +303,20 @@ private:
bool _add_backend(AP_Compass_Backend *backend, const char *name, bool external);
void _detect_backends(void);
//keep track of number of calibration reports sent
uint8_t _reports_sent[COMPASS_MAX_INSTANCES];
// compass cal
bool _accept_calibration(uint8_t i);
bool _accept_calibration_mask(uint8_t mask);
void _cancel_calibration(uint8_t i);
void _cancel_calibration_mask(uint8_t mask);
uint8_t _get_cal_mask() const;
bool _start_calibration(uint8_t i, bool retry=false, float delay_sec=0.0f);
bool _start_calibration_mask(uint8_t mask, bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false);
bool _auto_reboot() { return _compass_cal_autoreboot; }
//keep track of which calibrators have been saved
bool _cal_saved[COMPASS_MAX_INSTANCES];
bool _cal_autosave;
//autoreboot after compass calibration
bool _compass_cal_autoreboot;

158
libraries/AP_Compass/AP_Compass_Calibration.cpp
View File

@ -25,6 +25,8 @@ Compass::compass_cal_update()
if (_calibrator[i].running()) {
running = true;
} else if (_cal_autosave && !_cal_saved[i] && _calibrator[i].get_status() == COMPASS_CAL_SUCCESS) {
_accept_calibration(i);
}
}
@ -33,20 +35,22 @@ Compass::compass_cal_update()
if (is_calibrating()) {
_cal_has_run = true;
return;
} else if (_cal_has_run && auto_reboot()) {
} else if (_cal_has_run && _auto_reboot()) {
hal.scheduler->delay(1000);
hal.scheduler->reboot(false);
}
}
bool
Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay, bool autoreboot)
Compass::_start_calibration(uint8_t i, bool retry, float delay)
{
if (!healthy(i)) {
return false;
}
memset(_reports_sent,0,sizeof(_reports_sent));
if (!is_calibrating() && delay > 0.5f) {
if (!use_for_yaw(i)) {
return false;
}
if (!is_calibrating()) {
AP_Notify::events.initiated_compass_cal = 1;
}
if (i == get_primary() && _state[i].external != 0) {
@ -57,8 +61,8 @@ Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay, bo
// lot noisier
_calibrator[i].set_tolerance(_calibration_threshold*2);
}
_calibrator[i].start(retry, autosave, delay);
_compass_cal_autoreboot = autoreboot;
_cal_saved[i] = false;
_calibrator[i].start(retry, delay);
// disable compass learning both for calibration and after completion
_learn.set_and_save(0);
@ -67,26 +71,15 @@ Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay, bo
}
bool
Compass::start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay, bool autoreboot)
Compass::_start_calibration_mask(uint8_t mask, bool retry, bool autosave, float delay, bool autoreboot)
{
_cal_autosave = autosave;
_compass_cal_autoreboot = autoreboot;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if ((1<<i) & mask) {
if (!start_calibration(i,retry,autosave,delay,autoreboot)) {
cancel_calibration_mask(mask);
return false;
}
}
}
return true;
}
bool
Compass::start_calibration_all(bool retry, bool autosave, float delay, bool autoreboot)
{
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if (healthy(i) && use_for_yaw(i)) {
if (!start_calibration(i,retry,autosave,delay,autoreboot)) {
cancel_calibration_all();
if (!_start_calibration(i,retry,delay)) {
_cancel_calibration_mask(mask);
return false;
}
}
@ -95,22 +88,36 @@ Compass::start_calibration_all(bool retry, bool autosave, float delay, bool auto
}
void
Compass::cancel_calibration(uint8_t i)
Compass::start_calibration_all(bool retry, bool autosave, float delay, bool autoreboot)
{
_cal_autosave = autosave;
_compass_cal_autoreboot = autoreboot;
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
_start_calibration(i,retry,delay);
}
}
void
Compass::_cancel_calibration(uint8_t i)
{
AP_Notify::events.initiated_compass_cal = 0;
if (_calibrator[i].running() || _calibrator[i].get_status() == COMPASS_CAL_WAITING_TO_START) {
AP_Notify::events.compass_cal_canceled = 1;
}
_cal_saved[i] = false;
_calibrator[i].clear();
}
void
Compass::cancel_calibration_mask(uint8_t mask)
Compass::_cancel_calibration_mask(uint8_t mask)
{
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
if((1<<i) & mask) {
cancel_calibration(i);
_cancel_calibration(i);
}
}
}
@ -118,20 +125,23 @@ Compass::cancel_calibration_mask(uint8_t mask)
void
Compass::cancel_calibration_all()
{
cancel_calibration_mask(0xFF);
_cancel_calibration_mask(0xFF);
}
bool
Compass::accept_calibration(uint8_t i)
Compass::_accept_calibration(uint8_t i)
{
CompassCalibrator& cal = _calibrator[i];
uint8_t cal_status = cal.get_status();
if (cal_status == COMPASS_CAL_SUCCESS) {
if (_cal_saved[i] || cal_status == COMPASS_CAL_NOT_STARTED) {
return true;
} else if (cal_status == COMPASS_CAL_SUCCESS) {
_cal_complete_requires_reboot = true;
_cal_saved[i] = true;
Vector3f ofs, diag, offdiag;
cal.get_calibration(ofs, diag, offdiag);
cal.clear();
set_and_save_offsets(i, ofs);
set_and_save_diagonals(i,diag);
@ -147,43 +157,32 @@ Compass::accept_calibration(uint8_t i)
}
bool
Compass::accept_calibration_mask(uint8_t mask)
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)) {
if (!_accept_calibration(i)) {
success = false;
}
_calibrator[i].clear();
}
}
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();
uint8_t cal_mask = _get_cal_mask();
for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
// ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_PROGRESS)) {
return;
}
auto& calibrator = _calibrator[compass_id];
uint8_t cal_status = calibrator.get_status();
@ -195,11 +194,6 @@ Compass::send_mag_cal_progress(mavlink_channel_t chan)
Vector3f direction(0.0f,0.0f,0.0f);
uint8_t attempt = _calibrator[compass_id].get_attempt();
// ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_PROGRESS)) {
return;
}
mavlink_msg_mag_cal_progress_send(
chan,
compass_id, cal_mask,
@ -212,24 +206,22 @@ Compass::send_mag_cal_progress(mavlink_channel_t chan)
void Compass::send_mag_cal_report(mavlink_channel_t chan)
{
uint8_t cal_mask = get_cal_mask();
uint8_t cal_mask = _get_cal_mask();
for (uint8_t compass_id=0; compass_id<COMPASS_MAX_INSTANCES; compass_id++) {
uint8_t cal_status = _calibrator[compass_id].get_status();
if ((cal_status == COMPASS_CAL_SUCCESS ||
cal_status == COMPASS_CAL_FAILED) && ((_reports_sent[compass_id] < MAX_CAL_REPORTS) || CONTINUOUS_REPORTS)) {
float fitness = _calibrator[compass_id].get_fitness();
Vector3f ofs, diag, offdiag;
_calibrator[compass_id].get_calibration(ofs, diag, offdiag);
uint8_t autosaved = _calibrator[compass_id].get_autosave();
// ensure we don't try to send with no space available
if (!HAVE_PAYLOAD_SPACE(chan, MAG_CAL_REPORT)) {
return;
}
uint8_t cal_status = _calibrator[compass_id].get_status();
if ((cal_status == COMPASS_CAL_SUCCESS ||
cal_status == COMPASS_CAL_FAILED)) {
float fitness = _calibrator[compass_id].get_fitness();
Vector3f ofs, diag, offdiag;
_calibrator[compass_id].get_calibration(ofs, diag, offdiag);
uint8_t autosaved = _cal_saved[compass_id];
mavlink_msg_mag_cal_report_send(
chan,
compass_id, cal_mask,
@ -239,11 +231,6 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
diag.x, diag.y, diag.z,
offdiag.x, offdiag.y, offdiag.z
);
_reports_sent[compass_id]++;
}
if (cal_status == COMPASS_CAL_SUCCESS && _calibrator[compass_id].get_autosave()) {
accept_calibration(compass_id);
}
}
}
@ -251,11 +238,21 @@ void Compass::send_mag_cal_report(mavlink_channel_t chan)
bool
Compass::is_calibrating() const
{
return get_cal_mask();
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
switch(_calibrator[i].get_status()) {
case COMPASS_CAL_NOT_STARTED:
case COMPASS_CAL_SUCCESS:
case COMPASS_CAL_FAILED:
break;
default:
return true;
}
}
return false;
}
uint8_t
Compass::get_cal_mask() const
Compass::_get_cal_mask() const
{
uint8_t cal_mask = 0;
for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
@ -294,11 +291,9 @@ uint8_t Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
bool autoreboot = !is_zero(packet.param5);
if (mag_mask == 0) { // 0 means all
if (!start_calibration_all(retry, autosave, delay, autoreboot)) {
result = MAV_RESULT_FAILED;
}
start_calibration_all(retry, autosave, delay, autoreboot);
} else {
if (!start_calibration_mask(mag_mask, retry, autosave, delay, autoreboot)) {
if (!_start_calibration_mask(mag_mask, retry, autosave, delay, autoreboot)) {
result = MAV_RESULT_FAILED;
}
}
@ -316,13 +311,10 @@ uint8_t Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
uint8_t mag_mask = packet.param1;
if (mag_mask == 0) { // 0 means all
if(!accept_calibration_all()) {
result = MAV_RESULT_FAILED;
}
break;
mag_mask = 0xFF;
}
if(!accept_calibration_mask(mag_mask)) {
if(!_accept_calibration_mask(mag_mask)) {
result = MAV_RESULT_FAILED;
}
break;
@ -342,7 +334,7 @@ uint8_t Compass::handle_mag_cal_command(const mavlink_command_long_t &packet)
break;
}
cancel_calibration_mask(mag_mask);
_cancel_calibration_mask(mag_mask);
break;
}
}

3
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -86,11 +86,10 @@ void CompassCalibrator::clear() {
set_status(COMPASS_CAL_NOT_STARTED);
}
void CompassCalibrator::start(bool retry, bool autosave, float delay) {
void CompassCalibrator::start(bool retry, float delay) {
if(running()) {
return;
}
_autosave = autosave;
_attempt = 1;
_retry = retry;
_delay_start_sec = delay;

4
libraries/AP_Compass/CompassCalibrator.h
View File

@ -22,7 +22,7 @@ public:
CompassCalibrator();
void start(bool retry=false, bool autosave=false, float delay=0.0f);
void start(bool retry=false, float delay=0.0f);
void clear();
void update(bool &failure);
@ -40,7 +40,6 @@ public:
completion_mask_t& get_completion_mask();
enum compass_cal_status_t get_status() const { return _status; }
float get_fitness() const { return sqrtf(_fitness); }
bool get_autosave() const { return _autosave; }
uint8_t get_attempt() const { return _attempt; }
private:
@ -80,7 +79,6 @@ private:
// behavioral state
float _delay_start_sec;
uint32_t _start_time_ms;
bool _autosave;
bool _retry;
float _tolerance;
uint8_t _attempt;