Browse Source

AP_Compass: pull compass calibrator out into a separate thread

zr-v5.1
Siddharth Purohit 5 years ago committed by Andrew Tridgell
parent
commit
c0eb27cc35
  1. 16
      libraries/AP_Compass/AP_Compass.h
  2. 5
      libraries/AP_Compass/AP_Compass_Backend.cpp
  3. 163
      libraries/AP_Compass/AP_Compass_Calibration.cpp
  4. 290
      libraries/AP_Compass/CompassCalibrator.cpp
  5. 112
      libraries/AP_Compass/CompassCalibrator.h

16
libraries/AP_Compass/AP_Compass.h

@ -9,7 +9,6 @@ @@ -9,7 +9,6 @@
#include <AP_Param/AP_Param.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include "CompassCalibrator.h"
#include "AP_Compass_Backend.h"
#include "Compass_PerMotor.h"
#include <AP_Common/TSIndex.h>
@ -63,6 +62,8 @@ @@ -63,6 +62,8 @@
#define MAX_CONNECTED_MAGS (COMPASS_MAX_UNREG_DEV+COMPASS_MAX_INSTANCES)
#include "CompassCalibrator.h"
class CompassLearn;
class Compass
@ -167,8 +168,8 @@ public: @@ -167,8 +168,8 @@ public:
void cancel_calibration_all();
bool compass_cal_requires_reboot() const { return _cal_complete_requires_reboot; }
bool is_calibrating() const;
bool compass_cal_requires_reboot() const { return _cal_requires_reboot; }
bool is_calibrating();
// indicate which bit in LOG_BITMASK indicates we should log compass readings
void set_log_bit(uint32_t log_bit) { _log_bit = log_bit; }
@ -367,11 +368,12 @@ private: @@ -367,11 +368,12 @@ private:
void _detect_backends(void);
// compass cal
void _update_calibration_trampoline();
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;
uint8_t _get_cal_mask();
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; }
@ -393,7 +395,7 @@ private: @@ -393,7 +395,7 @@ private:
//autoreboot after compass calibration
bool _compass_cal_autoreboot;
bool _cal_complete_requires_reboot;
bool _cal_requires_reboot;
bool _cal_has_run;
// enum of drivers for COMPASS_TYPEMASK
@ -545,7 +547,7 @@ private: @@ -545,7 +547,7 @@ private:
AP_Int16 _options;
#if COMPASS_CAL_ENABLED
RestrictIDTypeArray<CompassCalibrator, COMPASS_MAX_INSTANCES, Priority> _calibrator;
RestrictIDTypeArray<CompassCalibrator*, COMPASS_MAX_INSTANCES, Priority> _calibrator;
#endif
#if COMPASS_MOT_ENABLED
@ -578,6 +580,8 @@ private: @@ -578,6 +580,8 @@ private:
///
void try_set_initial_location();
bool _initial_location_set;
bool _cal_thread_started;
};
namespace AP {

5
libraries/AP_Compass/AP_Compass_Backend.cpp

@ -52,7 +52,10 @@ void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint8_t instance @@ -52,7 +52,10 @@ void AP_Compass_Backend::publish_raw_field(const Vector3f &mag, uint8_t instance
// sensor rate. We want them to consume only the filtered fields
state.last_update_ms = AP_HAL::millis();
#if COMPASS_CAL_ENABLED
_compass._calibrator[_compass._get_priority(Compass::StateIndex(instance))].new_sample(mag);
auto& cal = _compass._calibrator[_compass._get_priority(Compass::StateIndex(instance))];
if (cal != nullptr) {
cal->new_sample(mag);
}
#endif
}

163
libraries/AP_Compass/AP_Compass_Calibration.cpp

@ -3,7 +3,6 @@ @@ -3,7 +3,6 @@
#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"
@ -20,20 +19,16 @@ void Compass::cal_update() @@ -20,20 +19,16 @@ void Compass::cal_update()
bool running = false;
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
bool failure;
_calibrator[i].update(failure);
if (failure) {
AP_Notify::events.compass_cal_failed = 1;
if (_calibrator[i] == nullptr) {
continue;
}
if (_calibrator[i].check_for_timeout()) {
if (_calibrator[i]->failed()) {
AP_Notify::events.compass_cal_failed = 1;
cancel_calibration_all();
}
if (_calibrator[i].running()) {
if (_calibrator[i]->running()) {
running = true;
} else if (_cal_autosave && !_cal_saved[i] && _calibrator[i].get_status() == CompassCalibrator::Status::SUCCESS) {
} else if (_cal_autosave && !_cal_saved[i] && _calibrator[i]->get_state().status == CompassCalibrator::Status::SUCCESS) {
_accept_calibration(uint8_t(i));
}
}
@ -62,6 +57,15 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay) @@ -62,6 +57,15 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
gcs().send_text(MAV_SEVERITY_ERROR, "Compass cal requires reboot after priority change");
return false;
}
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");
@ -71,22 +75,30 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay) @@ -71,22 +75,30 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
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);
}
}
_cal_saved[prio] = false;
if (i == 0 && _get_state(prio).external != 0) {
_calibrator[prio].set_tolerance(_calibration_threshold);
_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].set_tolerance(_calibration_threshold*2);
_calibrator[prio]->start(retry, delay, get_offsets_max(), i, _calibration_threshold*2);
}
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);
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;
}
_cal_saved[prio] = false;
_calibrator[prio].start(retry, delay, get_offsets_max(), i);
// disable compass learning both for calibration and after completion
_learn.set_and_save(0);
@ -94,6 +106,18 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay) @@ -94,6 +106,18 @@ bool Compass::_start_calibration(uint8_t i, bool retry, float delay)
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;
@ -126,12 +150,14 @@ void Compass::_cancel_calibration(uint8_t i) @@ -126,12 +150,14 @@ void Compass::_cancel_calibration(uint8_t i)
{
AP_Notify::events.initiated_compass_cal = 0;
Priority prio = Priority(i);
if (_calibrator[prio].running() || _calibrator[prio].get_status() == CompassCalibrator::Status::WAITING_TO_START) {
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();
_calibrator[prio]->stop();
}
void Compass::_cancel_calibration_mask(uint8_t mask)
@ -152,18 +178,19 @@ bool Compass::_accept_calibration(uint8_t i) @@ -152,18 +178,19 @@ bool Compass::_accept_calibration(uint8_t i)
{
Priority prio = Priority(i);
CompassCalibrator& cal = _calibrator[prio];
const CompassCalibrator::Status cal_status = cal.get_status();
CompassCalibrator* cal = _calibrator[prio];
if (cal == nullptr) {
return false;
}
const CompassCalibrator::Report cal_report = cal->get_report();
if (_cal_saved[prio] || cal_status == CompassCalibrator::Status::NOT_STARTED) {
if (_cal_saved[prio] || cal_report.status == CompassCalibrator::Status::NOT_STARTED) {
return true;
} else if (cal_status == CompassCalibrator::Status::SUCCESS) {
_cal_complete_requires_reboot = true;
} else if (cal_report.status == CompassCalibrator::Status::SUCCESS) {
_cal_saved[prio] = true;
Vector3f ofs, diag, offdiag;
float scale_factor;
cal.get_calibration(ofs, diag, offdiag, scale_factor);
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);
@ -171,7 +198,7 @@ bool Compass::_accept_calibration(uint8_t i) @@ -171,7 +198,7 @@ bool Compass::_accept_calibration(uint8_t i)
set_and_save_scale_factor(i,scale_factor);
if (_get_state(prio).external && _rotate_auto >= 2) {
set_and_save_orientation(i, cal.get_orientation());
set_and_save_orientation(i, cal_report.orientation);
}
if (!is_calibrating()) {
@ -187,11 +214,14 @@ bool Compass::_accept_calibration_mask(uint8_t mask) @@ -187,11 +214,14 @@ 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();
_calibrator[i]->stop();
}
}
@ -210,23 +240,21 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link) @@ -210,23 +240,21 @@ bool Compass::send_mag_cal_progress(const GCS_MAVLINK& link)
// hogging *all* the bandwidth
return true;
}
auto& calibrator = _calibrator[compass_id];
const CompassCalibrator::Status cal_status = calibrator.get_status();
if (cal_status == CompassCalibrator::Status::WAITING_TO_START ||
cal_status == CompassCalibrator::Status::RUNNING_STEP_ONE ||
cal_status == CompassCalibrator::Status::RUNNING_STEP_TWO) {
uint8_t completion_pct = calibrator.get_completion_percent();
const CompassCalibrator::completion_mask_t& completion_mask = calibrator.get_completion_mask();
const Vector3f direction;
uint8_t attempt = _calibrator[compass_id].get_attempt();
if (calibrator == nullptr) {
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) {
mavlink_msg_mag_cal_progress_send(
link.get_chan(),
uint8_t(compass_id), cal_mask,
(uint8_t)cal_status, attempt, completion_pct, completion_mask,
direction.x, direction.y, direction.z
(uint8_t)cal_state.status, cal_state.attempt, cal_state.completion_pct, cal_state.completion_mask,
0.0f, 0.0f, 0.0f
);
}
}
@ -246,39 +274,39 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link) @@ -246,39 +274,39 @@ bool Compass::send_mag_cal_report(const GCS_MAVLINK& link)
// hogging *all* the bandwidth
return true;
}
const CompassCalibrator::Status cal_status = _calibrator[compass_id].get_status();
if (cal_status == CompassCalibrator::Status::SUCCESS ||
cal_status == CompassCalibrator::Status::FAILED ||
cal_status == CompassCalibrator::Status::BAD_ORIENTATION) {
float fitness = _calibrator[compass_id].get_fitness();
Vector3f ofs, diag, offdiag;
float scale_factor;
_calibrator[compass_id].get_calibration(ofs, diag, offdiag, scale_factor);
if (_calibrator[compass_id] == nullptr) {
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) {
uint8_t autosaved = _cal_saved[compass_id];
mavlink_msg_mag_cal_report_send(
link.get_chan(),
uint8_t(compass_id), cal_mask,
(uint8_t)cal_status, autosaved,
fitness,
ofs.x, ofs.y, ofs.z,
diag.x, diag.y, diag.z,
offdiag.x, offdiag.y, offdiag.z,
_calibrator[compass_id].get_orientation_confidence(),
_calibrator[compass_id].get_original_orientation(),
_calibrator[compass_id].get_orientation(),
scale_factor
(uint8_t)cal_report.status, autosaved,
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
);
}
}
return true;
}
bool Compass::is_calibrating() const
bool Compass::is_calibrating()
{
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
switch(_calibrator[i].get_status()) {
if (_calibrator[i] == nullptr) {
continue;
}
switch(_calibrator[i]->get_state().status) {
case CompassCalibrator::Status::NOT_STARTED:
case CompassCalibrator::Status::SUCCESS:
case CompassCalibrator::Status::FAILED:
@ -291,11 +319,14 @@ bool Compass::is_calibrating() const @@ -291,11 +319,14 @@ bool Compass::is_calibrating() const
return false;
}
uint8_t Compass::_get_cal_mask() const
uint8_t Compass::_get_cal_mask()
{
uint8_t cal_mask = 0;
for (Priority i(0); i<COMPASS_MAX_INSTANCES; i++) {
if (_calibrator[i].get_status() != CompassCalibrator::Status::NOT_STARTED) {
if (_calibrator[i] == nullptr) {
continue;
}
if (_calibrator[i]->get_state().status != CompassCalibrator::Status::NOT_STARTED) {
cal_mask |= 1 << uint8_t(i);
}
}

290
libraries/AP_Compass/CompassCalibrator.cpp

@ -57,6 +57,7 @@ @@ -57,6 +57,7 @@
* http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm
*/
#include "AP_Compass.h"
#include "CompassCalibrator.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_GeodesicGrid.h>
@ -75,131 +76,111 @@ extern const AP_HAL::HAL& hal; @@ -75,131 +76,111 @@ extern const AP_HAL::HAL& hal;
CompassCalibrator::CompassCalibrator()
{
stop();
set_status(Status::NOT_STARTED);
}
// Request to cancel calibration
void CompassCalibrator::stop()
{
set_status(Status::NOT_STARTED);
WITH_SEMAPHORE(state_sem);
_requested_status = Status::NOT_STARTED;
_status_set_requested = true;
}
void CompassCalibrator::set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation)
{
_check_orientation = true;
_orientation = orientation;
_orig_orientation = orientation;
_is_external = is_external;
_fix_orientation = fix_orientation;
WITH_SEMAPHORE(state_sem);
cal_settings.check_orientation = true;
cal_settings.orientation = orientation;
cal_settings.orig_orientation = orientation;
cal_settings.is_external = is_external;
cal_settings.fix_orientation = fix_orientation;
}
void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx)
void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx, float tolerance)
{
if (running()) {
if (compass_idx > COMPASS_MAX_INSTANCES) {
return;
}
_offset_max = offset_max;
_attempt = 1;
_retry = retry;
_delay_start_sec = delay;
_start_time_ms = AP_HAL::millis();
_compass_idx = compass_idx;
set_status(Status::WAITING_TO_START);
}
void CompassCalibrator::get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals, float &scale_factor)
{
if (_status != Status::SUCCESS) {
WITH_SEMAPHORE(state_sem);
// Don't do this while we are already started
if (_running()) {
return;
}
offsets = _params.offset;
diagonals = _params.diag;
offdiagonals = _params.offdiag;
scale_factor = _params.scale_factor;
cal_settings.offset_max = offset_max;
cal_settings.attempt = 1;
cal_settings.retry = retry;
cal_settings.delay_start_sec = delay;
cal_settings.start_time_ms = AP_HAL::millis();
cal_settings.compass_idx = compass_idx;
cal_settings.tolerance = tolerance;
// Request status change to Waiting to start
_requested_status = Status::WAITING_TO_START;
_status_set_requested = true;
}
float CompassCalibrator::get_completion_percent() const
// Record point mag sample and associated attitude sample to intermediate struct
void CompassCalibrator::new_sample(const Vector3f& sample)
{
// first sampling step is 1/3rd of the progress bar
// never return more than 99% unless _status is Status::SUCCESS
switch (_status) {
case Status::NOT_STARTED:
case Status::WAITING_TO_START:
return 0.0f;
case Status::RUNNING_STEP_ONE:
return 33.3f * _samples_collected/COMPASS_CAL_NUM_SAMPLES;
case Status::RUNNING_STEP_TWO:
return 33.3f + 65.7f*((float)(_samples_collected-_samples_thinned)/(COMPASS_CAL_NUM_SAMPLES-_samples_thinned));
case Status::SUCCESS:
return 100.0f;
case Status::FAILED:
case Status::BAD_ORIENTATION:
case Status::BAD_RADIUS:
return 0.0f;
};
// will not get here if the compiler is doing its job (no default clause)
return 0.0f;
WITH_SEMAPHORE(sample_sem);
_last_sample.set(sample);
_last_sample.att.set_from_ahrs();
_new_sample = true;
}
// update completion mask based on latest sample
// used to ensure we have collected samples in all directions
void CompassCalibrator::update_completion_mask(const Vector3f& v)
{
Matrix3f softiron {
_params.diag.x, _params.offdiag.x, _params.offdiag.y,
_params.offdiag.x, _params.diag.y, _params.offdiag.z,
_params.offdiag.y, _params.offdiag.z, _params.diag.z
};
Vector3f corrected = softiron * (v + _params.offset);
int section = AP_GeodesicGrid::section(corrected, true);
if (section < 0) {
return;
}
_completion_mask[section / 8] |= 1 << (section % 8);
bool CompassCalibrator::failed() {
WITH_SEMAPHORE(state_sem);
return (cal_state.status == Status::FAILED ||
cal_state.status == Status::BAD_ORIENTATION ||
cal_state.status == Status::BAD_RADIUS);
}
// reset and update the completion mask using all samples in the sample buffer
void CompassCalibrator::update_completion_mask()
{
memset(_completion_mask, 0, sizeof(_completion_mask));
for (int i = 0; i < _samples_collected; i++) {
update_completion_mask(_sample_buffer[i].get());
}
bool CompassCalibrator::running() {
WITH_SEMAPHORE(state_sem);
return (cal_state.status == Status::RUNNING_STEP_ONE || cal_state.status == Status::RUNNING_STEP_TWO);
}
bool CompassCalibrator::check_for_timeout()
{
uint32_t tnow = AP_HAL::millis();
if (running() && tnow - _last_sample_ms > 1000) {
_retry = false;
set_status(Status::FAILED);
return true;
}
return false;
const CompassCalibrator::Report CompassCalibrator::get_report() {
WITH_SEMAPHORE(state_sem);
return cal_report;
}
void CompassCalibrator::new_sample(const Vector3f& sample)
const CompassCalibrator::State CompassCalibrator::get_state() {
WITH_SEMAPHORE(state_sem);
return cal_state;
}
/////////////////////////////////////////////////////////////
////////////////////// PRIVATE METHODS //////////////////////
/////////////////////////////////////////////////////////////
void CompassCalibrator::update()
{
_last_sample_ms = AP_HAL::millis();
if (_status == Status::WAITING_TO_START) {
set_status(Status::RUNNING_STEP_ONE);
}
//pickup samples from intermediate struct
pull_sample();
if (running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample)) {
update_completion_mask(sample);
_sample_buffer[_samples_collected].set(sample);
_sample_buffer[_samples_collected].att.set_from_ahrs();
_samples_collected++;
}
}
{
WITH_SEMAPHORE(state_sem);
//update_settings
if (!running()) {
update_cal_settings();
}
void CompassCalibrator::update(bool &failure)
{
failure = false;
//update requested state
if (_status_set_requested) {
_status_set_requested = false;
set_status(_requested_status);
}
//update report and status
update_cal_status();
update_cal_report();
}
// collect the minimum number of samples
if (!fitting()) {
if (!_fitting()) {
return;
}
@ -207,7 +188,6 @@ void CompassCalibrator::update(bool &failure) @@ -207,7 +188,6 @@ void CompassCalibrator::update(bool &failure)
if (_fit_step >= 10) {
if (is_equal(_fitness, _initial_fitness) || isnan(_fitness)) { // if true, means that fitness is diverging instead of converging
set_status(Status::FAILED);
failure = true;
} else {
set_status(Status::RUNNING_STEP_TWO);
}
@ -224,7 +204,6 @@ void CompassCalibrator::update(bool &failure) @@ -224,7 +204,6 @@ void CompassCalibrator::update(bool &failure)
set_status(Status::SUCCESS);
} else {
set_status(Status::FAILED);
failure = true;
}
} else if (_fit_step < 15) {
run_sphere_fit();
@ -236,17 +215,124 @@ void CompassCalibrator::update(bool &failure) @@ -236,17 +215,124 @@ void CompassCalibrator::update(bool &failure)
}
}
/////////////////////////////////////////////////////////////
////////////////////// PRIVATE METHODS //////////////////////
/////////////////////////////////////////////////////////////
bool CompassCalibrator::running() const
void CompassCalibrator::pull_sample()
{
CompassSample mag_sample;
{
WITH_SEMAPHORE(sample_sem);
if (!_new_sample) {
return;
}
if (_status == Status::WAITING_TO_START) {
set_status(Status::RUNNING_STEP_ONE);
}
_new_sample = false;
mag_sample = _last_sample;
}
if (_running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(mag_sample.get())) {
update_completion_mask(mag_sample.get());
_sample_buffer[_samples_collected] = mag_sample;
_samples_collected++;
}
}
void CompassCalibrator::update_cal_settings()
{
_tolerance = cal_settings.tolerance;
_check_orientation = cal_settings.check_orientation;
_orientation = cal_settings.orientation;
_orig_orientation = cal_settings.orig_orientation;
_is_external = cal_settings.is_external;
_fix_orientation = cal_settings.fix_orientation;
_offset_max = cal_settings.offset_max;
_attempt = cal_settings.attempt;
_retry = cal_settings.retry;
_delay_start_sec = cal_settings.delay_start_sec;
_start_time_ms = cal_settings.start_time_ms;
_compass_idx = cal_settings.compass_idx;
}
// update completion mask based on latest sample
// used to ensure we have collected samples in all directions
void CompassCalibrator::update_completion_mask(const Vector3f& v)
{
Matrix3f softiron {
_params.diag.x, _params.offdiag.x, _params.offdiag.y,
_params.offdiag.x, _params.diag.y, _params.offdiag.z,
_params.offdiag.y, _params.offdiag.z, _params.diag.z
};
Vector3f corrected = softiron * (v + _params.offset);
int section = AP_GeodesicGrid::section(corrected, true);
if (section < 0) {
return;
}
_completion_mask[section / 8] |= 1 << (section % 8);
}
// reset and update the completion mask using all samples in the sample buffer
void CompassCalibrator::update_completion_mask()
{
memset(_completion_mask, 0, sizeof(_completion_mask));
for (int i = 0; i < _samples_collected; i++) {
update_completion_mask(_sample_buffer[i].get());
}
}
void CompassCalibrator::update_cal_status()
{
cal_state.status = _status;
cal_state.attempt = _attempt;
memcpy(cal_state.completion_mask, _completion_mask, sizeof(completion_mask_t));
cal_state.completion_pct = 0.0f;
// first sampling step is 1/3rd of the progress bar
// never return more than 99% unless _status is Status::SUCCESS
switch (_status) {
case Status::NOT_STARTED:
case Status::WAITING_TO_START:
cal_state.completion_pct = 0.0f;
break;
case Status::RUNNING_STEP_ONE:
cal_state.completion_pct = 33.3f * _samples_collected/COMPASS_CAL_NUM_SAMPLES;
break;
case Status::RUNNING_STEP_TWO:
cal_state.completion_pct = 33.3f + 65.7f*((float)(_samples_collected-_samples_thinned)/(COMPASS_CAL_NUM_SAMPLES-_samples_thinned));
break;
case Status::SUCCESS:
cal_state.completion_pct = 100.0f;
break;
case Status::FAILED:
case Status::BAD_ORIENTATION:
case Status::BAD_RADIUS:
cal_state.completion_pct = 0.0f;
break;
};
}
void CompassCalibrator::update_cal_report()
{
cal_report.status = _status;
cal_report.fitness = sqrtf(_fitness);
cal_report.ofs = _params.offset;
cal_report.diag = _params.diag;
cal_report.offdiag = _params.offdiag;
cal_report.scale_factor = _params.scale_factor;
cal_report.orientation_confidence = _orientation_confidence;
cal_report.original_orientation = _orig_orientation;
cal_report.orientation = _orientation_solution;
}
// running method for use in thread
bool CompassCalibrator::_running() const
{
return _status == Status::RUNNING_STEP_ONE || _status == Status::RUNNING_STEP_TWO;
}
bool CompassCalibrator::fitting() const
// fitting method for use in thread
bool CompassCalibrator::_fitting() const
{
return running() && (_samples_collected == COMPASS_CAL_NUM_SAMPLES);
return _running() && (_samples_collected == COMPASS_CAL_NUM_SAMPLES);
}
// initialize fitness before starting a fit
@ -825,6 +911,8 @@ bool CompassCalibrator::calculate_orientation(void) @@ -825,6 +911,8 @@ bool CompassCalibrator::calculate_orientation(void)
float variance[ROTATION_MAX_AUTO_ROTATION+1] {};
_orientation_solution = _orientation;
for (enum Rotation r = ROTATION_NONE; r <= ROTATION_MAX_AUTO_ROTATION; r = (enum Rotation)(r+1)) {
// calculate the average implied earth field across all samples
Vector3f total_ef {};
@ -904,6 +992,7 @@ bool CompassCalibrator::calculate_orientation(void) @@ -904,6 +992,7 @@ bool CompassCalibrator::calculate_orientation(void)
// we won't change the orientation, but we set _orientation
// for reporting purposes
_orientation = besti;
_orientation_solution = besti;
set_status(Status::BAD_ORIENTATION);
return false;
}
@ -923,6 +1012,7 @@ bool CompassCalibrator::calculate_orientation(void) @@ -923,6 +1012,7 @@ bool CompassCalibrator::calculate_orientation(void)
}
_orientation = besti;
_orientation_solution = besti;
// re-run the fit to get the diagonals and off-diagonals for the
// new orientation

112
libraries/AP_Compass/CompassCalibrator.h

@ -13,24 +13,25 @@ class CompassCalibrator { @@ -13,24 +13,25 @@ class CompassCalibrator {
public:
CompassCalibrator();
// set tolerance of calibration (aka fitness)
void set_tolerance(float tolerance) { _tolerance = tolerance; }
// start or stop the calibration
void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx, float tolerance);
void stop();
// Update point sample
void new_sample(const Vector3f& sample);
// set compass's initial orientation and whether it should be automatically fixed (if required)
void set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation);
// start or stop the calibration
void start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx);
void stop();
// running is true if actively calculating offsets, diagonals or offdiagonals
bool running();
// update the state machine and calculate offsets, diagonals and offdiagonals
void update(bool &failure);
void new_sample(const Vector3f &sample);
// failed is true if either of the failure states are hit
bool failed();
bool check_for_timeout();
// running is true if actively calculating offsets, diagonals or offdiagonals
bool running() const;
// update the state machine and calculate offsets, diagonals and offdiagonals
void update();
// compass calibration states
enum class Status {
@ -44,27 +45,51 @@ public: @@ -44,27 +45,51 @@ public:
BAD_RADIUS = 7,
};
// get status of calibrations progress
Status get_status() const { return _status; }
// get calibration outputs (offsets, diagonals, offdiagonals) and fitness
void get_calibration(Vector3f &offsets, Vector3f &diagonals, Vector3f &offdiagonals, float &scale_factor);
float get_fitness() const { return sqrtf(_fitness); }
// get corrected (and original) orientation
enum Rotation get_orientation() const { return _orientation; }
enum Rotation get_original_orientation() const { return _orig_orientation; }
float get_orientation_confidence() const { return _orientation_confidence; }
// get completion percentage (0 to 100) for reporting to GCS
float get_completion_percent() const;
// get how many attempts have been made to calibrate for reporting to GCS
uint8_t get_attempt() const { return _attempt; }
// get completion mask for mavlink reporting (a bitmask of faces/directions for which we have compass samples)
typedef uint8_t completion_mask_t[10];
const completion_mask_t& get_completion_mask() const { return _completion_mask; }
// Structure accessed for cal status update via mavlink
struct State {
Status status;
uint8_t attempt;
float completion_pct;
completion_mask_t completion_mask;
} cal_state;
// Structure accessed after calibration is finished/failed
struct Report {
Status status;
float fitness;
Vector3f ofs;
Vector3f diag;
Vector3f offdiag;
float orientation_confidence;
Rotation original_orientation;
Rotation orientation;
float scale_factor;
} cal_report;
// Structure setup to set calibration run settings
struct Settings {
float tolerance;
bool check_orientation;
enum Rotation orientation;
enum Rotation orig_orientation;
bool is_external;
bool fix_orientation;
uint16_t offset_max;
uint8_t attempt;
bool retry;
float delay_start_sec;
uint32_t start_time_ms;
uint8_t compass_idx;
} cal_settings;
// Get calibration result
const Report get_report();
// Get current Calibration state
const State get_state();
private:
@ -112,6 +137,9 @@ private: @@ -112,6 +137,9 @@ private:
// set status including any required initialisation
bool set_status(Status status);
// consume point raw sample from intermediate structure
void pull_sample();
// returns true if sample should be added to buffer
bool accept_sample(const Vector3f &sample, uint16_t skip_index = UINT16_MAX);
bool accept_sample(const CompassSample &sample, uint16_t skip_index = UINT16_MAX);
@ -126,7 +154,7 @@ private: @@ -126,7 +154,7 @@ private:
void initialize_fit();
// true if enough samples have been collected and fitting has begun (aka runniong())
bool fitting() const;
bool _fitting() const;
// thins out samples between step one and step two
void thin_samples();
@ -162,9 +190,16 @@ private: @@ -162,9 +190,16 @@ private:
// fix radius to compensate for sensor scaling errors
bool fix_radius();
// update methods to read write intermediate structures, called inside thread
inline void update_cal_status();
inline void update_cal_report();
inline void update_cal_settings();
// running method for use in thread
bool _running() const;
uint8_t _compass_idx; // index of the compass providing data
Status _status; // current state of calibrator
uint32_t _last_sample_ms; // system time of last sample received for timeout
// values provided by caller
float _delay_start_sec; // seconds to delay start of calibration (provided by caller)
@ -191,8 +226,21 @@ private: @@ -191,8 +226,21 @@ private:
// variables for orientation checking
enum Rotation _orientation; // latest detected orientation
enum Rotation _orig_orientation; // original orientation provided by caller
enum Rotation _orientation_solution; // latest solution
bool _is_external; // true if compass is external (provided by caller)
bool _check_orientation; // true if orientation should be automatically checked
bool _fix_orientation; // true if orientation should be fixed if necessary
float _orientation_confidence; // measure of confidence in automatic orientation detection
CompassSample _last_sample;
Status _requested_status;
bool _status_set_requested;
bool _new_sample;
// Semaphore for state related intermediate structures
HAL_Semaphore state_sem;
// Semaphore for intermediate structure for point sample collection
HAL_Semaphore sample_sem;
};

Loading…
Cancel
Save