Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: continue finding best gyro after convergence

Browse Source 
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++) {
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++;
if (!converged[k] || last_average[k].length() < _gyro_offset[k].get().length()) {
_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);