2015-05-29 05:00:27 -03:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
|
|
|
|
/*
|
2015-07-06 17:12:45 -03:00
|
|
|
* The intention of a magnetometer in a compass application is to measure
|
|
|
|
* Earth's magnetic field. Measurements other than those of Earth's magnetic
|
|
|
|
* field are considered errors. This algorithm computes a set of correction
|
|
|
|
* parameters that null out errors from various sources:
|
2015-05-29 05:00:27 -03:00
|
|
|
*
|
2015-07-06 17:12:45 -03:00
|
|
|
* - Sensor bias error
|
|
|
|
* - "Hard iron" error caused by materials fixed to the vehicle body that
|
|
|
|
* produce static magnetic fields.
|
|
|
|
* - Sensor scale-factor error
|
|
|
|
* - Sensor cross-axis sensitivity
|
|
|
|
* - "Soft iron" error caused by materials fixed to the vehicle body that
|
|
|
|
* distort magnetic fields.
|
2015-05-29 05:00:27 -03:00
|
|
|
*
|
2015-07-06 17:12:45 -03:00
|
|
|
* This is done by taking a set of samples that are assumed to be the product
|
|
|
|
* of rotation in earth's magnetic field and fitting an offset ellipsoid to
|
|
|
|
* them, determining the correction to be applied to adjust the samples into an
|
|
|
|
* origin-centered sphere.
|
2015-05-29 05:00:27 -03:00
|
|
|
*
|
2015-07-06 17:12:45 -03:00
|
|
|
* The state machine of this library is described entirely by the
|
2019-11-21 23:57:17 -04:00
|
|
|
* CompassCalibrator::Status enum, and all state transitions are managed by the
|
2015-07-06 17:12:45 -03:00
|
|
|
* set_status function. Normally, the library is in the NOT_STARTED state. When
|
|
|
|
* the start function is called, the state transitions to WAITING_TO_START,
|
|
|
|
* until two conditions are met: the delay as elapsed, and the memory for the
|
|
|
|
* sample buffer has been successfully allocated.
|
|
|
|
* Once these conditions are met, the state transitions to RUNNING_STEP_ONE, and
|
|
|
|
* samples are collected via calls to the new_sample function. These samples are
|
|
|
|
* accepted or rejected based on distance to the nearest sample. The samples are
|
|
|
|
* assumed to cover the surface of a sphere, and the radius of that sphere is
|
|
|
|
* initialized to a conservative value. Based on a circle-packing pattern, the
|
|
|
|
* minimum distance is set such that some percentage of the surface of that
|
|
|
|
* sphere must be covered by samples.
|
2015-05-29 05:00:27 -03:00
|
|
|
*
|
2015-07-06 17:12:45 -03:00
|
|
|
* Once the sample buffer is full, a sphere fitting algorithm is run, which
|
|
|
|
* computes a new sphere radius. The sample buffer is thinned of samples which
|
|
|
|
* no longer meet the acceptance criteria, and the state transitions to
|
|
|
|
* RUNNING_STEP_TWO. Samples continue to be collected until the buffer is full
|
|
|
|
* again, the full ellipsoid fit is run, and the state transitions to either
|
|
|
|
* SUCCESS or FAILED.
|
2015-05-29 05:00:27 -03:00
|
|
|
*
|
2015-07-06 17:12:45 -03:00
|
|
|
* The fitting algorithm used is Levenberg-Marquardt. See also:
|
|
|
|
* http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm
|
2015-05-29 05:00:27 -03:00
|
|
|
*/
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
#include "AP_Compass.h"
|
2015-03-07 11:49:11 -04:00
|
|
|
#include "CompassCalibrator.h"
|
2015-08-11 03:28:42 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2016-04-22 19:21:18 -03:00
|
|
|
#include <AP_Math/AP_GeodesicGrid.h>
|
2018-07-16 05:21:53 -03:00
|
|
|
#include <AP_AHRS/AP_AHRS.h>
|
2019-11-26 18:11:54 -04:00
|
|
|
#include <AP_GPS/AP_GPS.h>
|
2018-07-16 05:42:06 -03:00
|
|
|
#include <GCS_MAVLink/GCS.h>
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2019-11-26 18:11:54 -04:00
|
|
|
#define FIELD_RADIUS_MIN 150
|
|
|
|
#define FIELD_RADIUS_MAX 950
|
|
|
|
|
2015-03-07 11:49:11 -04:00
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////
|
|
|
|
///////////////////// PUBLIC INTERFACE /////////////////////
|
|
|
|
////////////////////////////////////////////////////////////
|
|
|
|
|
2019-11-22 02:24:29 -04:00
|
|
|
CompassCalibrator::CompassCalibrator()
|
2015-03-07 11:49:11 -04:00
|
|
|
{
|
2020-04-14 11:09:12 -03:00
|
|
|
set_status(Status::NOT_STARTED);
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
// Request to cancel calibration
|
2019-11-19 02:03:12 -04:00
|
|
|
void CompassCalibrator::stop()
|
2019-11-14 22:14:24 -04:00
|
|
|
{
|
2020-04-14 11:09:12 -03:00
|
|
|
WITH_SEMAPHORE(state_sem);
|
|
|
|
_requested_status = Status::NOT_STARTED;
|
|
|
|
_status_set_requested = true;
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
void CompassCalibrator::set_orientation(enum Rotation orientation, bool is_external, bool fix_orientation)
|
|
|
|
{
|
2020-04-14 11:09:12 -03:00
|
|
|
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;
|
2019-11-19 02:02:58 -04:00
|
|
|
}
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
void CompassCalibrator::start(bool retry, float delay, uint16_t offset_max, uint8_t compass_idx, float tolerance)
|
2018-07-17 03:11:28 -03:00
|
|
|
{
|
2020-04-14 11:09:12 -03:00
|
|
|
if (compass_idx > COMPASS_MAX_INSTANCES) {
|
2015-03-07 11:49:11 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
WITH_SEMAPHORE(state_sem);
|
|
|
|
// Don't do this while we are already started
|
|
|
|
if (_running()) {
|
2015-03-07 11:49:11 -04:00
|
|
|
return;
|
|
|
|
}
|
2020-04-14 11:09:12 -03:00
|
|
|
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;
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
// Record point mag sample and associated attitude sample to intermediate struct
|
|
|
|
void CompassCalibrator::new_sample(const Vector3f& sample)
|
2019-11-14 22:14:24 -04:00
|
|
|
{
|
2020-04-14 11:09:12 -03:00
|
|
|
WITH_SEMAPHORE(sample_sem);
|
|
|
|
_last_sample.set(sample);
|
|
|
|
_last_sample.att.set_from_ahrs();
|
|
|
|
_new_sample = true;
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
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);
|
2016-04-22 19:21:18 -03:00
|
|
|
}
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
|
|
|
|
bool CompassCalibrator::running() {
|
|
|
|
WITH_SEMAPHORE(state_sem);
|
|
|
|
return (cal_state.status == Status::RUNNING_STEP_ONE || cal_state.status == Status::RUNNING_STEP_TWO);
|
2016-04-22 19:21:18 -03:00
|
|
|
}
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
const CompassCalibrator::Report CompassCalibrator::get_report() {
|
|
|
|
WITH_SEMAPHORE(state_sem);
|
|
|
|
return cal_report;
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
const CompassCalibrator::State CompassCalibrator::get_state() {
|
|
|
|
WITH_SEMAPHORE(state_sem);
|
|
|
|
return cal_state;
|
|
|
|
}
|
|
|
|
/////////////////////////////////////////////////////////////
|
|
|
|
////////////////////// PRIVATE METHODS //////////////////////
|
|
|
|
/////////////////////////////////////////////////////////////
|
|
|
|
|
|
|
|
void CompassCalibrator::update()
|
2019-11-14 22:14:24 -04:00
|
|
|
{
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
//pickup samples from intermediate struct
|
|
|
|
pull_sample();
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
{
|
|
|
|
WITH_SEMAPHORE(state_sem);
|
|
|
|
//update_settings
|
|
|
|
if (!running()) {
|
|
|
|
update_cal_settings();
|
|
|
|
}
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
//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();
|
|
|
|
}
|
2015-03-18 21:48:46 -03:00
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// collect the minimum number of samples
|
2020-04-14 11:09:12 -03:00
|
|
|
if (!_fitting()) {
|
2015-03-07 11:49:11 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2019-11-21 23:57:17 -04:00
|
|
|
if (_status == Status::RUNNING_STEP_ONE) {
|
2015-03-12 03:52:44 -03:00
|
|
|
if (_fit_step >= 10) {
|
2019-11-14 22:14:24 -04:00
|
|
|
if (is_equal(_fitness, _initial_fitness) || isnan(_fitness)) { // if true, means that fitness is diverging instead of converging
|
2019-11-21 23:57:17 -04:00
|
|
|
set_status(Status::FAILED);
|
2019-11-19 06:12:21 -04:00
|
|
|
} else {
|
2019-11-21 23:57:17 -04:00
|
|
|
set_status(Status::RUNNING_STEP_TWO);
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
2015-03-12 03:52:44 -03:00
|
|
|
} else {
|
2016-09-16 16:14:51 -03:00
|
|
|
if (_fit_step == 0) {
|
|
|
|
calc_initial_offset();
|
|
|
|
}
|
2015-03-12 03:52:44 -03:00
|
|
|
run_sphere_fit();
|
|
|
|
_fit_step++;
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
2019-11-21 23:57:17 -04:00
|
|
|
} else if (_status == Status::RUNNING_STEP_TWO) {
|
2015-03-12 03:52:44 -03:00
|
|
|
if (_fit_step >= 35) {
|
2019-11-26 18:11:54 -04:00
|
|
|
if (fit_acceptable() && fix_radius() && calculate_orientation()) {
|
2019-11-21 23:57:17 -04:00
|
|
|
set_status(Status::SUCCESS);
|
2015-03-07 11:49:11 -04:00
|
|
|
} else {
|
2019-11-21 23:57:17 -04:00
|
|
|
set_status(Status::FAILED);
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
2015-03-12 03:52:44 -03:00
|
|
|
} else if (_fit_step < 15) {
|
|
|
|
run_sphere_fit();
|
|
|
|
_fit_step++;
|
|
|
|
} else {
|
|
|
|
run_ellipsoid_fit();
|
|
|
|
_fit_step++;
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
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
|
2019-11-14 22:14:24 -04:00
|
|
|
{
|
2019-11-21 23:57:17 -04:00
|
|
|
return _status == Status::RUNNING_STEP_ONE || _status == Status::RUNNING_STEP_TWO;
|
2015-03-09 22:02:46 -03:00
|
|
|
}
|
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
// fitting method for use in thread
|
|
|
|
bool CompassCalibrator::_fitting() const
|
2019-11-14 22:14:24 -04:00
|
|
|
{
|
2020-04-14 11:09:12 -03:00
|
|
|
return _running() && (_samples_collected == COMPASS_CAL_NUM_SAMPLES);
|
2015-03-09 22:02:46 -03:00
|
|
|
}
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// initialize fitness before starting a fit
|
2019-11-14 22:14:24 -04:00
|
|
|
void CompassCalibrator::initialize_fit()
|
|
|
|
{
|
2015-03-09 22:02:46 -03:00
|
|
|
if (_samples_collected != 0) {
|
|
|
|
_fitness = calc_mean_squared_residuals(_params);
|
|
|
|
} else {
|
|
|
|
_fitness = 1.0e30f;
|
|
|
|
}
|
|
|
|
_initial_fitness = _fitness;
|
2019-11-19 02:02:58 -04:00
|
|
|
_sphere_lambda = 1.0f;
|
|
|
|
_ellipsoid_lambda = 1.0f;
|
2015-03-09 22:02:46 -03:00
|
|
|
_fit_step = 0;
|
|
|
|
}
|
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
void CompassCalibrator::reset_state()
|
|
|
|
{
|
2015-03-07 11:49:11 -04:00
|
|
|
_samples_collected = 0;
|
|
|
|
_samples_thinned = 0;
|
2015-03-09 22:02:46 -03:00
|
|
|
_params.radius = 200;
|
|
|
|
_params.offset.zero();
|
|
|
|
_params.diag = Vector3f(1.0f,1.0f,1.0f);
|
|
|
|
_params.offdiag.zero();
|
2019-11-26 18:11:54 -04:00
|
|
|
_params.scale_factor = 0;
|
2015-03-09 22:02:46 -03:00
|
|
|
|
2016-04-22 19:21:18 -03:00
|
|
|
memset(_completion_mask, 0, sizeof(_completion_mask));
|
2015-03-09 22:02:46 -03:00
|
|
|
initialize_fit();
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2019-11-21 23:57:17 -04:00
|
|
|
bool CompassCalibrator::set_status(CompassCalibrator::Status status)
|
2019-11-14 22:14:24 -04:00
|
|
|
{
|
2019-11-21 23:57:17 -04:00
|
|
|
if (status != Status::NOT_STARTED && _status == status) {
|
2015-03-07 11:49:11 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
switch (status) {
|
2019-11-21 23:57:17 -04:00
|
|
|
case Status::NOT_STARTED:
|
2015-03-07 11:49:11 -04:00
|
|
|
reset_state();
|
2019-11-21 23:57:17 -04:00
|
|
|
_status = Status::NOT_STARTED;
|
2019-11-14 22:14:24 -04:00
|
|
|
if (_sample_buffer != nullptr) {
|
2015-03-07 11:49:11 -04:00
|
|
|
free(_sample_buffer);
|
2016-10-30 02:24:21 -03:00
|
|
|
_sample_buffer = nullptr;
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
return true;
|
|
|
|
|
2019-11-21 23:57:17 -04:00
|
|
|
case Status::WAITING_TO_START:
|
2015-03-07 11:49:11 -04:00
|
|
|
reset_state();
|
2019-11-21 23:57:17 -04:00
|
|
|
_status = Status::WAITING_TO_START;
|
|
|
|
set_status(Status::RUNNING_STEP_ONE);
|
2015-03-07 11:49:11 -04:00
|
|
|
return true;
|
|
|
|
|
2019-11-21 23:57:17 -04:00
|
|
|
case Status::RUNNING_STEP_ONE:
|
|
|
|
if (_status != Status::WAITING_TO_START) {
|
2015-03-07 11:49:11 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// on first attempt delay start if requested by caller
|
2019-11-14 22:14:24 -04:00
|
|
|
if (_attempt == 1 && (AP_HAL::millis()-_start_time_ms)*1.0e-3f < _delay_start_sec) {
|
2015-03-07 11:49:11 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2016-10-30 02:24:21 -03:00
|
|
|
if (_sample_buffer == nullptr) {
|
2019-11-14 22:14:24 -04:00
|
|
|
_sample_buffer = (CompassSample*)calloc(COMPASS_CAL_NUM_SAMPLES, sizeof(CompassSample));
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
2019-11-14 22:14:24 -04:00
|
|
|
if (_sample_buffer != nullptr) {
|
2015-03-09 22:02:46 -03:00
|
|
|
initialize_fit();
|
2019-11-21 23:57:17 -04:00
|
|
|
_status = Status::RUNNING_STEP_ONE;
|
2015-03-07 11:49:11 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return false;
|
|
|
|
|
2019-11-21 23:57:17 -04:00
|
|
|
case Status::RUNNING_STEP_TWO:
|
|
|
|
if (_status != Status::RUNNING_STEP_ONE) {
|
2015-03-07 11:49:11 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
thin_samples();
|
2015-03-09 22:02:46 -03:00
|
|
|
initialize_fit();
|
2019-11-21 23:57:17 -04:00
|
|
|
_status = Status::RUNNING_STEP_TWO;
|
2015-03-07 11:49:11 -04:00
|
|
|
return true;
|
|
|
|
|
2019-11-21 23:57:17 -04:00
|
|
|
case Status::SUCCESS:
|
|
|
|
if (_status != Status::RUNNING_STEP_TWO) {
|
2015-03-07 11:49:11 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
if (_sample_buffer != nullptr) {
|
2015-03-07 11:49:11 -04:00
|
|
|
free(_sample_buffer);
|
2016-10-30 02:24:21 -03:00
|
|
|
_sample_buffer = nullptr;
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2019-11-21 23:57:17 -04:00
|
|
|
_status = Status::SUCCESS;
|
2015-03-07 11:49:11 -04:00
|
|
|
return true;
|
|
|
|
|
2019-11-21 23:57:17 -04:00
|
|
|
case Status::FAILED:
|
|
|
|
if (_status == Status::BAD_ORIENTATION ||
|
|
|
|
_status == Status::BAD_RADIUS) {
|
2018-07-17 03:11:28 -03:00
|
|
|
// don't overwrite bad orientation status
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
FALLTHROUGH;
|
2019-11-21 23:57:17 -04:00
|
|
|
|
|
|
|
case Status::BAD_ORIENTATION:
|
|
|
|
case Status::BAD_RADIUS:
|
|
|
|
if (_status == Status::NOT_STARTED) {
|
2015-03-07 11:49:11 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2019-11-21 23:57:17 -04:00
|
|
|
if (_retry && set_status(Status::WAITING_TO_START)) {
|
2015-03-07 11:49:11 -04:00
|
|
|
_attempt++;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
if (_sample_buffer != nullptr) {
|
2015-03-07 11:49:11 -04:00
|
|
|
free(_sample_buffer);
|
2016-10-30 02:24:21 -03:00
|
|
|
_sample_buffer = nullptr;
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2018-07-17 03:11:28 -03:00
|
|
|
_status = status;
|
2015-03-07 11:49:11 -04:00
|
|
|
return true;
|
2019-11-14 22:14:24 -04:00
|
|
|
|
2015-03-07 11:49:11 -04:00
|
|
|
default:
|
|
|
|
return false;
|
|
|
|
};
|
|
|
|
}
|
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
bool CompassCalibrator::fit_acceptable()
|
|
|
|
{
|
|
|
|
if (!isnan(_fitness) &&
|
2019-11-26 18:11:54 -04:00
|
|
|
_params.radius > FIELD_RADIUS_MIN && _params.radius < FIELD_RADIUS_MAX &&
|
2017-04-03 19:38:14 -03:00
|
|
|
fabsf(_params.offset.x) < _offset_max &&
|
|
|
|
fabsf(_params.offset.y) < _offset_max &&
|
|
|
|
fabsf(_params.offset.z) < _offset_max &&
|
2015-03-12 03:42:50 -03:00
|
|
|
_params.diag.x > 0.2f && _params.diag.x < 5.0f &&
|
|
|
|
_params.diag.y > 0.2f && _params.diag.y < 5.0f &&
|
|
|
|
_params.diag.z > 0.2f && _params.diag.z < 5.0f &&
|
2019-11-14 22:14:24 -04:00
|
|
|
fabsf(_params.offdiag.x) < 1.0f && //absolute of sine/cosine output cannot be greater than 1
|
|
|
|
fabsf(_params.offdiag.y) < 1.0f &&
|
|
|
|
fabsf(_params.offdiag.z) < 1.0f ) {
|
2015-03-08 14:57:52 -03:00
|
|
|
return _fitness <= sq(_tolerance);
|
|
|
|
}
|
|
|
|
return false;
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
void CompassCalibrator::thin_samples()
|
|
|
|
{
|
|
|
|
if (_sample_buffer == nullptr) {
|
2015-03-07 11:49:11 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
_samples_thinned = 0;
|
|
|
|
// shuffle the samples http://en.wikipedia.org/wiki/Fisher%E2%80%93Yates_shuffle
|
|
|
|
// this is so that adjacent samples don't get sequentially eliminated
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint16_t i=_samples_collected-1; i>=1; i--) {
|
2016-11-17 01:23:22 -04:00
|
|
|
uint16_t j = get_random16() % (i+1);
|
2015-03-07 11:49:11 -04:00
|
|
|
CompassSample temp = _sample_buffer[i];
|
|
|
|
_sample_buffer[i] = _sample_buffer[j];
|
|
|
|
_sample_buffer[j] = temp;
|
|
|
|
}
|
|
|
|
|
2019-11-20 07:15:40 -04:00
|
|
|
// remove any samples that are close together
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint16_t i=0; i < _samples_collected; i++) {
|
2019-11-20 07:15:40 -04:00
|
|
|
if (!accept_sample(_sample_buffer[i], i)) {
|
2015-03-07 11:49:11 -04:00
|
|
|
_sample_buffer[i] = _sample_buffer[_samples_collected-1];
|
2019-11-14 22:14:24 -04:00
|
|
|
_samples_collected--;
|
|
|
|
_samples_thinned++;
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
}
|
2016-04-22 19:21:18 -03:00
|
|
|
|
|
|
|
update_completion_mask();
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2015-10-19 10:58:30 -03:00
|
|
|
/*
|
|
|
|
* The sample acceptance distance is determined as follows:
|
|
|
|
* For any regular polyhedron with triangular faces, the angle theta subtended
|
|
|
|
* by two closest points is defined as
|
|
|
|
*
|
|
|
|
* theta = arccos(cos(A)/(1-cos(A)))
|
|
|
|
*
|
|
|
|
* Where:
|
|
|
|
* A = (4pi/F + pi)/3
|
|
|
|
* and
|
|
|
|
* F = 2V - 4 is the number of faces for the polyhedron in consideration,
|
|
|
|
* which depends on the number of vertices V
|
|
|
|
*
|
|
|
|
* The above equation was proved after solving for spherical triangular excess
|
|
|
|
* and related equations.
|
|
|
|
*/
|
2019-11-20 07:15:40 -04:00
|
|
|
bool CompassCalibrator::accept_sample(const Vector3f& sample, uint16_t skip_index)
|
2015-03-07 11:49:11 -04:00
|
|
|
{
|
2015-10-19 10:58:30 -03:00
|
|
|
static const uint16_t faces = (2 * COMPASS_CAL_NUM_SAMPLES - 4);
|
2016-02-25 13:13:02 -04:00
|
|
|
static const float a = (4.0f * M_PI / (3.0f * faces)) + M_PI / 3.0f;
|
2015-10-19 10:58:30 -03:00
|
|
|
static const float theta = 0.5f * acosf(cosf(a) / (1.0f - cosf(a)));
|
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
if (_sample_buffer == nullptr) {
|
2015-03-07 11:49:11 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2015-07-22 15:05:48 -03:00
|
|
|
float min_distance = _params.radius * 2*sinf(theta/2);
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint16_t i = 0; i<_samples_collected; i++) {
|
2019-11-20 07:15:40 -04:00
|
|
|
if (i != skip_index) {
|
|
|
|
float distance = (sample - _sample_buffer[i].get()).length();
|
|
|
|
if (distance < min_distance) {
|
|
|
|
return false;
|
|
|
|
}
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2019-11-20 07:15:40 -04:00
|
|
|
bool CompassCalibrator::accept_sample(const CompassSample& sample, uint16_t skip_index)
|
2019-11-14 22:14:24 -04:00
|
|
|
{
|
2019-11-20 07:15:40 -04:00
|
|
|
return accept_sample(sample.get(), skip_index);
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
float CompassCalibrator::calc_residual(const Vector3f& sample, const param_t& params) const
|
|
|
|
{
|
2015-03-07 11:49:11 -04:00
|
|
|
Matrix3f softiron(
|
2015-03-09 22:02:46 -03:00
|
|
|
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
|
2015-03-07 11:49:11 -04:00
|
|
|
);
|
2015-03-09 22:02:46 -03:00
|
|
|
return params.radius - (softiron*(sample+params.offset)).length();
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// calc the fitness given a set of parameters (offsets, diagonals, off diagonals)
|
2015-03-09 22:02:46 -03:00
|
|
|
float CompassCalibrator::calc_mean_squared_residuals(const param_t& params) const
|
2015-03-07 11:49:11 -04:00
|
|
|
{
|
2019-11-14 22:14:24 -04:00
|
|
|
if (_sample_buffer == nullptr || _samples_collected == 0) {
|
2015-03-09 22:02:46 -03:00
|
|
|
return 1.0e30f;
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
float sum = 0.0f;
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint16_t i=0; i < _samples_collected; i++) {
|
2015-03-07 11:49:11 -04:00
|
|
|
Vector3f sample = _sample_buffer[i].get();
|
2015-03-09 22:02:46 -03:00
|
|
|
float resid = calc_residual(sample, params);
|
2015-03-07 11:49:11 -04:00
|
|
|
sum += sq(resid);
|
|
|
|
}
|
|
|
|
sum /= _samples_collected;
|
|
|
|
return sum;
|
|
|
|
}
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// calculate initial offsets by simply taking the average values of the samples
|
|
|
|
void CompassCalibrator::calc_initial_offset()
|
|
|
|
{
|
|
|
|
// Set initial offset to the average value of the samples
|
|
|
|
_params.offset.zero();
|
|
|
|
for (uint16_t k = 0; k < _samples_collected; k++) {
|
|
|
|
_params.offset -= _sample_buffer[k].get();
|
|
|
|
}
|
|
|
|
_params.offset /= _samples_collected;
|
|
|
|
}
|
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
void CompassCalibrator::calc_sphere_jacob(const Vector3f& sample, const param_t& params, float* ret) const
|
|
|
|
{
|
2015-03-09 22:02:46 -03:00
|
|
|
const Vector3f &offset = params.offset;
|
|
|
|
const Vector3f &diag = params.diag;
|
|
|
|
const Vector3f &offdiag = params.offdiag;
|
2015-03-08 15:00:02 -03:00
|
|
|
Matrix3f softiron(
|
|
|
|
diag.x , offdiag.x , offdiag.y,
|
|
|
|
offdiag.x , diag.y , offdiag.z,
|
|
|
|
offdiag.y , offdiag.z , diag.z
|
|
|
|
);
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-03-08 15:00:02 -03:00
|
|
|
float A = (diag.x * (sample.x + offset.x)) + (offdiag.x * (sample.y + offset.y)) + (offdiag.y * (sample.z + offset.z));
|
|
|
|
float B = (offdiag.x * (sample.x + offset.x)) + (diag.y * (sample.y + offset.y)) + (offdiag.z * (sample.z + offset.z));
|
|
|
|
float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z));
|
|
|
|
float length = (softiron*(sample+offset)).length();
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-05-29 05:00:27 -03:00
|
|
|
// 0: partial derivative (radius wrt fitness fn) fn operated on sample
|
2015-03-12 03:42:37 -03:00
|
|
|
ret[0] = 1.0f;
|
2015-05-29 05:00:27 -03:00
|
|
|
// 1-3: partial derivative (offsets wrt fitness fn) fn operated on sample
|
2015-03-09 22:02:46 -03:00
|
|
|
ret[1] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length);
|
|
|
|
ret[2] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length);
|
|
|
|
ret[3] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length);
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// run sphere fit to calculate diagonals and offdiagonals
|
2015-03-09 22:02:46 -03:00
|
|
|
void CompassCalibrator::run_sphere_fit()
|
2015-03-07 11:49:11 -04:00
|
|
|
{
|
2019-11-14 22:14:24 -04:00
|
|
|
if (_sample_buffer == nullptr) {
|
2015-03-07 11:49:11 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2015-03-09 22:02:46 -03:00
|
|
|
const float lma_damping = 10.0f;
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// take backup of fitness and parameters so we can determine later if this fit has improved the calibration
|
2015-03-09 22:02:46 -03:00
|
|
|
float fitness = _fitness;
|
|
|
|
float fit1, fit2;
|
|
|
|
param_t fit1_params, fit2_params;
|
|
|
|
fit1_params = fit2_params = _params;
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2016-11-18 15:05:36 -04:00
|
|
|
float JTJ[COMPASS_CAL_NUM_SPHERE_PARAMS*COMPASS_CAL_NUM_SPHERE_PARAMS] = { };
|
|
|
|
float JTJ2[COMPASS_CAL_NUM_SPHERE_PARAMS*COMPASS_CAL_NUM_SPHERE_PARAMS] = { };
|
|
|
|
float JTFI[COMPASS_CAL_NUM_SPHERE_PARAMS] = { };
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-07-29 20:26:10 -03:00
|
|
|
// Gauss Newton Part common for all kind of extensions including LM
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint16_t k = 0; k<_samples_collected; k++) {
|
2015-03-09 22:02:46 -03:00
|
|
|
Vector3f sample = _sample_buffer[k].get();
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-03-09 22:02:46 -03:00
|
|
|
float sphere_jacob[COMPASS_CAL_NUM_SPHERE_PARAMS];
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-03-09 22:02:46 -03:00
|
|
|
calc_sphere_jacob(sample, fit1_params, sphere_jacob);
|
2015-03-08 14:52:52 -03:00
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint8_t i = 0;i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) {
|
2015-03-09 22:02:46 -03:00
|
|
|
// compute JTJ
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint8_t j = 0; j < COMPASS_CAL_NUM_SPHERE_PARAMS; j++) {
|
2015-03-09 22:02:46 -03:00
|
|
|
JTJ[i*COMPASS_CAL_NUM_SPHERE_PARAMS+j] += sphere_jacob[i] * sphere_jacob[j];
|
|
|
|
JTJ2[i*COMPASS_CAL_NUM_SPHERE_PARAMS+j] += sphere_jacob[i] * sphere_jacob[j]; //a backup JTJ for LM
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
2015-03-09 22:02:46 -03:00
|
|
|
// compute JTFI
|
|
|
|
JTFI[i] += sphere_jacob[i] * calc_residual(sample, fit1_params);
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
2015-03-09 22:02:46 -03:00
|
|
|
}
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-07-29 20:26:10 -03:00
|
|
|
//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
|
2019-11-19 02:02:58 -04:00
|
|
|
// refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint8_t i = 0; i < COMPASS_CAL_NUM_SPHERE_PARAMS; i++) {
|
2015-03-09 22:02:46 -03:00
|
|
|
JTJ[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda;
|
|
|
|
JTJ2[i*COMPASS_CAL_NUM_SPHERE_PARAMS+i] += _sphere_lambda/lma_damping;
|
|
|
|
}
|
2015-03-08 14:52:52 -03:00
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
if (!inverse(JTJ, JTJ, 4)) {
|
2015-03-09 22:02:46 -03:00
|
|
|
return;
|
|
|
|
}
|
2015-03-08 14:52:52 -03:00
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
if (!inverse(JTJ2, JTJ2, 4)) {
|
2015-03-09 22:02:46 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// extract radius, offset, diagonals and offdiagonal parameters
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint8_t row=0; row < COMPASS_CAL_NUM_SPHERE_PARAMS; row++) {
|
|
|
|
for (uint8_t col=0; col < COMPASS_CAL_NUM_SPHERE_PARAMS; col++) {
|
2015-03-09 22:02:46 -03:00
|
|
|
fit1_params.get_sphere_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_SPHERE_PARAMS+col];
|
|
|
|
fit2_params.get_sphere_params()[row] -= JTFI[col] * JTJ2[row*COMPASS_CAL_NUM_SPHERE_PARAMS+col];
|
2015-03-08 14:52:52 -03:00
|
|
|
}
|
2015-03-09 22:02:46 -03:00
|
|
|
}
|
2015-03-08 14:52:52 -03:00
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// calculate fitness of two possible sets of parameters
|
2015-03-09 22:02:46 -03:00
|
|
|
fit1 = calc_mean_squared_residuals(fit1_params);
|
|
|
|
fit2 = calc_mean_squared_residuals(fit2_params);
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// decide which of the two sets of parameters is best and store in fit1_params
|
2019-11-14 22:14:24 -04:00
|
|
|
if (fit1 > _fitness && fit2 > _fitness) {
|
2019-11-19 02:02:58 -04:00
|
|
|
// if neither set of parameters provided better results, increase lambda
|
2015-03-09 22:02:46 -03:00
|
|
|
_sphere_lambda *= lma_damping;
|
2019-11-14 22:14:24 -04:00
|
|
|
} else if (fit2 < _fitness && fit2 < fit1) {
|
2019-11-19 02:02:58 -04:00
|
|
|
// if fit2 was better we will use it. decrease lambda
|
2015-03-09 22:02:46 -03:00
|
|
|
_sphere_lambda /= lma_damping;
|
|
|
|
fit1_params = fit2_params;
|
|
|
|
fitness = fit2;
|
2019-11-14 22:14:24 -04:00
|
|
|
} else if (fit1 < _fitness) {
|
2015-03-09 22:02:46 -03:00
|
|
|
fitness = fit1;
|
|
|
|
}
|
2015-07-29 20:26:10 -03:00
|
|
|
//--------------------Levenberg-Marquardt-part-ends-here--------------------------------//
|
2015-03-08 14:52:52 -03:00
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// store new parameters and update fitness
|
2019-11-14 22:14:24 -04:00
|
|
|
if (!isnan(fitness) && fitness < _fitness) {
|
2015-03-09 22:02:46 -03:00
|
|
|
_fitness = fitness;
|
|
|
|
_params = fit1_params;
|
2016-04-22 19:21:18 -03:00
|
|
|
update_completion_mask();
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
void CompassCalibrator::calc_ellipsoid_jacob(const Vector3f& sample, const param_t& params, float* ret) const
|
|
|
|
{
|
2015-03-09 22:02:46 -03:00
|
|
|
const Vector3f &offset = params.offset;
|
|
|
|
const Vector3f &diag = params.diag;
|
|
|
|
const Vector3f &offdiag = params.offdiag;
|
2015-03-08 15:00:02 -03:00
|
|
|
Matrix3f softiron(
|
|
|
|
diag.x , offdiag.x , offdiag.y,
|
|
|
|
offdiag.x , diag.y , offdiag.z,
|
|
|
|
offdiag.y , offdiag.z , diag.z
|
|
|
|
);
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-03-08 15:00:02 -03:00
|
|
|
float A = (diag.x * (sample.x + offset.x)) + (offdiag.x * (sample.y + offset.y)) + (offdiag.y * (sample.z + offset.z));
|
|
|
|
float B = (offdiag.x * (sample.x + offset.x)) + (diag.y * (sample.y + offset.y)) + (offdiag.z * (sample.z + offset.z));
|
|
|
|
float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) + (diag.z * (sample.z + offset.z));
|
|
|
|
float length = (softiron*(sample+offset)).length();
|
|
|
|
|
2015-05-29 05:00:27 -03:00
|
|
|
// 0-2: partial derivative (offset wrt fitness fn) fn operated on sample
|
2015-03-09 22:02:46 -03:00
|
|
|
ret[0] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C))/length);
|
|
|
|
ret[1] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C))/length);
|
|
|
|
ret[2] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C))/length);
|
2015-05-29 05:00:27 -03:00
|
|
|
// 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample
|
2015-03-09 22:02:46 -03:00
|
|
|
ret[3] = -1.0f * ((sample.x + offset.x) * A)/length;
|
|
|
|
ret[4] = -1.0f * ((sample.y + offset.y) * B)/length;
|
|
|
|
ret[5] = -1.0f * ((sample.z + offset.z) * C)/length;
|
2015-05-29 05:00:27 -03:00
|
|
|
// 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample
|
2015-03-12 03:42:37 -03:00
|
|
|
ret[6] = -1.0f * (((sample.y + offset.y) * A) + ((sample.x + offset.x) * B))/length;
|
2015-03-09 22:02:46 -03:00
|
|
|
ret[7] = -1.0f * (((sample.z + offset.z) * A) + ((sample.x + offset.x) * C))/length;
|
|
|
|
ret[8] = -1.0f * (((sample.z + offset.z) * B) + ((sample.y + offset.y) * C))/length;
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2015-03-09 22:02:46 -03:00
|
|
|
void CompassCalibrator::run_ellipsoid_fit()
|
2015-03-07 11:49:11 -04:00
|
|
|
{
|
2019-11-14 22:14:24 -04:00
|
|
|
if (_sample_buffer == nullptr) {
|
2015-03-07 11:49:11 -04:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2015-03-09 22:02:46 -03:00
|
|
|
const float lma_damping = 10.0f;
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// take backup of fitness and parameters so we can determine later if this fit has improved the calibration
|
2015-03-09 22:02:46 -03:00
|
|
|
float fitness = _fitness;
|
|
|
|
float fit1, fit2;
|
|
|
|
param_t fit1_params, fit2_params;
|
|
|
|
fit1_params = fit2_params = _params;
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2016-11-18 15:05:36 -04:00
|
|
|
float JTJ[COMPASS_CAL_NUM_ELLIPSOID_PARAMS*COMPASS_CAL_NUM_ELLIPSOID_PARAMS] = { };
|
|
|
|
float JTJ2[COMPASS_CAL_NUM_ELLIPSOID_PARAMS*COMPASS_CAL_NUM_ELLIPSOID_PARAMS] = { };
|
|
|
|
float JTFI[COMPASS_CAL_NUM_ELLIPSOID_PARAMS] = { };
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-07-29 20:26:10 -03:00
|
|
|
// Gauss Newton Part common for all kind of extensions including LM
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint16_t k = 0; k<_samples_collected; k++) {
|
2015-03-09 22:02:46 -03:00
|
|
|
Vector3f sample = _sample_buffer[k].get();
|
|
|
|
|
|
|
|
float ellipsoid_jacob[COMPASS_CAL_NUM_ELLIPSOID_PARAMS];
|
|
|
|
|
|
|
|
calc_ellipsoid_jacob(sample, fit1_params, ellipsoid_jacob);
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint8_t i = 0;i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++) {
|
2015-03-09 22:02:46 -03:00
|
|
|
// compute JTJ
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint8_t j = 0; j < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; j++) {
|
2015-03-09 22:02:46 -03:00
|
|
|
JTJ [i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+j] += ellipsoid_jacob[i] * ellipsoid_jacob[j];
|
|
|
|
JTJ2[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+j] += ellipsoid_jacob[i] * ellipsoid_jacob[j];
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
2015-03-09 22:02:46 -03:00
|
|
|
// compute JTFI
|
|
|
|
JTFI[i] += ellipsoid_jacob[i] * calc_residual(sample, fit1_params);
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
2015-03-09 22:02:46 -03:00
|
|
|
}
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2015-07-29 20:26:10 -03:00
|
|
|
//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
|
2015-05-29 05:00:27 -03:00
|
|
|
//refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint8_t i = 0; i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++) {
|
2015-03-09 22:02:46 -03:00
|
|
|
JTJ[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+i] += _ellipsoid_lambda;
|
|
|
|
JTJ2[i*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+i] += _ellipsoid_lambda/lma_damping;
|
|
|
|
}
|
2015-03-09 11:38:05 -03:00
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
if (!inverse(JTJ, JTJ, 9)) {
|
2015-03-09 22:02:46 -03:00
|
|
|
return;
|
|
|
|
}
|
2015-03-09 11:38:05 -03:00
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
if (!inverse(JTJ2, JTJ2, 9)) {
|
2015-03-09 22:02:46 -03:00
|
|
|
return;
|
|
|
|
}
|
2015-03-09 11:38:05 -03:00
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// extract radius, offset, diagonals and offdiagonal parameters
|
2019-11-14 22:14:24 -04:00
|
|
|
for (uint8_t row=0; row < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; row++) {
|
|
|
|
for (uint8_t col=0; col < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; col++) {
|
2015-03-09 22:02:46 -03:00
|
|
|
fit1_params.get_ellipsoid_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col];
|
|
|
|
fit2_params.get_ellipsoid_params()[row] -= JTFI[col] * JTJ2[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col];
|
2015-03-09 11:38:05 -03:00
|
|
|
}
|
2015-03-09 22:02:46 -03:00
|
|
|
}
|
2015-03-07 11:49:11 -04:00
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// calculate fitness of two possible sets of parameters
|
2015-03-09 22:02:46 -03:00
|
|
|
fit1 = calc_mean_squared_residuals(fit1_params);
|
|
|
|
fit2 = calc_mean_squared_residuals(fit2_params);
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// decide which of the two sets of parameters is best and store in fit1_params
|
2019-11-14 22:14:24 -04:00
|
|
|
if (fit1 > _fitness && fit2 > _fitness) {
|
2019-11-19 02:02:58 -04:00
|
|
|
// if neither set of parameters provided better results, increase lambda
|
2015-03-09 22:02:46 -03:00
|
|
|
_ellipsoid_lambda *= lma_damping;
|
2019-11-14 22:14:24 -04:00
|
|
|
} else if (fit2 < _fitness && fit2 < fit1) {
|
2019-11-19 02:02:58 -04:00
|
|
|
// if fit2 was better we will use it. decrease lambda
|
2015-03-09 22:02:46 -03:00
|
|
|
_ellipsoid_lambda /= lma_damping;
|
|
|
|
fit1_params = fit2_params;
|
|
|
|
fitness = fit2;
|
2019-11-14 22:14:24 -04:00
|
|
|
} else if (fit1 < _fitness) {
|
2015-03-09 22:02:46 -03:00
|
|
|
fitness = fit1;
|
|
|
|
}
|
|
|
|
//--------------------Levenberg-part-ends-here--------------------------------//
|
|
|
|
|
2019-11-19 02:02:58 -04:00
|
|
|
// store new parameters and update fitness
|
2019-11-14 22:14:24 -04:00
|
|
|
if (fitness < _fitness) {
|
2015-03-09 22:02:46 -03:00
|
|
|
_fitness = fitness;
|
|
|
|
_params = fit1_params;
|
2016-04-22 19:21:18 -03:00
|
|
|
update_completion_mask();
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2015-03-09 11:35:26 -03:00
|
|
|
|
2015-03-07 11:49:11 -04:00
|
|
|
//////////////////////////////////////////////////////////
|
|
|
|
//////////// CompassSample public interface //////////////
|
|
|
|
//////////////////////////////////////////////////////////
|
|
|
|
|
2016-09-16 16:07:53 -03:00
|
|
|
#define COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(__X) ((int16_t)constrain_float(roundf(__X*8.0f), INT16_MIN, INT16_MAX))
|
|
|
|
#define COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(__X) (__X/8.0f)
|
2016-09-16 16:04:10 -03:00
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
Vector3f CompassCalibrator::CompassSample::get() const
|
|
|
|
{
|
2016-09-16 16:04:10 -03:00
|
|
|
return Vector3f(COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(x),
|
|
|
|
COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(y),
|
|
|
|
COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(z));
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
void CompassCalibrator::CompassSample::set(const Vector3f &in)
|
|
|
|
{
|
2016-09-16 16:04:10 -03:00
|
|
|
x = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.x);
|
|
|
|
y = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.y);
|
|
|
|
z = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.z);
|
2015-03-07 11:49:11 -04:00
|
|
|
}
|
2018-07-16 05:21:53 -03:00
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
void CompassCalibrator::AttitudeSample::set_from_ahrs(void)
|
|
|
|
{
|
2018-07-16 05:21:53 -03:00
|
|
|
const Matrix3f &dcm = AP::ahrs().get_DCM_rotation_body_to_ned();
|
|
|
|
float roll_rad, pitch_rad, yaw_rad;
|
|
|
|
dcm.to_euler(&roll_rad, &pitch_rad, &yaw_rad);
|
|
|
|
roll = constrain_int16(127 * (roll_rad / M_PI), -127, 127);
|
|
|
|
pitch = constrain_int16(127 * (pitch_rad / M_PI_2), -127, 127);
|
|
|
|
yaw = constrain_int16(127 * (yaw_rad / M_PI), -127, 127);
|
|
|
|
}
|
|
|
|
|
2019-11-14 22:14:24 -04:00
|
|
|
Matrix3f CompassCalibrator::AttitudeSample::get_rotmat(void)
|
|
|
|
{
|
2018-07-16 05:21:53 -03:00
|
|
|
float roll_rad, pitch_rad, yaw_rad;
|
|
|
|
roll_rad = roll * (M_PI / 127);
|
|
|
|
pitch_rad = pitch * (M_PI_2 / 127);
|
|
|
|
yaw_rad = yaw * (M_PI / 127);
|
|
|
|
Matrix3f dcm;
|
|
|
|
dcm.from_euler(roll_rad, pitch_rad, yaw_rad);
|
|
|
|
return dcm;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
2018-07-17 21:09:21 -03:00
|
|
|
calculate the implied earth field for a compass sample and compass
|
|
|
|
rotation. This is used to check for consistency between
|
|
|
|
samples.
|
|
|
|
|
|
|
|
If the orientation is correct then when rotated the same (or
|
|
|
|
similar) earth field should be given for all samples.
|
|
|
|
|
|
|
|
Note that this earth field uses an arbitrary north reference, so it
|
|
|
|
may not match the true earth field.
|
2018-07-16 05:21:53 -03:00
|
|
|
*/
|
|
|
|
Vector3f CompassCalibrator::calculate_earth_field(CompassSample &sample, enum Rotation r)
|
|
|
|
{
|
|
|
|
Vector3f v = sample.get();
|
|
|
|
|
|
|
|
// convert the sample back to sensor frame
|
|
|
|
v.rotate_inverse(_orientation);
|
|
|
|
|
|
|
|
// rotate to body frame for this rotation
|
|
|
|
v.rotate(r);
|
|
|
|
|
2018-07-17 21:09:21 -03:00
|
|
|
// apply offsets, rotating them for the orientation we are testing
|
2018-07-16 05:21:53 -03:00
|
|
|
Vector3f rot_offsets = _params.offset;
|
|
|
|
rot_offsets.rotate_inverse(_orientation);
|
|
|
|
|
|
|
|
rot_offsets.rotate(r);
|
2019-11-14 22:14:24 -04:00
|
|
|
|
2018-07-16 05:21:53 -03:00
|
|
|
v += rot_offsets;
|
|
|
|
|
2018-07-17 21:09:21 -03:00
|
|
|
// rotate the sample from body frame back to earth frame
|
2018-07-16 05:21:53 -03:00
|
|
|
Matrix3f rot = sample.att.get_rotmat();
|
|
|
|
|
|
|
|
Vector3f efield = rot * v;
|
2018-07-17 21:09:21 -03:00
|
|
|
|
|
|
|
// earth field is the mag sample in earth frame
|
2018-07-16 05:21:53 -03:00
|
|
|
return efield;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
2018-07-17 21:09:21 -03:00
|
|
|
calculate compass orientation using the attitude estimate associated
|
|
|
|
with each sample, and fix orientation on external compasses if
|
|
|
|
the feature is enabled
|
2018-07-16 05:21:53 -03:00
|
|
|
*/
|
2018-07-16 05:42:06 -03:00
|
|
|
bool CompassCalibrator::calculate_orientation(void)
|
2018-07-16 05:21:53 -03:00
|
|
|
{
|
2018-07-17 21:09:21 -03:00
|
|
|
if (!_check_orientation) {
|
2018-07-17 03:11:28 -03:00
|
|
|
// we are not checking orientation
|
2018-07-16 05:42:06 -03:00
|
|
|
return true;
|
2018-07-16 05:21:53 -03:00
|
|
|
}
|
|
|
|
|
2019-04-11 08:12:20 -03:00
|
|
|
// this function is very slow
|
2019-05-15 01:10:04 -03:00
|
|
|
EXPECT_DELAY_MS(1000);
|
2019-04-11 08:12:20 -03:00
|
|
|
|
2019-12-31 18:08:31 -04:00
|
|
|
float variance[ROTATION_MAX_AUTO_ROTATION+1] {};
|
2018-07-16 05:21:53 -03:00
|
|
|
|
2020-04-14 11:09:12 -03:00
|
|
|
_orientation_solution = _orientation;
|
|
|
|
|
2019-12-31 18:08:31 -04:00
|
|
|
for (enum Rotation r = ROTATION_NONE; r <= ROTATION_MAX_AUTO_ROTATION; r = (enum Rotation)(r+1)) {
|
2018-07-16 05:21:53 -03:00
|
|
|
// calculate the average implied earth field across all samples
|
|
|
|
Vector3f total_ef {};
|
|
|
|
for (uint32_t i=0; i<_samples_collected; i++) {
|
|
|
|
Vector3f efield = calculate_earth_field(_sample_buffer[i], r);
|
|
|
|
total_ef += efield;
|
|
|
|
}
|
|
|
|
Vector3f avg_efield = total_ef / _samples_collected;
|
|
|
|
|
|
|
|
// now calculate the square error for this rotation against the average earth field
|
|
|
|
for (uint32_t i=0; i<_samples_collected; i++) {
|
|
|
|
Vector3f efield = calculate_earth_field(_sample_buffer[i], r);
|
|
|
|
float err = (efield - avg_efield).length_squared();
|
2018-07-17 21:09:21 -03:00
|
|
|
// divide by number of samples collected to get the variance
|
|
|
|
variance[r] += err / _samples_collected;
|
2018-07-16 05:21:53 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-07-17 21:09:21 -03:00
|
|
|
// find the rotation with the lowest variance
|
2018-07-16 05:21:53 -03:00
|
|
|
enum Rotation besti = ROTATION_NONE;
|
2018-07-17 21:09:21 -03:00
|
|
|
float bestv = variance[0];
|
2019-12-31 18:08:31 -04:00
|
|
|
for (enum Rotation r = ROTATION_NONE; r <= ROTATION_MAX_AUTO_ROTATION; r = (enum Rotation)(r+1)) {
|
2018-07-17 21:09:21 -03:00
|
|
|
if (variance[r] < bestv) {
|
|
|
|
bestv = variance[r];
|
2018-07-16 05:21:53 -03:00
|
|
|
besti = r;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-07-17 21:09:21 -03:00
|
|
|
// consider this a pass if the best orientation is 2x better
|
|
|
|
// variance than 2nd best
|
|
|
|
const float variance_threshold = 2.0;
|
2019-11-14 22:14:24 -04:00
|
|
|
|
2018-07-17 21:09:21 -03:00
|
|
|
float second_best = besti==ROTATION_NONE?variance[1]:variance[0];
|
2018-09-19 00:48:17 -03:00
|
|
|
enum Rotation besti2 = ROTATION_NONE;
|
2019-12-31 18:08:31 -04:00
|
|
|
for (enum Rotation r = ROTATION_NONE; r <= ROTATION_MAX_AUTO_ROTATION; r = (enum Rotation)(r+1)) {
|
2018-09-19 00:48:17 -03:00
|
|
|
if (!rotation_equal(besti, r)) {
|
2018-07-17 21:09:21 -03:00
|
|
|
if (variance[r] < second_best) {
|
|
|
|
second_best = variance[r];
|
2018-09-19 00:48:17 -03:00
|
|
|
besti2 = r;
|
2018-07-16 05:21:53 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2018-07-17 03:11:28 -03:00
|
|
|
|
|
|
|
_orientation_confidence = second_best/bestv;
|
2019-11-14 22:14:24 -04:00
|
|
|
|
2018-07-16 05:42:06 -03:00
|
|
|
bool pass;
|
|
|
|
if (besti == _orientation) {
|
|
|
|
// if the orientation matched then allow for a low threshold
|
|
|
|
pass = true;
|
|
|
|
} else {
|
2018-07-17 21:09:21 -03:00
|
|
|
pass = _orientation_confidence > variance_threshold;
|
2018-07-16 05:42:06 -03:00
|
|
|
}
|
2018-07-16 05:21:53 -03:00
|
|
|
if (!pass) {
|
2020-03-29 19:49:53 -03:00
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Mag(%u) bad orientation: %u/%u %.1f", _compass_idx,
|
2018-09-19 00:48:17 -03:00
|
|
|
besti, besti2, (double)_orientation_confidence);
|
2020-03-29 19:49:53 -03:00
|
|
|
(void)besti2;
|
2018-07-16 05:21:53 -03:00
|
|
|
} else if (besti == _orientation) {
|
|
|
|
// no orientation change
|
2020-03-29 19:49:53 -03:00
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mag(%u) good orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence);
|
2018-07-17 21:09:21 -03:00
|
|
|
} else if (!_is_external || !_fix_orientation) {
|
2020-03-29 19:49:53 -03:00
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Mag(%u) internal bad orientation: %u %.1f", _compass_idx, besti, (double)_orientation_confidence);
|
2018-07-16 05:21:53 -03:00
|
|
|
} else {
|
2020-03-29 19:49:53 -03:00
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f", _compass_idx, besti, _orientation, (double)_orientation_confidence);
|
2018-07-16 05:21:53 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
if (!pass) {
|
2019-11-21 23:57:17 -04:00
|
|
|
set_status(Status::BAD_ORIENTATION);
|
2018-07-16 05:42:06 -03:00
|
|
|
return false;
|
2018-07-16 05:21:53 -03:00
|
|
|
}
|
2018-07-16 23:57:23 -03:00
|
|
|
|
|
|
|
if (_orientation == besti) {
|
|
|
|
// no orientation change
|
|
|
|
return true;
|
|
|
|
}
|
2018-07-17 03:11:28 -03:00
|
|
|
|
2018-07-17 21:09:21 -03:00
|
|
|
if (!_is_external || !_fix_orientation) {
|
|
|
|
// we won't change the orientation, but we set _orientation
|
|
|
|
// for reporting purposes
|
|
|
|
_orientation = besti;
|
2020-04-14 11:09:12 -03:00
|
|
|
_orientation_solution = besti;
|
2019-11-21 23:57:17 -04:00
|
|
|
set_status(Status::BAD_ORIENTATION);
|
2018-07-17 03:11:28 -03:00
|
|
|
return false;
|
|
|
|
}
|
2019-11-14 22:14:24 -04:00
|
|
|
|
2018-07-16 23:57:23 -03:00
|
|
|
// correct the offsets for the new orientation
|
|
|
|
Vector3f rot_offsets = _params.offset;
|
|
|
|
rot_offsets.rotate_inverse(_orientation);
|
|
|
|
rot_offsets.rotate(besti);
|
|
|
|
_params.offset = rot_offsets;
|
|
|
|
|
|
|
|
// rotate the samples for the new orientation
|
|
|
|
for (uint32_t i=0; i<_samples_collected; i++) {
|
|
|
|
Vector3f s = _sample_buffer[i].get();
|
|
|
|
s.rotate_inverse(_orientation);
|
|
|
|
s.rotate(besti);
|
|
|
|
_sample_buffer[i].set(s);
|
2018-07-16 05:21:53 -03:00
|
|
|
}
|
2018-07-16 23:57:23 -03:00
|
|
|
|
|
|
|
_orientation = besti;
|
2020-04-14 11:09:12 -03:00
|
|
|
_orientation_solution = besti;
|
2018-07-16 23:57:23 -03:00
|
|
|
|
|
|
|
// re-run the fit to get the diagonals and off-diagonals for the
|
|
|
|
// new orientation
|
|
|
|
initialize_fit();
|
|
|
|
run_sphere_fit();
|
|
|
|
run_ellipsoid_fit();
|
2019-11-14 22:14:24 -04:00
|
|
|
|
2018-07-16 23:57:23 -03:00
|
|
|
return fit_acceptable();
|
2018-07-16 05:21:53 -03:00
|
|
|
}
|
2019-11-26 18:11:54 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
fix radius of the fit to compensate for sensor scale factor errors
|
|
|
|
return false if radius is outside acceptable range
|
|
|
|
*/
|
|
|
|
bool CompassCalibrator::fix_radius(void)
|
|
|
|
{
|
|
|
|
if (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) {
|
|
|
|
// we don't have a position, leave scale factor as 0. This
|
|
|
|
// will disable use of WMM in the EKF. Users can manually set
|
|
|
|
// scale factor after calibration if it is known
|
|
|
|
_params.scale_factor = 0;
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
const struct Location &loc = AP::gps().location();
|
|
|
|
float intensity;
|
|
|
|
float declination;
|
|
|
|
float inclination;
|
|
|
|
AP_Declination::get_mag_field_ef(loc.lat * 1e-7f, loc.lng * 1e-7f, intensity, declination, inclination);
|
|
|
|
|
|
|
|
float expected_radius = intensity * 1000; // mGauss
|
|
|
|
float correction = expected_radius / _params.radius;
|
|
|
|
|
|
|
|
if (correction > COMPASS_MAX_SCALE_FACTOR || correction < COMPASS_MIN_SCALE_FACTOR) {
|
|
|
|
// don't allow more than 30% scale factor correction
|
2020-03-29 19:49:53 -03:00
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag(%u) bad radius %.0f expected %.0f",
|
2019-11-26 18:11:54 -04:00
|
|
|
_compass_idx,
|
|
|
|
_params.radius,
|
|
|
|
expected_radius);
|
2019-11-21 23:57:17 -04:00
|
|
|
set_status(Status::BAD_RADIUS);
|
2019-11-26 18:11:54 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
_params.scale_factor = correction;
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|