aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: fix best_diff on gyro initialization

Browse Source 
Without this patch, if accel_diff.length() > 0.2f and j == 0, then
best_diff[k] would be zero forever since diff_norm[k] >= 0 for any j.
This commit is contained in:
Gustavo Jose de Sousa 2016-06-17 17:44:48 -03:00 committed by Lucas De Marchi
parent de94392759
commit e6f62080f5
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -803,7 +803,7 @@ AP_InertialSensor::_init_gyro()
for (uint8_t k=0; k<num_gyros; k++) { for (uint8_t k=0; k<num_gyros; k++) {
_gyro_offset[k].set(Vector3f()); _gyro_offset[k].set(Vector3f());
new_gyro_offset[k].zero(); new_gyro_offset[k].zero();
best_diff[k] = 0; best_diff[k] = -1.f;
last_average[k].zero(); last_average[k].zero();
converged[k] = false; converged[k] = false;
} }
@ -859,7 +859,7 @@ AP_InertialSensor::_init_gyro()
} }
for (uint8_t k=0; k<num_gyros; k++) { for (uint8_t k=0; k<num_gyros; k++) {
if (j == 0) { if (best_diff[k] < 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)) {