uint8_t AP_InertialSensor_HIL::get_accel_count(void) const
{
if (get_accel_health(1)) {
return 2;
}
return 1;
}
uint8_t AP_InertialSensor_VRBRAIN::get_primary_accel(void) const
{
for (uint8_t i=0; i<_num_accel_instances; i++) {
if (get_accel_health(i)) return i;
}
return 0;
}
uint8_t AP_InertialSensor_PX4::_get_primary_accel(void) const
{
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (get_accel_health(i)) return i;
}
return 0;
}
// return true if accel instance should be used (must be healthy and have it's use parameter set to 1)
bool AP_InertialSensor::use_accel(uint8_t instance) const
{
if (instance >= INS_MAX_INSTANCES) {
return false;
}
return (get_accel_health(instance) && _use[instance]);
}
// get_accel_health_all - return true if all accels are healthy
bool AP_InertialSensor::get_accel_health_all(void) const
{
for (uint8_t i=0; i<get_accel_count(); i++) {
if (!get_accel_health(i)) {
return false;
}
}
// return true if we have at least one accel
return (get_accel_count() > 0);
}
/*
get delta velocity if available
*/
bool AP_InertialSensor::get_delta_velocity(uint8_t i, Vector3f &delta_velocity) const
{
if (_delta_velocity_valid[i]) {
delta_velocity = _delta_velocity[i];
return true;
} else if (get_accel_health(i)) {
delta_velocity = get_accel(i) * get_delta_time();
return true;
}
return false;
}
/*
calculate the trim_roll and trim_pitch. This is used for redoing the
trim without needing a full accel cal
*/
bool AP_InertialSensor::calibrate_trim(float &trim_roll, float &trim_pitch)
{
Vector3f level_sample;
// exit immediately if calibration is already in progress
if (_calibrating) {
return false;
}
_calibrating = true;
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
Vector3f samp;
samp = get_accel(0);
level_sample += samp;
if (!get_accel_health(0)) {
goto failed;
}
hal.scheduler->delay(update_dt_milliseconds);
num_samples++;
}
level_sample /= num_samples;
if (!_calculate_trim(level_sample, trim_roll, trim_pitch)) {
goto failed;
}
_calibrating = false;
return true;
failed:
_calibrating = false;
return false;
}
/**
try to detect bad accel/gyro sensors
*/
bool AP_InertialSensor_VRBRAIN::healthy(void) const
{
return get_gyro_health(0) && get_accel_health(0);
}
// 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,
(double)new_offsets[k].x, (double)new_offsets[k].y, (double)new_offsets[k].z);
interact->printf_P(PSTR("Scaling[%u]: %.2f %.2f %.2f\n"),
(unsigned)k,
(double)new_scaling[k].x, (double)new_scaling[k].y, (double)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]);
}
for (uint8_t k=num_accels; k<INS_MAX_INSTANCES; k++) {
// clear unused accelerometer's scaling and offsets
_accel_offset[k] = Vector3f(0,0,0);
_accel_scale[k] = Vector3f(0,0,0);
}
_save_parameters();
/*
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.rotate(saved_orientation);
_calculate_trim(level_sample, trim_roll, trim_pitch);
_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;
}
