Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: inform maximum gyro average difference 

While at it, define GYRO_INIT_MAX_DIFF_DPS.
This commit is contained in:
Gustavo Jose de Sousa 2016-06-17 18:00:29 -03:00 committed by Lucas De Marchi
parent e6f62080f5
commit e85ac8b2c5
1 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -53,6 +53,8 @@ extern const AP_HAL::HAL& hal;
#define SAMPLE_UNIT 1
#define GYRO_INIT_MAX_DIFF_DPS 0.1f
// Class level parameters
const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
// @Param: PRODUCT_ID
@ -862,7 +864,7 @@ AP_InertialSensor::_init_gyro()
if (best_diff[k] < 0) {
best_diff[k] = diff_norm[k];
best_avg[k] = gyro_avg[k];
} else if (gyro_diff[k].length() < ToRad(0.1f)) {
} else if (gyro_diff[k].length() < ToRad(GYRO_INIT_MAX_DIFF_DPS)) {
// 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()) {
@ -885,8 +887,10 @@ AP_InertialSensor::_init_gyro()
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]));
hal.console->printf("gyro[%u] did not converge: diff=%f dps (expected < %f)\n",
(unsigned)k,
(double)ToDeg(best_diff[k]),
(double)GYRO_INIT_MAX_DIFF_DPS);
_gyro_offset[k] = best_avg[k];
// flag calibration as failed for this gyro
_gyro_cal_ok[k] = false;