Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AccelCal: fixes in response to review

This commit is contained in:
Jonathan Challinger 2015-12-29 22:51:11 -08:00
parent 660d9e86d5
commit 24e413c6af
2 changed files with 12 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_AccelCal/AccelCalibrator.cpp
View File

@ -69,6 +69,11 @@ void AccelCalibrator::start(enum accel_cal_fit_type_t fit_type, uint8_t num_samp
_conf_sample_time = sample_time; _conf_sample_time = sample_time;
_conf_fit_type = fit_type; _conf_fit_type = fit_type;
const uint16_t faces = 2*_conf_num_samples-4;
const float a = (4.0f * M_PI_F / (3.0f * faces)) + M_PI_F / 3.0f;
const float theta = 0.5f * acosf(cosf(a) / (1.0f - cosf(a)));
_min_sample_dist = GRAVITY_MSS * 2*sinf(theta/2);
_param_struct.offset = offset; _param_struct.offset = offset;
_param_struct.diag = diag; _param_struct.diag = diag;
_param_struct.offdiag = offdiag; _param_struct.offdiag = offdiag;
@ -108,7 +113,7 @@ void AccelCalibrator::collect_sample() {
} }
// collect and avg sample to be passed onto LSQ estimator after all requisite orientations are done // 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) { void AccelCalibrator::new_sample(const Vector3f& delta_velocity, float dt) {
if (_status != ACCEL_CAL_COLLECTING_SAMPLE) { if (_status != ACCEL_CAL_COLLECTING_SAMPLE) {
return; return;
} }
@ -190,7 +195,7 @@ bool AccelCalibrator::get_sample_corrected(uint8_t i, Vector3f& s) const {
// checks if no new sample has been recieved for considerable amount of time // checks if no new sample has been recieved for considerable amount of time
void AccelCalibrator::check_for_timeout() { void AccelCalibrator::check_for_timeout() {
static const uint32_t timeout = _conf_sample_time*2*1000 + 500; 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) { if (_status == ACCEL_CAL_COLLECTING_SAMPLE && AP_HAL::millis() - _last_samp_frag_collected_ms > timeout) {
set_status(ACCEL_CAL_FAILED); set_status(ACCEL_CAL_FAILED);
} }
@ -236,21 +241,15 @@ void AccelCalibrator::get_calibration(Vector3f& offset, Vector3f& diag, Vector3f
*/ */
bool AccelCalibrator::accept_sample(const Vector3f& sample) 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) { if (_sample_buffer == NULL) {
return false; return false;
} }
float min_distance = GRAVITY_MSS * 2*sinf(theta/2);
for(uint8_t i=0; i < _samples_collected; i++) { for(uint8_t i=0; i < _samples_collected; i++) {
Vector3f other_sample; Vector3f other_sample;
get_sample(i, other_sample); get_sample(i, other_sample);
if ((other_sample - sample).length() < min_distance){ if ((other_sample - sample).length() < _min_sample_dist){
return false; return false;
} }
} }
@ -277,12 +276,11 @@ void AccelCalibrator::set_status(enum accel_cal_status_t status) {
if (!running()) { if (!running()) {
_samples_collected = 0; _samples_collected = 0;
if (_sample_buffer == NULL) { if (_sample_buffer == NULL) {
_sample_buffer = (struct AccelSample*)malloc(sizeof(struct AccelSample)*_conf_num_samples); _sample_buffer = (struct AccelSample*)calloc(_conf_num_samples,sizeof(struct AccelSample));
if (_sample_buffer == NULL) { if (_sample_buffer == NULL) {
set_status(ACCEL_CAL_FAILED); set_status(ACCEL_CAL_FAILED);
break; break;
} }
memset(_sample_buffer, 0, sizeof(struct AccelSample)*_conf_num_samples);
} }
} }
if (_samples_collected >= _conf_num_samples) { if (_samples_collected >= _conf_num_samples) {
@ -341,9 +339,8 @@ void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness)
while(num_iterations < max_iterations) { while(num_iterations < max_iterations) {
float last_fitness = fitness; float last_fitness = fitness;
float JTJ[ACCEL_CAL_MAX_NUM_PARAMS*ACCEL_CAL_MAX_NUM_PARAMS]; float JTJ[ACCEL_CAL_MAX_NUM_PARAMS*ACCEL_CAL_MAX_NUM_PARAMS] {};
VectorP JTFI; VectorP JTFI;
memset(JTJ,0,sizeof(JTJ));
for(uint16_t k = 0; k<_samples_collected; k++) { for(uint16_t k = 0; k<_samples_collected; k++) {
Vector3f sample; Vector3f sample;

3
libraries/AP_AccelCal/AccelCalibrator.h
View File

@ -60,7 +60,7 @@ public:
// collect and avg sample to be passed onto LSQ estimator after all requisite orientations are done // 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); void new_sample(const Vector3f& delta_velocity, float dt);
// interface for LSq estimator to read sample buffer sent after conversion from delta velocity // interface for LSq estimator to read sample buffer sent after conversion from delta velocity
// to averaged acc over time // to averaged acc over time
@ -109,6 +109,7 @@ private:
VectorP _param_array; VectorP _param_array;
float _fitness; float _fitness;
uint32_t _last_samp_frag_collected_ms; uint32_t _last_samp_frag_collected_ms;
float _min_sample_dist;
// private methods // private methods
// check sanity of including the sample and add it to buffer if test is passed // check sanity of including the sample and add it to buffer if test is passed