/*
check if the accelerometers are calibrated in 3D and that current number of accels matched number when calibrated
*/
bool AP_InertialSensor::accel_calibrated_ok_all() const
{
// check each accelerometer has offsets saved
for (uint8_t i=0; i<get_accel_count(); i++) {
// exactly 0.0 offset is extremely unlikely
if (_accel_offset[i].get().is_zero()) {
return false;
}
// exactly 1.0 scaling is extremely unlikely
const Vector3f &scaling = _accel_scale[i].get();
if (is_equal(scaling.x,1.0f) && is_equal(scaling.y,1.0f) && is_equal(scaling.z,1.0f)) {
return false;
}
// zero scaling also indicates not calibrated
if (_accel_scale[i].get().is_zero()) {
return false;
}
}
// check calibrated accels matches number of accels (no unused accels should have offsets or scaling)
if (get_accel_count() < INS_MAX_INSTANCES) {
for (uint8_t i=get_accel_count(); i<INS_MAX_INSTANCES; i++) {
const Vector3f &scaling = _accel_scale[i].get();
bool have_scaling = (!is_zero(scaling.x) && !is_equal(scaling.x,1.0f)) || (!is_zero(scaling.y) && !is_equal(scaling.y,1.0f)) || (!is_zero(scaling.z) && !is_equal(scaling.z,1.0f));
bool have_offsets = !_accel_offset[i].get().is_zero();
if (have_scaling || have_offsets) {
return false;
}
}
}
// if we got this far the accelerometers must have been calibrated
return true;
}
// 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);
}
// accelerometer clipping reporting
uint32_t AP_InertialSensor::get_accel_clip_count(uint8_t instance) const
{
if (instance >= get_accel_count()) {
return 0;
}
return _accel_clip_count[instance];
}
void
AP_InertialSensor::init(uint16_t sample_rate)
{
// remember the sample rate
_sample_rate = sample_rate;
_loop_delta_t = 1.0f / sample_rate;
if (_gyro_count == 0 && _accel_count == 0) {
_start_backends();
}
// initialise accel scale if need be. This is needed as we can't
// give non-zero default values for vectors in AP_Param
for (uint8_t i=0; i<get_accel_count(); i++) {
if (_accel_scale[i].get().is_zero()) {
_accel_scale[i].set(Vector3f(1,1,1));
}
}
// calibrate gyros unless gyro calibration has been disabled
if (gyro_calibration_timing() != GYRO_CAL_NEVER) {
_init_gyro();
}
_sample_period_usec = 1000*1000UL / _sample_rate;
// establish the baseline time between samples
_delta_time = 0;
_next_sample_usec = 0;
_last_sample_usec = 0;
_have_sample = false;
}
/// calibrated - returns true if the accelerometers have been calibrated
/// @note this should not be called while flying because it reads from the eeprom which can be slow
bool AP_InertialSensor::calibrated()
{
// check each accelerometer has offsets saved
for (uint8_t i=0; i<get_accel_count(); i++) {
if (!_accel_offset[i].load()) {
return false;
}
}
// if we got this far the accelerometers must have been calibrated
return true;
}
void
AP_InertialSensor::init( Start_style style,
Sample_rate sample_rate)
{
// remember the sample rate
_sample_rate = sample_rate;
if (_gyro_count == 0 && _accel_count == 0) {
// detect available backends. Only called once
_detect_backends();
}
_product_id = 0; // FIX
// initialise accel scale if need be. This is needed as we can't
// give non-zero default values for vectors in AP_Param
for (uint8_t i=0; i<get_accel_count(); i++) {
if (_accel_scale[i].get().is_zero()) {
_accel_scale[i].set(Vector3f(1,1,1));
}
}
// remember whether we have 3D calibration so this can be used for
// AHRS health
check_3D_calibration();
if (WARM_START != style) {
// do cold-start calibration for gyro only
_init_gyro();
}
switch (sample_rate) {
case RATE_50HZ:
_sample_period_usec = 20000;
break;
case RATE_100HZ:
_sample_period_usec = 10000;
break;
case RATE_200HZ:
_sample_period_usec = 5000;
break;
case RATE_400HZ:
default:
_sample_period_usec = 2500;
break;
}
// establish the baseline time between samples
_delta_time = 0;
_next_sample_usec = 0;
_last_sample_usec = 0;
_have_sample = false;
}
void
AP_InertialSensor::init( Start_style style,
Sample_rate sample_rate)
{
_product_id = _init_sensor(sample_rate);
// check scaling
for (uint8_t i=0; i<get_accel_count(); i++) {
if (_accel_scale[i].get().is_zero()) {
_accel_scale[i].set(Vector3f(1,1,1));
}
}
if (WARM_START != style) {
// do cold-start calibration for gyro only
_init_gyro();
}
}
/*
check if the accelerometers are calibrated in 3D. Called on startup
and any accel cal
*/
void AP_InertialSensor::check_3D_calibration()
{
_have_3D_calibration = false;
// check each accelerometer has offsets saved
for (uint8_t i=0; i<get_accel_count(); i++) {
// exactly 0.0 offset is extremely unlikely
if (_accel_offset[i].get().is_zero()) {
return;
}
// exactly 1.0 scaling is extremely unlikely
const Vector3f &scaling = _accel_scale[i].get();
if (fabsf(scaling.x - 1.0f) < 0.00001f &&
fabsf(scaling.y - 1.0f) < 0.00001f &&
fabsf(scaling.z - 1.0f) < 0.00001f) {
return;
}
}
// if we got this far the accelerometers must have been calibrated
_have_3D_calibration = true;
}
void
AP_InertialSensor::_init_accel()
{
uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES);
uint8_t flashcount = 0;
Vector3f prev[INS_MAX_INSTANCES];
Vector3f accel_offset[INS_MAX_INSTANCES];
float total_change[INS_MAX_INSTANCES];
float max_offset[INS_MAX_INSTANCES];
memset(max_offset, 0, sizeof(max_offset));
memset(total_change, 0, sizeof(total_change));
// cold start
hal.scheduler->delay(100);
hal.console->print_P(PSTR("Init Accel"));
// flash leds to tell user to keep the IMU still
AP_Notify::flags.initialising = true;
// clear accelerometer offsets and scaling
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k] = Vector3f(0,0,0);
_accel_scale[k] = Vector3f(1,1,1);
// initialise accel offsets to a large value the first time
// this will force us to calibrate accels at least twice
accel_offset[k] = Vector3f(500, 500, 500);
}
// loop until we calculate acceptable offsets
while (true) {
// get latest accelerometer values
update();
for (uint8_t k=0; k<num_accels; k++) {
// store old offsets
prev[k] = accel_offset[k];
// get new offsets
accel_offset[k] = get_accel(k);
}
// We take some readings...
for(int8_t i = 0; i < 50; i++) {
hal.scheduler->delay(20);
update();
// low pass filter the offsets
for (uint8_t k=0; k<num_accels; k++) {
accel_offset[k] = accel_offset[k] * 0.9f + get_accel(k) * 0.1f;
}
// display some output to the user
if(flashcount >= 10) {
hal.console->print_P(PSTR("*"));
flashcount = 0;
}
flashcount++;
}
for (uint8_t k=0; k<num_accels; k++) {
// null gravity from the Z accel
accel_offset[k].z += GRAVITY_MSS;
total_change[k] =
fabsf(prev[k].x - accel_offset[k].x) +
fabsf(prev[k].y - accel_offset[k].y) +
fabsf(prev[k].z - accel_offset[k].z);
max_offset[k] = (accel_offset[k].x > accel_offset[k].y) ? accel_offset[k].x : accel_offset[k].y;
max_offset[k] = (max_offset[k] > accel_offset[k].z) ? max_offset[k] : accel_offset[k].z;
}
uint8_t num_converged = 0;
for (uint8_t k=0; k<num_accels; k++) {
if (total_change[k] <= AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE &&
max_offset[k] <= AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET) {
num_converged++;
}
}
if (num_converged == num_accels) break;
hal.scheduler->delay(500);
}
// set the global accel offsets
for (uint8_t k=0; k<num_accels; k++) {
_accel_offset[k] = accel_offset[k];
}
// stop flashing the leds
AP_Notify::flags.initialising = false;
hal.console->print_P(PSTR(" "));
}
// 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;
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 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;
}
// clear out any existing samples from ins
update();
// 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) {
wait_for_sample();
// read samples from ins
update();
// capture sample
for (uint8_t k=0; k<num_accels; k++) {
samples[k][i] += get_accel(k);
}
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,
(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;
}
