bool AP_InertialSensor::calibrate_dynamic_accel(AP_InertialSensor_UserInteract* interact,
float &trim_roll,
float &trim_pitch)
{
uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES);
Vector3f samples[INS_MAX_INSTANCES][6];
Vector3f new_offsets[INS_MAX_INSTANCES];
Vector3f new_scaling[INS_MAX_INSTANCES];
Vector3f orig_offset[INS_MAX_INSTANCES];
Vector3f orig_scale[INS_MAX_INSTANCES];
uint8_t num_ok = 0;
for (uint8_t k=0; k<num_accels; k++) {
// backup original offsets and scaling
orig_offset[k] = _accel_offset[k].get();
orig_scale[k] = _accel_scale[k].get();
// clear accelerometer offsets and scaling
_accel_offset[k] = Vector3f(0,0,0);
_accel_scale[k] = Vector3f(1,1,1);
}
// capture data from 6 positions
for (uint8_t i=0; i<6; i++) {
const prog_char_t *msg;
// display message to user
switch ( i ) {
case 0:
msg = PSTR("level");
break;
case 1:
msg = PSTR("on its LEFT side");
break;
case 2:
msg = PSTR("on its RIGHT side");
break;
case 3:
msg = PSTR("nose DOWN");
break;
case 4:
msg = PSTR("nose UP");
break;
default: // default added to avoid compiler warning
case 5:
msg = PSTR("on its BACK");
break;
}
interact->printf_P(
PSTR("Place vehicle %S and press any key.\n"), msg);
// wait for GPS input
if (!GPS_ok()) {
//if GPS is out of the status,it fails
goto failed;
}
// clear out any existing samples from gps
// average 32 samples
for (uint8_t k=0; k<num_accels; k++) {
samples[k][i] = Vector3f();
}
uint8_t num_samples = 0;
while (num_samples < 32) {
// read samples from gps
// capture sample
accel_update();
for (uint8_t k=0; k<num_accels; k++) {
samples[k][i] += get_gps_accel;
}
hal.scheduler->delay(10);
num_samples++;
}
for (uint8_t k=0; k<num_accels; k++) {
samples[k][i] /= num_samples;
}
}
// run the calibration routine
for (uint8_t k=0; k<num_accels; k++) {
bool success = _calibrate_accel(samples[k], new_offsets[k], new_scaling[k]);
interact->printf_P(PSTR("Offsets[%u]: %.2f %.2f %.2f\n"),
(unsigned)k,
new_offsets[k].x, new_offsets[k].y, new_offsets[k].z);
interact->printf_P(PSTR("Scaling[%u]: %.2f %.2f %.2f\n"),
(unsigned)k,
new_scaling[k].x, new_scaling[k].y, new_scaling[k].z);
if (success) num_ok++;
}
if (num_ok == num_accels) {
interact->printf_P(PSTR("Calibration successful\n"));
for (uint8_t k=0; k<num_accels; k++) {
// set and save calibration
_accel_offset[k].set(new_offsets[k]);
_accel_scale[k].set(new_scaling[k]);
}
_save_parameters();
check_3D_calibration();
// calculate the trims as well from primary accels and pass back to caller
_calculate_trim(samples[0][0], trim_roll, trim_pitch);
return true;
}
failed:
interact->printf_P(PSTR("Calibration FAILED\n"));
// restore original scaling and offsets
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k].set(orig_offset[k]);
_accel_scale[k].set(orig_scale[k]);
}
return false;
}
// calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed
// from Rolfe Schmidt blog post describing the method:
// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at
// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact,
float &trim_roll,
float &trim_pitch)
{
uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES);
Vector3f samples[INS_MAX_INSTANCES][6];
Vector3f new_offsets[INS_MAX_INSTANCES];
Vector3f new_scaling[INS_MAX_INSTANCES];
Vector3f orig_offset[INS_MAX_INSTANCES];
Vector3f orig_scale[INS_MAX_INSTANCES];
uint8_t num_ok = 0;
// exit immediately if calibration is already in progress
if (_calibrating) {
return false;
}
_calibrating = true;
/*
we do the accel calibration with no board rotation. This avoids
having to rotate readings during the calibration
*/
enum Rotation saved_orientation = _board_orientation;
_board_orientation = ROTATION_NONE;
for (uint8_t k=0; k<num_accels; k++) {
// backup original offsets and scaling
orig_offset[k] = _accel_offset[k].get();
orig_scale[k] = _accel_scale[k].get();
// clear accelerometer offsets and scaling
_accel_offset[k] = Vector3f(0,0,0);
_accel_scale[k] = Vector3f(1,1,1);
}
memset(samples, 0, sizeof(samples));
// capture data from 6 positions
for (uint8_t i=0; i<6; i++) {
const prog_char_t *msg;
// display message to user
switch ( i ) {
case 0:
msg = PSTR("level");
break;
case 1:
msg = PSTR("on its LEFT side");
break;
case 2:
msg = PSTR("on its RIGHT side");
break;
case 3:
msg = PSTR("nose DOWN");
break;
case 4:
msg = PSTR("nose UP");
break;
default: // default added to avoid compiler warning
case 5:
msg = PSTR("on its BACK");
break;
}
interact->printf_P(
PSTR("Place vehicle %S and press any key.\n"), msg);
// wait for user input
if (!interact->blocking_read()) {
//No need to use interact->printf_P for an error, blocking_read does this when it fails
goto failed;
}
const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/get_sample_rate()+0.5f);
// wait 100ms for ins filter to rise
for (uint8_t k=0; k<100/update_dt_milliseconds; k++) {
wait_for_sample();
update();
hal.scheduler->delay(update_dt_milliseconds);
}
uint32_t num_samples = 0;
while (num_samples < 400/update_dt_milliseconds) {
wait_for_sample();
// read samples from ins
update();
// capture sample
for (uint8_t k=0; k<num_accels; k++) {
Vector3f samp;
if(get_delta_velocity(k,samp) && _delta_velocity_dt[k] > 0) {
samp /= _delta_velocity_dt[k];
} else {
samp = get_accel(k);
}
samples[k][i] += samp;
if (!get_accel_health(k)) {
interact->printf_P(PSTR("accel[%u] not healthy"), (unsigned)k);
goto failed;
}
}
hal.scheduler->delay(update_dt_milliseconds);
num_samples++;
}
for (uint8_t k=0; k<num_accels; k++) {
samples[k][i] /= num_samples;
}
}
// run the calibration routine
for (uint8_t k=0; k<num_accels; k++) {
if (!_check_sample_range(samples[k], saved_orientation, interact)) {
interact->printf_P(PSTR("Insufficient accel range"));
continue;
}
bool success = _calibrate_accel(samples[k], new_offsets[k], new_scaling[k], saved_orientation);
interact->printf_P(PSTR("Offsets[%u]: %.2f %.2f %.2f\n"),
(unsigned)k,
new_offsets[k].x, new_offsets[k].y, new_offsets[k].z);
interact->printf_P(PSTR("Scaling[%u]: %.2f %.2f %.2f\n"),
(unsigned)k,
new_scaling[k].x, new_scaling[k].y, new_scaling[k].z);
if (success) num_ok++;
}
if (num_ok == num_accels) {
interact->printf_P(PSTR("Calibration successful\n"));
for (uint8_t k=0; k<num_accels; k++) {
// set and save calibration
_accel_offset[k].set(new_offsets[k]);
_accel_scale[k].set(new_scaling[k]);
}
_save_parameters();
check_3D_calibration();
/*
calculate the trims as well from primary accels
We use the original board rotation for this sample
*/
Vector3f level_sample = samples[0][0];
level_sample.x *= new_scaling[0].x;
level_sample.y *= new_scaling[0].y;
level_sample.z *= new_scaling[0].z;
level_sample -= new_offsets[0];
level_sample.rotate(saved_orientation);
if (!_calculate_trim(level_sample, trim_roll, trim_pitch)) {
goto failed;
}
_board_orientation = saved_orientation;
_calibrating = false;
return true;
}
failed:
interact->printf_P(PSTR("Calibration FAILED\n"));
// restore original scaling and offsets
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k].set(orig_offset[k]);
_accel_scale[k].set(orig_scale[k]);
}
_board_orientation = saved_orientation;
_calibrating = false;
return false;
}