// 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; } } }