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

AP_InertialSensor: continue finding best gyro after convergence 

with multiple gyros if we are still calibrating one of the gyros then
keep looking for better values for the already converged gyros.
This commit is contained in:
Andrew Tridgell 2015-03-08 07:49:38 +11:00
parent 20a4c98bac
commit 38bde56523
1 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -769,16 +769,19 @@ AP_InertialSensor::_init_gyro()
} }
for (uint8_t k=0; k<num_gyros; k++) { for (uint8_t k=0; k<num_gyros; k++) {
if (converged[k]) continue;
if (j == 0) { if (j == 0) {
best_diff[k] = diff_norm[k]; best_diff[k] = diff_norm[k];
best_avg[k] = gyro_avg[k]; best_avg[k] = gyro_avg[k];
} else if (gyro_diff[k].length() < ToRad(0.1f)) { } 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 // 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); last_average[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);
_gyro_offset[k] = last_average[k]; if (!converged[k] || last_average[k].length() < _gyro_offset[k].get().length()) {
converged[k] = true; _gyro_offset[k] = last_average[k];
num_converged++; }
if (!converged[k]) {
converged[k] = true;
num_converged++;
}
} else if (diff_norm[k] < best_diff[k]) { } else if (diff_norm[k] < best_diff[k]) {
best_diff[k] = diff_norm[k]; best_diff[k] = diff_norm[k];
best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f); best_avg[k] = (gyro_avg[k] * 0.5f) + (last_average[k] * 0.5f);