mirror of https://github.com/ArduPilot/ardupilot
AP_AccelCal: Add separate lib for accel calibration
This commit is contained in:
bugobliterator
Jonathan Challinger
parent
f70d9d26ba
commit
d24b5023f4
|
|
@ -0,0 +1,357 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
|
||||
#include "AP_AccelCal.h"
|
||||
#include <stdarg.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
const extern AP_HAL::HAL& hal;
|
||||
static bool _start_collect_sample;
|
||||
static void _snoop(const mavlink_message_t* msg);
|
||||
|
||||
void AP_AccelCal::update()
|
||||
{
|
||||
if (!get_calibrator(0)) {
|
||||
// no calibrators
|
||||
return;
|
||||
}
|
||||
|
||||
if (_started) {
|
||||
update_status();
|
||||
|
||||
AccelCalibrator *cal;
|
||||
uint8_t num_active_calibrators = 0;
|
||||
for(uint8_t i=0; (cal = get_calibrator(i)); i++) {
|
||||
num_active_calibrators++;
|
||||
}
|
||||
if (num_active_calibrators != _num_active_calibrators) {
|
||||
fail();
|
||||
return;
|
||||
}
|
||||
if(_start_collect_sample) {
|
||||
collect_sample();
|
||||
_gcs->set_snoop(NULL);
|
||||
_start_collect_sample = false;
|
||||
}
|
||||
switch(_status) {
|
||||
case ACCEL_CAL_NOT_STARTED:
|
||||
fail();
|
||||
return;
|
||||
case ACCEL_CAL_WAITING_FOR_ORIENTATION: {
|
||||
// if we're waiting for orientation, first ensure that all calibrators are on the same step
|
||||
uint8_t step;
|
||||
cal = get_calibrator(0);
|
||||
step = cal->get_num_samples_collected()+1;
|
||||
|
||||
for(uint8_t i=1 ; (cal = get_calibrator(i)) ; i++) {
|
||||
if (step != cal->get_num_samples_collected()+1) {
|
||||
fail();
|
||||
return;
|
||||
}
|
||||
}
|
||||
// if we're on a new step, print a message describing the step
|
||||
if (step != _step) {
|
||||
_step = step;
|
||||
|
||||
const char *msg;
|
||||
switch (step) {
|
||||
case 1:
|
||||
msg = "level";
|
||||
break;
|
||||
case 2:
|
||||
msg = "on its LEFT side";
|
||||
break;
|
||||
case 3:
|
||||
msg = "on its RIGHT side";
|
||||
break;
|
||||
case 4:
|
||||
msg = "nose DOWN";
|
||||
break;
|
||||
case 5:
|
||||
msg = "nose UP";
|
||||
break;
|
||||
case 6:
|
||||
msg = "on its BACK";
|
||||
break;
|
||||
default:
|
||||
fail();
|
||||
return;
|
||||
}
|
||||
_printf("Place vehicle %s and press any key.", msg);
|
||||
}
|
||||
// setup snooping of packets so we can see the COMMAND_ACK
|
||||
_gcs->set_snoop(_snoop);
|
||||
break;
|
||||
}
|
||||
case ACCEL_CAL_COLLECTING_SAMPLE:
|
||||
// check for timeout
|
||||
|
||||
for(uint8_t i=0; (cal = get_calibrator(i)); i++) {
|
||||
cal->check_for_timeout();
|
||||
}
|
||||
|
||||
update_status();
|
||||
|
||||
if (_status == ACCEL_CAL_FAILED) {
|
||||
fail();
|
||||
}
|
||||
return;
|
||||
case ACCEL_CAL_SUCCESS:
|
||||
// save
|
||||
if (_saving) {
|
||||
bool done = true;
|
||||
for(uint8_t i=0; i<_num_clients; i++) {
|
||||
if (client_active(i) && _clients[i]->_acal_get_saving()) {
|
||||
done = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (done) {
|
||||
success();
|
||||
}
|
||||
return;
|
||||
} else {
|
||||
for(uint8_t i=0; i<_num_clients; i++) {
|
||||
if(client_active(i) && _clients[i]->_acal_get_fail()) {
|
||||
fail();
|
||||
return;
|
||||
}
|
||||
}
|
||||
for(uint8_t i=0; i<_num_clients; i++) {
|
||||
if(client_active(i)) {
|
||||
_clients[i]->_acal_save_calibrations();
|
||||
}
|
||||
}
|
||||
_saving = true;
|
||||
}
|
||||
return;
|
||||
default:
|
||||
case ACCEL_CAL_FAILED:
|
||||
fail();
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AP_AccelCal::start(GCS_MAVLINK *gcs)
|
||||
{
|
||||
if (gcs == NULL || _started) {
|
||||
return;
|
||||
}
|
||||
_start_collect_sample = false;
|
||||
_num_active_calibrators = 0;
|
||||
|
||||
AccelCalibrator *cal;
|
||||
for(uint8_t i=0; (cal = get_calibrator(i)); i++) {
|
||||
cal->clear();
|
||||
cal->start(ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID, 6, 0.5f);
|
||||
_num_active_calibrators++;
|
||||
}
|
||||
|
||||
_started = true;
|
||||
_saving = false;
|
||||
_gcs = gcs;
|
||||
_step = 0;
|
||||
|
||||
update_status();
|
||||
}
|
||||
|
||||
void AP_AccelCal::success()
|
||||
{
|
||||
_printf("Calibration successful");
|
||||
|
||||
for(uint8_t i=0 ; i < _num_clients ; i++) {
|
||||
_clients[i]->_acal_event_success();
|
||||
}
|
||||
|
||||
clear();
|
||||
}
|
||||
|
||||
void AP_AccelCal::cancel()
|
||||
{
|
||||
_printf("Calibration cancelled");
|
||||
|
||||
for(uint8_t i=0 ; i < _num_clients ; i++) {
|
||||
_clients[i]->_acal_event_cancellation();
|
||||
}
|
||||
|
||||
clear();
|
||||
}
|
||||
|
||||
void AP_AccelCal::fail()
|
||||
{
|
||||
_printf("Calibration FAILED");
|
||||
|
||||
for(uint8_t i=0 ; i < _num_clients ; i++) {
|
||||
_clients[i]->_acal_event_failure();
|
||||
}
|
||||
|
||||
clear();
|
||||
}
|
||||
|
||||
void AP_AccelCal::clear()
|
||||
{
|
||||
if (!_started) {
|
||||
return;
|
||||
}
|
||||
|
||||
AccelCalibrator *cal;
|
||||
for(uint8_t i=0 ; (cal = get_calibrator(i)) ; i++) {
|
||||
cal->clear();
|
||||
}
|
||||
|
||||
_gcs = NULL;
|
||||
|
||||
_step = 0;
|
||||
_started = false;
|
||||
_saving = false;
|
||||
|
||||
update_status();
|
||||
}
|
||||
|
||||
void AP_AccelCal::collect_sample()
|
||||
{
|
||||
if (_status != ACCEL_CAL_WAITING_FOR_ORIENTATION) {
|
||||
return;
|
||||
}
|
||||
|
||||
for(uint8_t i=0; i<_num_clients; i++) {
|
||||
if (client_active(i) && !_clients[i]->_acal_get_ready_to_sample()) {
|
||||
_printf("Not ready to sample");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
AccelCalibrator *cal;
|
||||
for(uint8_t i=0 ; (cal = get_calibrator(i)) ; i++) {
|
||||
cal->collect_sample();
|
||||
}
|
||||
// setup snooping of packets so we can see the COMMAND_ACK
|
||||
_gcs->set_snoop(NULL);
|
||||
update_status();
|
||||
}
|
||||
|
||||
void AP_AccelCal::register_client(AP_AccelCal_Client* client) {
|
||||
if (client == NULL || _num_clients == AP_ACCELCAL_MAX_NUM_CLIENTS) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (_started) {
|
||||
fail();
|
||||
}
|
||||
|
||||
for(uint8_t i=0; i<_num_clients; i++) {
|
||||
if(_clients[i] == client) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
_clients[_num_clients] = client;
|
||||
_num_clients++;
|
||||
}
|
||||
|
||||
AccelCalibrator* AP_AccelCal::get_calibrator(uint8_t index) {
|
||||
AccelCalibrator* ret;
|
||||
for(uint8_t i=0; i<_num_clients; i++) {
|
||||
for(uint8_t j=0 ; (ret = _clients[i]->_acal_get_calibrator(j)) ; j++) {
|
||||
if (index == 0) {
|
||||
return ret;
|
||||
}
|
||||
index--;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
|
||||
void AP_AccelCal::update_status() {
|
||||
AccelCalibrator *cal;
|
||||
|
||||
if (!get_calibrator(0)) {
|
||||
// no calibrators
|
||||
_status = ACCEL_CAL_NOT_STARTED;
|
||||
return;
|
||||
}
|
||||
|
||||
for(uint8_t i=0 ; (cal = get_calibrator(i)) ; i++) {
|
||||
if (cal->get_status() == ACCEL_CAL_FAILED) {
|
||||
_status = ACCEL_CAL_FAILED; //fail if even one of the calibration has
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
for(uint8_t i=0 ; (cal = get_calibrator(i)) ; i++) {
|
||||
if (cal->get_status() == ACCEL_CAL_COLLECTING_SAMPLE) {
|
||||
_status = ACCEL_CAL_COLLECTING_SAMPLE; // move to Collecting sample state if all the callibrators have
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
for(uint8_t i=0 ; (cal = get_calibrator(i)) ; i++) {
|
||||
if (cal->get_status() == ACCEL_CAL_WAITING_FOR_ORIENTATION) {
|
||||
_status = ACCEL_CAL_WAITING_FOR_ORIENTATION; // move to waiting for user ack for orientation confirmation
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
for(uint8_t i=0 ; (cal = get_calibrator(i)) ; i++) {
|
||||
if (cal->get_status() == ACCEL_CAL_NOT_STARTED) {
|
||||
_status = ACCEL_CAL_NOT_STARTED; // we haven't started if all the calibrators haven't
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
_status = ACCEL_CAL_SUCCESS; // we have succeeded calibration if all the calibrators have
|
||||
return;
|
||||
}
|
||||
|
||||
bool AP_AccelCal::client_active(uint8_t client_num)
|
||||
{
|
||||
return (bool)_clients[client_num]->_acal_get_calibrator(0);
|
||||
}
|
||||
|
||||
static void _snoop(const mavlink_message_t* msg)
|
||||
{
|
||||
if (msg->msgid == MAVLINK_MSG_ID_COMMAND_ACK) {
|
||||
_start_collect_sample = true;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_AccelCal::_printf(const char* fmt, ...)
|
||||
{
|
||||
if (!_gcs) {
|
||||
return;
|
||||
}
|
||||
char msg[50];
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
hal.util->vsnprintf(msg, sizeof(msg), fmt, ap);
|
||||
va_end(ap);
|
||||
if (msg[strlen(msg)-1] == '\n') {
|
||||
// STATUSTEXT messages should not add linefeed
|
||||
msg[strlen(msg)-1] = 0;
|
||||
}
|
||||
AP_HAL::UARTDriver *uart = _gcs->get_uart();
|
||||
/*
|
||||
* to ensure these messages get to the user we need to wait for the
|
||||
* port send buffer to have enough room
|
||||
*/
|
||||
while (uart->txspace() < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) {
|
||||
hal.scheduler->delay(1);
|
||||
}
|
||||
|
||||
#if !APM_BUILD_TYPE(APM_BUILD_Replay)
|
||||
_gcs->send_text(MAV_SEVERITY_CRITICAL, msg);
|
||||
#endif
|
||||
}
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
#ifndef __AP_ACCELCAL_H__
|
||||
#define __AP_ACCELCAL_H__
|
||||
|
||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||
#include "AccelCalibrator.h"
|
||||
|
||||
#define AP_ACCELCAL_MAX_NUM_CLIENTS 4
|
||||
class GCS_MAVLINK;
|
||||
class AP_AccelCal_Client;
|
||||
|
||||
class AP_AccelCal {
|
||||
public:
|
||||
AP_AccelCal():
|
||||
_num_clients(0),
|
||||
_started(false),
|
||||
_saving(false)
|
||||
{ update_status(); }
|
||||
|
||||
// start all the registered calibrations
|
||||
void start(GCS_MAVLINK *gcs);
|
||||
|
||||
// called on calibration cancellation
|
||||
void cancel();
|
||||
|
||||
// Run an iteration of all registered calibrations
|
||||
void update();
|
||||
|
||||
// interface to the clients for registration
|
||||
void register_client(AP_AccelCal_Client* client);
|
||||
|
||||
// get the status of the calibrator server as a whole
|
||||
accel_cal_status_t get_status() { return _status; }
|
||||
|
||||
private:
|
||||
GCS_MAVLINK *_gcs;
|
||||
uint8_t _step;
|
||||
accel_cal_status_t _status;
|
||||
uint8_t _num_clients;
|
||||
AP_AccelCal_Client* _clients[AP_ACCELCAL_MAX_NUM_CLIENTS];
|
||||
|
||||
// called on calibration success
|
||||
void success();
|
||||
|
||||
// called on calibration failure
|
||||
void fail();
|
||||
|
||||
// reset all the calibrators to there pre calibration stage so as to make them ready for next calibration request
|
||||
void clear();
|
||||
|
||||
// proceed through the collection step for each of the registered calibrators
|
||||
void collect_sample();
|
||||
|
||||
// update the state of the Accel calibrator server
|
||||
void update_status();
|
||||
|
||||
// checks if no new sample has been recieved for considerable amount of time
|
||||
bool check_for_timeout();
|
||||
|
||||
// check if client's calibrator is active
|
||||
bool client_active(uint8_t client_num);
|
||||
|
||||
bool _started;
|
||||
bool _saving;
|
||||
|
||||
uint8_t _num_active_calibrators;
|
||||
|
||||
AccelCalibrator* get_calibrator(uint8_t i);
|
||||
void _printf(const char*, ...);
|
||||
};
|
||||
|
||||
class AP_AccelCal_Client {
|
||||
friend class AP_AccelCal;
|
||||
private:
|
||||
// getters
|
||||
virtual bool _acal_get_saving() { return false; }
|
||||
virtual bool _acal_get_ready_to_sample() { return true; }
|
||||
virtual bool _acal_get_fail() { return false; }
|
||||
virtual AccelCalibrator* _acal_get_calibrator(uint8_t instance) = 0;
|
||||
|
||||
// events
|
||||
virtual void _acal_save_calibrations() = 0;
|
||||
virtual void _acal_event_success() {};
|
||||
virtual void _acal_event_cancellation() {};
|
||||
virtual void _acal_event_failure() {};
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,463 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
|
||||
// Authored by Jonathan Challinger, 3D Robotics Inc.
|
||||
|
||||
#include "AccelCalibrator.h"
|
||||
#include <stdio.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
const extern AP_HAL::HAL& hal;
|
||||
/*
|
||||
* TODO
|
||||
* - time out when not receiving samples
|
||||
*/
|
||||
|
||||
////////////////////////////////////////////////////////////
|
||||
///////////////////// PUBLIC INTERFACE /////////////////////
|
||||
////////////////////////////////////////////////////////////
|
||||
|
||||
AccelCalibrator::AccelCalibrator() :
|
||||
_conf_tolerance(ACCEL_CAL_TOLERANCE),
|
||||
_sample_buffer(NULL),
|
||||
_param_struct(*reinterpret_cast<struct param_t *>(&_param_array))
|
||||
{
|
||||
clear();
|
||||
}
|
||||
/*
|
||||
Select options, initialise variables and initiate accel calibration
|
||||
Options:
|
||||
Fit Type: Will assume that if accelerometer static samples around all possible orientatio
|
||||
are spread in space will cover a surface of AXIS_ALIGNED_ELLIPSOID or any general
|
||||
ELLIPSOID as chosen
|
||||
|
||||
Num Samples: Number of samples user should will be gathering, please note that with type of surface
|
||||
chosen the minimum number of samples required will vary, for instance in the case of AXIS
|
||||
ALIGNED ELLIPSOID atleast 6 distinct samples are required while for generic ELLIPSOIDAL fit
|
||||
which will include calculation of offdiagonal parameters too requires atleast 8 parameters
|
||||
to form a distinct ELLIPSOID
|
||||
|
||||
Sample Time: Time over which the samples will be gathered and averaged to add to a single sample for least
|
||||
square regression
|
||||
|
||||
offset,diag,offdiag: initial parameter values for LSQ calculation
|
||||
*/
|
||||
void AccelCalibrator::start(enum accel_cal_fit_type_t fit_type, uint8_t num_samples, float sample_time) {
|
||||
start(fit_type, num_samples, sample_time, Vector3f(0,0,0), Vector3f(1,1,1), Vector3f(0,0,0));
|
||||
}
|
||||
|
||||
void AccelCalibrator::start(enum accel_cal_fit_type_t fit_type, uint8_t num_samples, float sample_time, Vector3f offset, Vector3f diag, Vector3f offdiag) {
|
||||
if (_status == ACCEL_CAL_FAILED || _status == ACCEL_CAL_SUCCESS) {
|
||||
clear();
|
||||
}
|
||||
if (_status != ACCEL_CAL_NOT_STARTED) {
|
||||
return;
|
||||
}
|
||||
|
||||
_conf_num_samples = num_samples;
|
||||
_conf_sample_time = sample_time;
|
||||
_conf_fit_type = fit_type;
|
||||
|
||||
_param_struct.offset = offset;
|
||||
_param_struct.diag = diag;
|
||||
_param_struct.offdiag = offdiag;
|
||||
|
||||
switch (_conf_fit_type) {
|
||||
case ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID:
|
||||
if (_conf_num_samples < 6) {
|
||||
set_status(ACCEL_CAL_FAILED);
|
||||
return;
|
||||
}
|
||||
break;
|
||||
case ACCEL_CAL_ELLIPSOID:
|
||||
if (_conf_num_samples < 8) {
|
||||
set_status(ACCEL_CAL_FAILED);
|
||||
return;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
set_status(ACCEL_CAL_WAITING_FOR_ORIENTATION);
|
||||
}
|
||||
|
||||
|
||||
// set Accel calibrator status to make itself ready for future accel cals
|
||||
void AccelCalibrator::clear() {
|
||||
set_status(ACCEL_CAL_NOT_STARTED);
|
||||
}
|
||||
|
||||
// returns true if accel calibrator is running
|
||||
bool AccelCalibrator::running() {
|
||||
return _status == ACCEL_CAL_WAITING_FOR_ORIENTATION || _status == ACCEL_CAL_COLLECTING_SAMPLE;
|
||||
}
|
||||
|
||||
// set Accel calibrator to start collecting samples in the next cycle
|
||||
void AccelCalibrator::collect_sample() {
|
||||
set_status(ACCEL_CAL_COLLECTING_SAMPLE);
|
||||
}
|
||||
|
||||
// collect and avg sample to be passed onto LSQ estimator after all requisite orientations are done
|
||||
void AccelCalibrator::new_sample(const Vector3f delta_velocity, float dt) {
|
||||
if (_status != ACCEL_CAL_COLLECTING_SAMPLE) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (_samples_collected >= _conf_num_samples) {
|
||||
set_status(ACCEL_CAL_FAILED);
|
||||
return;
|
||||
}
|
||||
|
||||
_sample_buffer[_samples_collected].delta_velocity += delta_velocity;
|
||||
_sample_buffer[_samples_collected].delta_time += dt;
|
||||
|
||||
_last_samp_frag_collected_ms = AP_HAL::millis();
|
||||
|
||||
if (_sample_buffer[_samples_collected].delta_time > _conf_sample_time) {
|
||||
Vector3f sample = _sample_buffer[_samples_collected].delta_velocity/_sample_buffer[_samples_collected].delta_time;
|
||||
if (!accept_sample(sample)) {
|
||||
set_status(ACCEL_CAL_FAILED);
|
||||
return;
|
||||
}
|
||||
|
||||
_samples_collected++;
|
||||
|
||||
if (_samples_collected >= _conf_num_samples) {
|
||||
run_fit(MAX_ITERATIONS, _fitness);
|
||||
|
||||
if (_fitness < _conf_tolerance) {
|
||||
set_status(ACCEL_CAL_SUCCESS);
|
||||
} else {
|
||||
set_status(ACCEL_CAL_FAILED);
|
||||
}
|
||||
} else {
|
||||
set_status(ACCEL_CAL_WAITING_FOR_ORIENTATION);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// interface for LSq estimator to read sample buffer sent after conversion from delta velocity
|
||||
// to averaged acc over time
|
||||
bool AccelCalibrator::get_sample(uint8_t i, Vector3f& s) const {
|
||||
if (i >= _samples_collected) {
|
||||
return false;
|
||||
}
|
||||
s = _sample_buffer[i].delta_velocity / _sample_buffer[i].delta_time;
|
||||
return true;
|
||||
}
|
||||
|
||||
// returns truen and sample corrected with diag offdiag parameters as calculated by LSq estimation procedure
|
||||
// returns false if no correct parameter exists to be applied along with existing sample without corrections
|
||||
bool AccelCalibrator::get_sample_corrected(uint8_t i, Vector3f& s) const {
|
||||
if (_status != ACCEL_CAL_SUCCESS || !get_sample(i,s)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
Matrix3f M (
|
||||
_param_struct.diag.x , _param_struct.offdiag.x , _param_struct.offdiag.y,
|
||||
_param_struct.offdiag.x , _param_struct.diag.y , _param_struct.offdiag.z,
|
||||
_param_struct.offdiag.y , _param_struct.offdiag.z , _param_struct.diag.z
|
||||
);
|
||||
|
||||
s = M*(s+_param_struct.offset);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// checks if no new sample has been recieved for considerable amount of time
|
||||
void AccelCalibrator::check_for_timeout() {
|
||||
static const uint32_t timeout = _conf_sample_time*2*1000 + 500;
|
||||
if (_status == ACCEL_CAL_COLLECTING_SAMPLE && AP_HAL::millis() - _last_samp_frag_collected_ms > timeout) {
|
||||
set_status(ACCEL_CAL_FAILED);
|
||||
}
|
||||
}
|
||||
|
||||
// returns spherical fit paramteters
|
||||
void AccelCalibrator::get_calibration(Vector3f& offset) const {
|
||||
offset = -_param_struct.offset;
|
||||
}
|
||||
|
||||
// returns axis aligned ellipsoidal fit parameters
|
||||
void AccelCalibrator::get_calibration(Vector3f& offset, Vector3f& diag) const {
|
||||
offset = -_param_struct.offset;
|
||||
diag = _param_struct.diag;
|
||||
}
|
||||
|
||||
// returns generic ellipsoidal fit parameters
|
||||
void AccelCalibrator::get_calibration(Vector3f& offset, Vector3f& diag, Vector3f& offdiag) const {
|
||||
offset = -_param_struct.offset;
|
||||
diag = _param_struct.diag;
|
||||
offdiag = _param_struct.offdiag;
|
||||
}
|
||||
|
||||
/////////////////////////////////////////////////////////////
|
||||
////////////////////// PRIVATE METHODS //////////////////////
|
||||
/////////////////////////////////////////////////////////////
|
||||
|
||||
/*
|
||||
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.
|
||||
*/
|
||||
bool AccelCalibrator::accept_sample(const Vector3f& sample)
|
||||
{
|
||||
static const uint16_t faces = 2*_conf_num_samples-4;
|
||||
static const float a = (4.0f * M_PI_F / (3.0f * faces)) + M_PI_F / 3.0f;
|
||||
static const float theta = 0.5f * acosf(cosf(a) / (1.0f - cosf(a)));
|
||||
|
||||
if (_sample_buffer == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
float min_distance = GRAVITY_MSS * 2*sinf(theta/2);
|
||||
|
||||
for(uint8_t i=0; i < _samples_collected; i++) {
|
||||
Vector3f other_sample;
|
||||
get_sample(i, other_sample);
|
||||
|
||||
if ((other_sample - sample).length() < min_distance){
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// sets status of calibrator and takes appropriate actions
|
||||
void AccelCalibrator::set_status(enum accel_cal_status_t status) {
|
||||
switch (status) {
|
||||
case ACCEL_CAL_NOT_STARTED:
|
||||
//Calibrator not started
|
||||
_status = ACCEL_CAL_NOT_STARTED;
|
||||
|
||||
_samples_collected = 0;
|
||||
if (_sample_buffer != NULL) {
|
||||
free(_sample_buffer);
|
||||
_sample_buffer = NULL;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case ACCEL_CAL_WAITING_FOR_ORIENTATION:
|
||||
//Callibrator has been started and is waiting for user to ack after orientation setting
|
||||
if (!running()) {
|
||||
_samples_collected = 0;
|
||||
if (_sample_buffer == NULL) {
|
||||
_sample_buffer = (struct AccelSample*)malloc(sizeof(struct AccelSample)*_conf_num_samples);
|
||||
if (_sample_buffer == NULL) {
|
||||
set_status(ACCEL_CAL_FAILED);
|
||||
break;
|
||||
}
|
||||
memset(_sample_buffer, 0, sizeof(struct AccelSample)*_conf_num_samples);
|
||||
}
|
||||
}
|
||||
if (_samples_collected >= _conf_num_samples) {
|
||||
break;
|
||||
}
|
||||
_status = ACCEL_CAL_WAITING_FOR_ORIENTATION;
|
||||
break;
|
||||
|
||||
case ACCEL_CAL_COLLECTING_SAMPLE:
|
||||
// Calibrator is waiting on collecting samples from acceleromter for amount of
|
||||
// time as requested by user/GCS
|
||||
if (_status != ACCEL_CAL_WAITING_FOR_ORIENTATION) {
|
||||
break;
|
||||
}
|
||||
_last_samp_frag_collected_ms = AP_HAL::millis();
|
||||
_status = ACCEL_CAL_COLLECTING_SAMPLE;
|
||||
break;
|
||||
|
||||
case ACCEL_CAL_SUCCESS:
|
||||
// Calibrator has successfully fitted the samples to user requested surface model
|
||||
// and has passed tolerance test
|
||||
if (_status != ACCEL_CAL_COLLECTING_SAMPLE) {
|
||||
break;
|
||||
}
|
||||
|
||||
_status = ACCEL_CAL_SUCCESS;
|
||||
break;
|
||||
|
||||
case ACCEL_CAL_FAILED:
|
||||
// Calibration has failed with reasons that can range from
|
||||
// bad sample data leading to faillure in tolerance test to lack of distinct samples
|
||||
if (_status == ACCEL_CAL_NOT_STARTED) {
|
||||
break;
|
||||
}
|
||||
|
||||
_status = ACCEL_CAL_FAILED;
|
||||
break;
|
||||
};
|
||||
}
|
||||
|
||||
/*
|
||||
Run Gauss Newton fitting algorithm over the sample space and come up with offsets, diagonal/scale factors
|
||||
and crosstalk/offdiagonal parameters
|
||||
*/
|
||||
void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness)
|
||||
{
|
||||
if (_sample_buffer == NULL) {
|
||||
return;
|
||||
}
|
||||
fitness = calc_mean_squared_residuals(_param_struct);
|
||||
float min_fitness = fitness;
|
||||
VectorP param_array = _param_array;
|
||||
struct param_t &fit_param(*reinterpret_cast<struct param_t *>(¶m_array));
|
||||
uint8_t num_iterations = 0;
|
||||
|
||||
while(num_iterations < max_iterations) {
|
||||
float last_fitness = fitness;
|
||||
|
||||
float JTJ[ACCEL_CAL_MAX_NUM_PARAMS*ACCEL_CAL_MAX_NUM_PARAMS];
|
||||
VectorP JTFI;
|
||||
memset(JTJ,0,sizeof(JTJ));
|
||||
|
||||
for(uint16_t k = 0; k<_samples_collected; k++) {
|
||||
Vector3f sample;
|
||||
get_sample(k, sample);
|
||||
|
||||
VectorN<float,ACCEL_CAL_MAX_NUM_PARAMS> jacob;
|
||||
|
||||
calc_jacob(sample, fit_param, jacob);
|
||||
|
||||
for(uint8_t i = 0; i < get_num_params(); i++) {
|
||||
// compute JTJ
|
||||
for(uint8_t j = 0; j < get_num_params(); j++) {
|
||||
JTJ[i*get_num_params()+j] += jacob[i] * jacob[j];
|
||||
}
|
||||
// compute JTFI
|
||||
JTFI[i] += jacob[i] * calc_residual(sample, fit_param);
|
||||
}
|
||||
}
|
||||
|
||||
if (!inverse(JTJ, JTJ, get_num_params())) {
|
||||
return;
|
||||
}
|
||||
|
||||
for(uint8_t row=0; row < get_num_params(); row++) {
|
||||
for(uint8_t col=0; col < get_num_params(); col++) {
|
||||
param_array[row] -= JTFI[col] * JTJ[row*get_num_params()+col];
|
||||
}
|
||||
}
|
||||
|
||||
fitness = calc_mean_squared_residuals(fit_param);
|
||||
|
||||
if (isnan(fitness) || isinf(fitness)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (fitness < min_fitness) {
|
||||
min_fitness = fitness;
|
||||
_param_struct = fit_param;
|
||||
}
|
||||
|
||||
num_iterations++;
|
||||
if (fitness - last_fitness < 1.0e-9f) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// calculates residual from samples(corrected using supplied parameter) to gravity
|
||||
// used to create Fitness column matrix
|
||||
float AccelCalibrator::calc_residual(const Vector3f& sample, const struct param_t& params) const {
|
||||
Matrix3f M (
|
||||
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
|
||||
);
|
||||
return GRAVITY_MSS - (M*(sample+params.offset)).length();
|
||||
}
|
||||
|
||||
// calculated the total mean squared fitness of all the collected samples using parameters
|
||||
// converged to LSq estimator so far
|
||||
float AccelCalibrator::calc_mean_squared_residuals() const
|
||||
{
|
||||
return calc_mean_squared_residuals(_param_struct);
|
||||
}
|
||||
|
||||
// calculated the total mean squared fitness of all the collected samples using parameters
|
||||
// supplied
|
||||
float AccelCalibrator::calc_mean_squared_residuals(const struct param_t& params) const
|
||||
{
|
||||
if (_sample_buffer == NULL || _samples_collected == 0) {
|
||||
return 1.0e30f;
|
||||
}
|
||||
float sum = 0.0f;
|
||||
for(uint16_t i=0; i < _samples_collected; i++){
|
||||
Vector3f sample;
|
||||
get_sample(i, sample);
|
||||
float resid = calc_residual(sample, params);
|
||||
sum += sq(resid);
|
||||
}
|
||||
sum /= _samples_collected;
|
||||
return sum;
|
||||
}
|
||||
|
||||
// calculate jacobian, a matrix that defines relation to variation in fitness with variation in each of the parameters
|
||||
// this is used in LSq estimator to adjust variation in parameter to be used for next iteration of LSq
|
||||
void AccelCalibrator::calc_jacob(const Vector3f& sample, const struct param_t& params, VectorP &ret) const {
|
||||
switch (_conf_fit_type) {
|
||||
case ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID:
|
||||
case ACCEL_CAL_ELLIPSOID: {
|
||||
const Vector3f &offset = params.offset;
|
||||
const Vector3f &diag = params.diag;
|
||||
const Vector3f &offdiag = params.offdiag;
|
||||
Matrix3f M(
|
||||
diag.x , offdiag.x , offdiag.y,
|
||||
offdiag.x , diag.y , offdiag.z,
|
||||
offdiag.y , offdiag.z , diag.z
|
||||
);
|
||||
|
||||
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 = (M*(sample+offset)).length();
|
||||
|
||||
// 0-2: offsets
|
||||
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);
|
||||
// 3-5: diagonals
|
||||
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;
|
||||
// 6-8: off-diagonals
|
||||
ret[6] = -1.0f * (((sample.y + offset.y) * A) + ((sample.x + offset.x) * B))/length;
|
||||
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;
|
||||
return;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
// returns number of paramters are required for selected Fit type
|
||||
uint8_t AccelCalibrator::get_num_params() const {
|
||||
switch (_conf_fit_type) {
|
||||
case ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID:
|
||||
return 6;
|
||||
case ACCEL_CAL_ELLIPSOID:
|
||||
return 9;
|
||||
default:
|
||||
return 6;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,133 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
|
||||
#ifndef __ACCELCALIBRATOR_H__
|
||||
#define __ACCELCALIBRATOR_H__
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_Math/vectorN.h>
|
||||
|
||||
#define ACCEL_CAL_MAX_NUM_PARAMS 9
|
||||
#define ACCEL_CAL_TOLERANCE 0.1
|
||||
#define MAX_ITERATIONS 50
|
||||
enum accel_cal_status_t {
|
||||
ACCEL_CAL_NOT_STARTED=0,
|
||||
ACCEL_CAL_WAITING_FOR_ORIENTATION=1,
|
||||
ACCEL_CAL_COLLECTING_SAMPLE=2,
|
||||
ACCEL_CAL_SUCCESS=3,
|
||||
ACCEL_CAL_FAILED=4
|
||||
};
|
||||
|
||||
enum accel_cal_fit_type_t {
|
||||
ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID=0,
|
||||
ACCEL_CAL_ELLIPSOID=1
|
||||
};
|
||||
|
||||
class AccelCalibrator {
|
||||
public:
|
||||
AccelCalibrator();
|
||||
|
||||
//Select options, initialise variables and initiate accel calibration
|
||||
void start(enum accel_cal_fit_type_t fit_type = ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID, uint8_t num_samples = 6, float sample_time = 0.5f);
|
||||
void start(enum accel_cal_fit_type_t fit_type, uint8_t num_samples, float sample_time, Vector3f offset, Vector3f diag, Vector3f offdiag);
|
||||
|
||||
// set Accel calibrator status to make itself ready for future accel cals
|
||||
void clear();
|
||||
|
||||
// returns true if accel calibrator is running
|
||||
bool running();
|
||||
|
||||
// set Accel calibrator to start collecting samples in the next cycle
|
||||
void collect_sample();
|
||||
|
||||
// check if client's calibrator is active
|
||||
void check_for_timeout();
|
||||
|
||||
// get diag,offset or offdiag parameters as per the selected fitting surface or request
|
||||
void get_calibration(Vector3f& offset) const;
|
||||
void get_calibration(Vector3f& offset, Vector3f& diag) const;
|
||||
void get_calibration(Vector3f& offset, Vector3f& diag, Vector3f& offdiag) const;
|
||||
|
||||
|
||||
// collect and avg sample to be passed onto LSQ estimator after all requisite orientations are done
|
||||
void new_sample(const Vector3f delta_velocity, float dt);
|
||||
|
||||
// interface for LSq estimator to read sample buffer sent after conversion from delta velocity
|
||||
// to averaged acc over time
|
||||
bool get_sample(uint8_t i, Vector3f& s) const;
|
||||
|
||||
// returns truen and sample corrected with diag offdiag parameters as calculated by LSq estimation procedure
|
||||
// returns false if no correct parameter exists to be applied along with existing sample without corrections
|
||||
bool get_sample_corrected(uint8_t i, Vector3f& s) const;
|
||||
|
||||
// set tolerance bar for parameter fitness value to cross so as to be deemed as correct values
|
||||
void set_tolerance(float tolerance) { _conf_tolerance = tolerance; }
|
||||
|
||||
// returns current state of accel calibrators
|
||||
enum accel_cal_status_t get_status() const { return _status; }
|
||||
|
||||
// returns number of samples collected
|
||||
uint8_t get_num_samples_collected() const { return _samples_collected; }
|
||||
|
||||
// returns mean squared fitness of sample points to the selected surface
|
||||
float get_fitness() const { return _fitness; }
|
||||
|
||||
struct param_t {
|
||||
Vector3f offset;
|
||||
Vector3f diag;
|
||||
Vector3f offdiag;
|
||||
};
|
||||
|
||||
private:
|
||||
struct AccelSample {
|
||||
Vector3f delta_velocity;
|
||||
float delta_time;
|
||||
};
|
||||
typedef VectorN<float, ACCEL_CAL_MAX_NUM_PARAMS> VectorP;
|
||||
|
||||
//configuration
|
||||
uint8_t _conf_num_samples;
|
||||
float _conf_sample_time;
|
||||
enum accel_cal_fit_type_t _conf_fit_type;
|
||||
float _conf_tolerance;
|
||||
|
||||
// state
|
||||
accel_cal_status_t _status;
|
||||
struct AccelSample* _sample_buffer;
|
||||
uint8_t _samples_collected;
|
||||
struct param_t &_param_struct;
|
||||
VectorP _param_array;
|
||||
float _fitness;
|
||||
uint32_t _last_samp_frag_collected_ms;
|
||||
|
||||
// private methods
|
||||
// check sanity of including the sample and add it to buffer if test is passed
|
||||
bool accept_sample(const Vector3f& sample);
|
||||
|
||||
// reset to calibrator state before the start of calibration
|
||||
void reset_state();
|
||||
|
||||
// sets status of calibrator and takes appropriate actions
|
||||
void set_status(enum accel_cal_status_t);
|
||||
|
||||
// returns number of paramters are required for selected Fit type
|
||||
uint8_t get_num_params() const;
|
||||
|
||||
// Function related to Gauss Newton Least square regression process
|
||||
float calc_residual(const Vector3f& sample, const struct param_t& params) const;
|
||||
float calc_mean_squared_residuals() const;
|
||||
float calc_mean_squared_residuals(const struct param_t& params) const;
|
||||
void calc_jacob(const Vector3f& sample, const struct param_t& params, VectorP& ret) const;
|
||||
void run_fit(uint8_t max_iterations, float& fitness);
|
||||
};
|
||||
#endif //__ACCELCALIBRATOR_H__
|
||||
Loading…
Reference in New Issue