// gyro_calibration_ok_all - returns true if all gyros were calibrated successfully
bool AP_InertialSensor::gyro_calibrated_ok_all() const
{
for (uint8_t i=0; i<get_gyro_count(); i++) {
if (!gyro_calibrated_ok(i)) {
return false;
}
}
return (get_gyro_count() > 0);
}
// get_gyro_health_all - return true if all gyros are healthy
bool AP_InertialSensor::get_gyro_health_all(void) const
{
for (uint8_t i=0; i<get_gyro_count(); i++) {
if (!get_gyro_health(i)) {
return false;
}
}
// return true if we have at least one gyro
return (get_gyro_count() > 0);
}
void
AP_InertialSensor::_init_gyro()
{
uint8_t num_gyros = MIN(get_gyro_count(), INS_MAX_INSTANCES);
Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES];
Vector3f new_gyro_offset[INS_MAX_INSTANCES];
float best_diff[INS_MAX_INSTANCES];
bool converged[INS_MAX_INSTANCES];
// exit immediately if calibration is already in progress
if (_calibrating) {
return;
}
// record we are calibrating
_calibrating = true;
// flash leds to tell user to keep the IMU still
AP_Notify::flags.initialising = true;
// cold start
hal.console->print("Init Gyro");
/*
we do the gyro calibration with no board rotation. This avoids
having to rotate readings during the calibration
*/
enum Rotation saved_orientation = _board_orientation;
_board_orientation = ROTATION_NONE;
// remove existing gyro offsets
for (uint8_t k=0; k<num_gyros; k++) {
_gyro_offset[k].set(Vector3f());
new_gyro_offset[k].zero();
best_diff[k] = 0;
last_average[k].zero();
converged[k] = false;
}
for(int8_t c = 0; c < 5; c++) {
hal.scheduler->delay(5);
update();
}
// the strategy is to average 50 points over 0.5 seconds, then do it
// again and see if the 2nd average is within a small margin of
// the first
uint8_t num_converged = 0;
// we try to get a good calibration estimate for up to 30 seconds
// if the gyros are stable, we should get it in 1 second
for (int16_t j = 0; j <= 30*4 && num_converged < num_gyros; j++) {
Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES];
Vector3f accel_start;
float diff_norm[INS_MAX_INSTANCES];
uint8_t i;
memset(diff_norm, 0, sizeof(diff_norm));
hal.console->print("*");
for (uint8_t k=0; k<num_gyros; k++) {
gyro_sum[k].zero();
}
accel_start = get_accel(0);
for (i=0; i<50; i++) {
update();
for (uint8_t k=0; k<num_gyros; k++) {
gyro_sum[k] += get_gyro(k);
}
hal.scheduler->delay(5);
}
Vector3f accel_diff = get_accel(0) - accel_start;
if (accel_diff.length() > 0.2f) {
// the accelerometers changed during the gyro sum. Skip
// this sample. This copes with doing gyro cal on a
// steadily moving platform. The value 0.2 corresponds
// with around 5 degrees/second of rotation.
continue;
}
for (uint8_t k=0; k<num_gyros; k++) {
gyro_avg[k] = gyro_sum[k] / i;
gyro_diff[k] = last_average[k] - gyro_avg[k];
diff_norm[k] = gyro_diff[k].length();
warnx("gyro[%d]: avg=%5.5f last_avg=%5.5f diff=%5.5f diff_norm=%5.5f\n", k, gyro_avg[k].length(), last_average[k].length(), gyro_diff[k].length(), diff_norm[k]);
}
for (uint8_t k=0; k<num_gyros; k++) {
if (j == 0) {
best_diff[k] = diff_norm[k];
best_avg[k] = gyro_avg[k];
} else if (gyro_diff[k].length() < ToRad(0.1f)) {
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
if (!converged[k] || last_average[k].length() < new_gyro_offset[k].length()) {
new_gyro_offset[k] = last_average[k];
}
if (!converged[k]) {
converged[k] = true;
num_converged++;
}
} else if (diff_norm[k] < best_diff[k]) {
best_diff[k] = diff_norm[k];
best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
}
last_average[k] = gyro_avg[k];
}
}
// we've kept the user waiting long enough - use the best pair we
// found so far
hal.console->println();
for (uint8_t k=0; k<num_gyros; k++) {
if (!converged[k]) {
hal.console->printf("gyro[%u] did not converge: diff=%f dps\n",
(unsigned)k, (double)ToDeg(best_diff[k]));
_gyro_offset[k] = best_avg[k];
// flag calibration as failed for this gyro
_gyro_cal_ok[k] = false;
} else {
_gyro_cal_ok[k] = true;
_gyro_offset[k] = new_gyro_offset[k];
}
}
// restore orientation
_board_orientation = saved_orientation;
// record calibration complete
_calibrating = false;
// stop flashing leds
AP_Notify::flags.initialising = false;
}
void
AP_InertialSensor::_init_gyro()
{
uint8_t num_gyros = min(get_gyro_count(), INS_MAX_INSTANCES);
Vector3f last_average[INS_MAX_INSTANCES], best_avg[INS_MAX_INSTANCES];
float best_diff[INS_MAX_INSTANCES];
bool converged[INS_MAX_INSTANCES];
// cold start
hal.console->print_P(PSTR("Init Gyro"));
// flash leds to tell user to keep the IMU still
AP_Notify::flags.initialising = true;
// remove existing gyro offsets
for (uint8_t k=0; k<num_gyros; k++) {
_gyro_offset[k] = Vector3f(0,0,0);
best_diff[k] = 0;
last_average[k].zero();
converged[k] = false;
_gyro_cal_ok[k] = true; // default calibration ok flag to true
}
for(int8_t c = 0; c < 5; c++) {
hal.scheduler->delay(5);
update();
}
// the strategy is to average 50 points over 0.5 seconds, then do it
// again and see if the 2nd average is within a small margin of
// the first
uint8_t num_converged = 0;
// we try to get a good calibration estimate for up to 30 seconds
// if the gyros are stable, we should get it in 1 second
for (int16_t j = 0; j <= 30*4 && num_converged < num_gyros; j++) {
Vector3f gyro_sum[INS_MAX_INSTANCES], gyro_avg[INS_MAX_INSTANCES], gyro_diff[INS_MAX_INSTANCES];
float diff_norm[INS_MAX_INSTANCES];
uint8_t i;
memset(diff_norm, 0, sizeof(diff_norm));
hal.console->print_P(PSTR("*"));
for (uint8_t k=0; k<num_gyros; k++) {
gyro_sum[k].zero();
}
for (i=0; i<50; i++) {
update();
for (uint8_t k=0; k<num_gyros; k++) {
gyro_sum[k] += get_gyro(k);
}
hal.scheduler->delay(5);
}
for (uint8_t k=0; k<num_gyros; k++) {
gyro_avg[k] = gyro_sum[k] / i;
gyro_diff[k] = last_average[k] - gyro_avg[k];
diff_norm[k] = gyro_diff[k].length();
}
for (uint8_t k=0; k<num_gyros; k++) {
if (converged[k]) continue;
if (j == 0) {
best_diff[k] = diff_norm[k];
best_avg[k] = gyro_avg[k];
} else if (gyro_diff[k].length() < ToRad(0.1f)) {
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
_gyro_offset[k] = last_average[k];
converged[k] = true;
num_converged++;
} else if (diff_norm[k] < best_diff[k]) {
best_diff[k] = diff_norm[k];
best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
}
last_average[k] = gyro_avg[k];
}
}
// stop flashing leds
AP_Notify::flags.initialising = false;
if (num_converged == num_gyros) {
// all OK
return;
}
// we've kept the user waiting long enough - use the best pair we
// found so far
hal.console->println();
for (uint8_t k=0; k<num_gyros; k++) {
if (!converged[k]) {
hal.console->printf_P(PSTR("gyro[%u] did not converge: diff=%f dps\n"),
(unsigned)k, ToDeg(best_diff[k]));
_gyro_offset[k] = best_avg[k];
// flag calibration as failed for this gyro
_gyro_cal_ok[k] = false;
}
}
}
