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

AP_InertialSensor: fixed gyro calibration bug

Browse Source 
we must not update _gyro_offset[] until we have completed calibration
of that gyro, or we will end up using the new offsets when asking for
the raw gyro vector
This commit is contained in:
Andrew Tridgell 2015-04-03 06:57:30 -07:00
parent 2aa73c427b
commit 4147825b87
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -672,6 +672,7 @@ AP_InertialSensor::_init_gyro()
{ {
uint8_t num_gyros = min(get_gyro_count(), INS_MAX_INSTANCES); uint8_t num_gyros = min(get_gyro_count(), INS_MAX_INSTANCES);
Vector3f last_average[INS_MAX_INSTANCES], best_avg[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]; float best_diff[INS_MAX_INSTANCES];
bool converged[INS_MAX_INSTANCES]; bool converged[INS_MAX_INSTANCES];
@ -699,6 +700,7 @@ AP_InertialSensor::_init_gyro()
// remove existing gyro offsets // remove existing gyro offsets
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();
best_diff[k] = 0; best_diff[k] = 0;
last_average[k].zero(); last_average[k].zero();
converged[k] = false; converged[k] = false;
@ -761,8 +763,8 @@ AP_InertialSensor::_init_gyro()
} 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);
if (!converged[k] || last_average[k].length() < _gyro_offset[k].get().length()) { if (!converged[k] || last_average[k].length() < new_gyro_offset[k].length()) {
_gyro_offset[k] = last_average[k]; new_gyro_offset[k] = last_average[k];
} }
if (!converged[k]) { if (!converged[k]) {
converged[k] = true; converged[k] = true;
@ -788,6 +790,7 @@ AP_InertialSensor::_init_gyro()
_gyro_cal_ok[k] = false; _gyro_cal_ok[k] = false;
} else { } else {
_gyro_cal_ok[k] = true; _gyro_cal_ok[k] = true;
_gyro_offset[k] = new_gyro_offset[k];
} }
} }