void
AP_IMU_INS::init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
_init_gyro(delay_cb, flash_leds_cb);
//_sensor_cal.save();
save();
FLASH_LEDS(false);
}
void
AP_InertialSensor::_init_accel(void (*flash_leds_cb)(bool on))
{
int8_t flashcount = 0;
Vector3f ins_accel;
Vector3f prev;
Vector3f accel_offset;
float total_change;
float max_offset;
// cold start
hal.scheduler->delay(100);
hal.console->printf_P(PSTR("Init Accel"));
// clear accelerometer offsets and scaling
_accel_offset = Vector3f(0,0,0);
_accel_scale = 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 = Vector3f(500, 500, 500);
// loop until we calculate acceptable offsets
do {
// get latest accelerometer values
update();
ins_accel = get_accel();
// store old offsets
prev = accel_offset;
// get new offsets
accel_offset = ins_accel;
// We take some readings...
for(int8_t i = 0; i < 50; i++) {
hal.scheduler->delay(20);
update();
ins_accel = get_accel();
// low pass filter the offsets
accel_offset = accel_offset * 0.9 + ins_accel * 0.1;
// display some output to the user
if(flashcount == 5) {
hal.console->printf_P(PSTR("*"));
FLASH_LEDS(true);
}
if(flashcount >= 10) {
flashcount = 0;
FLASH_LEDS(false);
}
flashcount++;
}
// null gravity from the Z accel
// TO-DO: replace with gravity #define form location.cpp
accel_offset.z += GRAVITY;
total_change = fabsf(prev.x - accel_offset.x) + fabsf(prev.y - accel_offset.y) + fabsf(prev.z - accel_offset.z);
max_offset = (accel_offset.x > accel_offset.y) ? accel_offset.x : accel_offset.y;
max_offset = (max_offset > accel_offset.z) ? max_offset : accel_offset.z;
hal.scheduler->delay(500);
} while ( total_change > AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE || max_offset > AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET);
// set the global accel offsets
_accel_offset = accel_offset;
hal.console->printf_P(PSTR(" "));
}
void
AP_InertialSensor::_init_gyro(void (*flash_leds_cb)(bool on))
{
Vector3f last_average, best_avg;
Vector3f ins_gyro;
float best_diff = 0;
// cold start
hal.scheduler->delay(100);
hal.console->printf_P(PSTR("Init Gyro"));
// remove existing gyro offsets
_gyro_offset = Vector3f(0,0,0);
for(int8_t c = 0; c < 25; c++) {
// Mostly we are just flashing the LED's here
// to tell the user to keep the IMU still
FLASH_LEDS(true);
hal.scheduler->delay(20);
update();
ins_gyro = get_gyro();
FLASH_LEDS(false);
hal.scheduler->delay(20);
}
// the strategy is to average 200 points over 1 second, then do it
// again and see if the 2nd average is within a small margin of
// the first
last_average.zero();
// we try to get a good calibration estimate for up to 10 seconds
// if the gyros are stable, we should get it in 2 seconds
for (int16_t j = 0; j <= 10; j++) {
Vector3f gyro_sum, gyro_avg, gyro_diff;
float diff_norm;
uint8_t i;
hal.console->printf_P(PSTR("*"));
gyro_sum.zero();
for (i=0; i<200; i++) {
update();
ins_gyro = get_gyro();
gyro_sum += ins_gyro;
if (i % 40 == 20) {
FLASH_LEDS(true);
} else if (i % 40 == 0) {
FLASH_LEDS(false);
}
hal.scheduler->delay(5);
}
gyro_avg = gyro_sum / i;
gyro_diff = last_average - gyro_avg;
diff_norm = gyro_diff.length();
if (j == 0) {
best_diff = diff_norm;
best_avg = gyro_avg;
} else if (gyro_diff.length() < ToRad(0.04)) {
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
last_average = (gyro_avg * 0.5) + (last_average * 0.5);
_gyro_offset = last_average;
// all done
return;
} else if (diff_norm < best_diff) {
best_diff = diff_norm;
best_avg = (gyro_avg * 0.5) + (last_average * 0.5);
}
last_average = gyro_avg;
}
// we've kept the user waiting long enough - use the best pair we
// found so far
hal.console->printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff));
_gyro_offset = best_avg;
}
void
AP_IMU_INS::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
Vector3f last_average, best_avg;
float ins_gyro[3];
float best_diff = 0;
// cold start
delay_cb(100);
debug_P(PSTR("Init Gyro"));
for(int c = 0; c < 75; c++)
{
// Mostly we are just flashing the LED's here
// to tell the user to keep the IMU still
FLASH_LEDS(true);
delay_cb(20);
_ins->update();
//_ins->get_gyros(ins_gyro);
FLASH_LEDS(false);
delay_cb(20);
}
// the strategy is to average 200 points over 1 second, then do it
// again and see if the 2nd average is within a small margin of
// the first
last_average.zero();
// we try to get a good calibration estimate for up to 10 seconds
// if the gyros are stable, we should get it in 2 seconds
for (int j = 0; j <= 10; j++)
{
Vector3f gyro_sum, gyro_avg, gyro_diff;
float diff_norm;
uint8_t i;
debug_P(PSTR("*"));
gyro_sum.zero();
for (i = 0; i < 200; i++)
{
_ins->update();
//_ins->get_gyros(ins_gyro);
gyro_sum.add(_ins->gyro[0], _ins->gyro[1], _ins->gyro[2]);
if (i % 40 == 20)
{
FLASH_LEDS(true);
}
else if (i % 40 == 0)
{
FLASH_LEDS(false);
}
delay_cb(5);
}
gyro_avg = gyro_sum / i;
gyro_diff = last_average - gyro_avg;
diff_norm = gyro_diff.length();
if (j == 0)
{
best_diff = diff_norm;
best_avg = gyro_avg;
}
else if (gyro_diff.length() < ToRad(0.02))
{
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
last_average = (gyro_avg * 0.5) + (last_average * 0.5);
_sensor_cal[0] = last_average.x;
_sensor_cal[1] = last_average.y;
_sensor_cal[2] = last_average.z;
// all done
return;
}
else if (diff_norm < best_diff)
{
best_diff = diff_norm;
best_avg = (gyro_avg * 0.5) + (last_average * 0.5);
}
last_average = gyro_avg;
}
// we've kept the user waiting long enough - use the best pair we
// found so far
debug_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff));
_sensor_cal[0] = best_avg.x;
_sensor_cal[1] = best_avg.y;
_sensor_cal[2] = best_avg.z;
}
void
AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
int flashcount = 0;
float adc_in;
float prev[6] = {0,0,0,0,0,0};
float total_change;
float max_offset;
//float* ins_accel = ins->accel;
// cold start
delay_cb(500);
debug_P(PSTR("Init Accel"));
for (int j=3; j<=5; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
do
{
_ins->update();
//_ins->get_accels(ins_accel);
for (int j = 3; j <= 5; j++)
{
prev[j] = _sensor_cal[j];
adc_in = _ins->accel[j-3];
_sensor_cal[j] = adc_in;
}
for(int i = 0; i < 50; i++) // We take some readings...
{
delay_cb(20);
_ins->update();
//_ins->get_accels(ins_accel);
for (int j = 3; j < 6; j++)
{
adc_in = _ins->accel[j-3];
_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
}
if(flashcount == 5)
{
debug_P(PSTR("*"));
FLASH_LEDS(true);
}
if(flashcount >= 10)
{
flashcount = 0;
FLASH_LEDS(false);
}
flashcount++;
}
// null gravity from the Z accel
_sensor_cal[5] += 9.805;
//_sensor_cal[5] -= 9.805;
total_change = fabs(prev[3] - _sensor_cal[3]) + fabs(prev[4] - _sensor_cal[4]) +fabs(prev[5] - _sensor_cal[5]);
max_offset = (_sensor_cal[3] > _sensor_cal[4]) ? _sensor_cal[3] : _sensor_cal[4];
max_offset = (max_offset > _sensor_cal[5]) ? max_offset : _sensor_cal[5];
delay_cb(500);
}
while ( total_change > _accel_total_cal_change || max_offset > _accel_max_cal_offset);
debug_P(PSTR(" "));
}
void
AP_InertialSensor::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
Vector3f last_average, best_avg;
Vector3f ins_gyro;
float best_diff = 0;
// cold start
delay_cb(100);
if (_serial)
_serial->printf_P(PSTR("Init Gyro"));
// remove existing gyro offsets
_gyro_offset = Vector3f(0,0,0);
for(int16_t c = 0; c < 25; c++) {
// Mostly we are just flashing the LED's here
// to tell the user to keep the IMU still
FLASH_LEDS(true);
delay_cb(20);
read();
update();
ins_gyro = get_gyro();
FLASH_LEDS(false);
delay_cb(20);
}
// the strategy is to average 200 points over 1 second, then do it
// again and see if the 2nd average is within a small margin of
// the first
last_average.zero();
// we try to get a good calibration estimate for up to 10 seconds
// if the gyros are stable, we should get it in 2 seconds
int num_attempts = 10;
float target_average = 0.04;
#ifdef INS_VRIMUFULL
num_attempts = 5;
target_average = 0.1
#endif
for (int16_t j = 0; j <= num_attempts; j++) {
Vector3f gyro_sum, gyro_avg, gyro_diff;
float diff_norm;
uint8_t i;
if (_serial)
_serial->printf_P(PSTR("*"));
gyro_sum.zero();
for (i=0; i<200; i++) {
//for (i=0; i<10; i++) {
for(int z = 0; z < 10; z++){
read();
if (_serial)
_serial->printf_P(PSTR("."));
}
update();
ins_gyro = get_gyro();
// if (_serial) {
// _serial->println();
// _serial->print(ins_gyro.x, 5); _serial->print(" ");
// _serial->print(ins_gyro.y, 5); _serial->print(" ");
// _serial->print(ins_gyro.z, 5); _serial->print(" ");
//
// }
gyro_sum += ins_gyro;
if (i % 40 == 20) {
FLASH_LEDS(true);
} else if (i % 40 == 0) {
FLASH_LEDS(false);
}
delay_cb(5);
}
gyro_avg = gyro_sum / i;
gyro_diff = last_average - gyro_avg;
diff_norm = gyro_diff.length();
if (j == 0) {
best_diff = diff_norm;
best_avg = gyro_avg;
} else if (gyro_diff.length() < ToRad(target_average)) {
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
last_average = (gyro_avg * 0.5) + (last_average * 0.5);
_gyro_offset = last_average;
// all done
if (_serial) {
//_serial->printf_P(PSTR("\ngyro converged: diff=%f dps\n"), ToDeg(diff_norm));
_serial->printf_P(PSTR("\ngyro converged: diff="));
_serial->print(ToDeg(diff_norm), 5);
_serial->printf_P(PSTR(" dps\n"));
}
return;
} else if (diff_norm < best_diff) {
best_diff = diff_norm;
best_avg = (gyro_avg * 0.5) + (last_average * 0.5);
}
last_average = gyro_avg;
}
// we've kept the user waiting long enough - use the best pair we
// found so far
if (_serial)
_serial->printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff));
_gyro_offset = best_avg;
}
