Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 01:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: handle FIFO overflow for MPU6k and 9250

Browse Source 
this prevents temporary corruption of INS data on FIFO overflow
This commit is contained in:
Andrew Tridgell 2016-11-19 12:53:25 +11:00
parent c6b0a71c47
commit 216cf53922
2 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -583,7 +583,7 @@ void AP_InertialSensor_MPU6000::_check_temperature(void)
if (fabsf(_last_temp - temp) > 2 && !is_zero(_last_temp)) { if (fabsf(_last_temp - temp) > 2 && !is_zero(_last_temp)) {
// a 2 degree change in one sample is a highly likely // a 2 degree change in one sample is a highly likely
// sign of a FIFO alignment error // sign of a FIFO alignment error
printf("FIFO temperature reset: %.2f %.2f\n", printf("MPU6000: FIFO temperature reset: %.2f %.2f\n",
(double)temp, (double)_last_temp); (double)temp, (double)_last_temp);
_last_temp = temp; _last_temp = temp;
_fifo_reset(); _fifo_reset();
@ -624,6 +624,11 @@ void AP_InertialSensor_MPU6000::_read_fifo()
n_samples -= n; n_samples -= n;
} }
if (bytes_read > MPU6000_SAMPLE_SIZE * 35) {
printf("MPU60x0: fifo reset\n");
_fifo_reset();
}
if (_temp_counter++ == 255) { if (_temp_counter++ == 255) {
// check FIFO integrity every 0.25s // check FIFO integrity every 0.25s
_check_temperature(); _check_temperature();

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -513,7 +513,7 @@ void AP_InertialSensor_MPU9250::_check_temperature(void)
if (fabsf(_last_temp - temp) > 2 && !is_zero(_last_temp)) { if (fabsf(_last_temp - temp) > 2 && !is_zero(_last_temp)) {
// a 2 degree change in one sample is a highly likely // a 2 degree change in one sample is a highly likely
// sign of a FIFO alignment error // sign of a FIFO alignment error
printf("FIFO temperature reset: %.2f %.2f\n", printf("MPU9250: FIFO temperature reset: %.2f %.2f\n",
(double)temp, (double)_last_temp); (double)temp, (double)_last_temp);
_last_temp = temp; _last_temp = temp;
_fifo_reset(); _fifo_reset();
@ -557,6 +557,11 @@ bool AP_InertialSensor_MPU9250::_read_sample()
n_samples -= n; n_samples -= n;
} }
if (bytes_read > MPU9250_SAMPLE_SIZE * 35) {
printf("MPU9250: fifo reset\n");
_fifo_reset();
}
if (_temp_counter++ == 255) { if (_temp_counter++ == 255) {
// check FIFO integrity every 0.25s // check FIFO integrity every 0.25s
_check_temperature(); _check_temperature();