From d2f6a514b98b30918fac460e642a6be2f8566cfd Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 9 Nov 2016 16:16:52 +1100
Subject: [PATCH] AP_InertialSensor: catch FIFO alignment errors using
temperature reading
Two cases of what seems to be FIFO alignment errors have been seen on
a Pixracer-beta board with a ICM-20608. At a cost of 2 extra bytes per
transfer we can catch these by looking for sudden temperature changes
caused by bad data in the temperature registers.
---
.../AP_InertialSensor_MPU6000.cpp | 62 +++++++++++--------
.../AP_InertialSensor_MPU6000.h | 7 +--
2 files changed, 37 insertions(+), 32 deletions(-)
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
index 68c3c51677..e2613bfd82 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
@@ -225,7 +225,7 @@ extern const AP_HAL::HAL& hal;
#define MPU6000_REV_D8 0x58 // 0101 1000
#define MPU6000_REV_D9 0x59 // 0101 1001
-#define MPU6000_SAMPLE_SIZE 12
+#define MPU6000_SAMPLE_SIZE 14
#define MPU6000_MAX_FIFO_SAMPLES 20
#define MAX_DATA_READ (MPU6000_MAX_FIFO_SAMPLES * MPU6000_SAMPLE_SIZE)
@@ -250,7 +250,7 @@ AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation)
: AP_InertialSensor_Backend(imu)
- , _temp_filter(10, 1)
+ , _temp_filter(1000, 1)
, _dev(std::move(dev))
, _rotation(rotation)
{
@@ -330,7 +330,7 @@ void AP_InertialSensor_MPU6000::_fifo_reset()
void AP_InertialSensor_MPU6000::_fifo_enable()
{
_register_write(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
- BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN);
+ BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN);
_fifo_reset();
hal.scheduler->delay(1);
}
@@ -471,7 +471,6 @@ bool AP_InertialSensor_MPU6000::_data_ready()
bool AP_InertialSensor_MPU6000::_poll_data()
{
_read_fifo();
- _read_temperature();
return true;
}
@@ -491,9 +490,21 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
-int16_val(data, 2));
accel *= _accel_scale;
- gyro = Vector3f(int16_val(data, 4),
- int16_val(data, 3),
- -int16_val(data, 5));
+ float temp = int16_val(data, 3);
+ temp = temp/340 + 36.53;
+
+ if (fabsf(_last_temp - temp) > 10 && !is_zero(_last_temp)) {
+ // a 10 degree change in one sample is a highly likely
+ // sign of a FIFO alignment error
+ _last_temp = 0;
+ _fifo_reset();
+ return;
+ }
+ _last_temp = temp;
+
+ gyro = Vector3f(int16_val(data, 5),
+ int16_val(data, 4),
+ -int16_val(data, 6));
gyro *= GYRO_SCALE;
_rotate_and_correct_accel(_accel_instance, accel);
@@ -501,6 +512,8 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
_notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), fsync_set);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
+
+ _temp_filtered = _temp_filter.apply(temp);
}
}
@@ -513,9 +526,21 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
asum += Vector3l(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
- gsum += Vector3l(int16_val(data, 4),
- int16_val(data, 3),
- -int16_val(data, 5));
+ gsum += Vector3l(int16_val(data, 5),
+ int16_val(data, 4),
+ -int16_val(data, 6));
+
+ float temp = int16_val(data, 3);
+ temp = temp/340 + 36.53;
+
+ if (fabsf(_last_temp - temp) > 10 && !is_zero(_last_temp)) {
+ // a 10 degree change in one sample is a highly likely
+ // sign of a FIFO alignment error
+ _last_temp = 0;
+ _fifo_reset();
+ return;
+ }
+ _last_temp = temp;
}
float ascale = _accel_scale / n_samples;
@@ -572,23 +597,6 @@ void AP_InertialSensor_MPU6000::_read_fifo()
}
}
-void AP_InertialSensor_MPU6000::_read_temperature()
-{
- uint32_t now = AP_HAL::millis();
- if (now - _last_temp_read_ms < 100) {
- // read at 10Hz
- return;
- }
- uint8_t d[2];
-
- if (_block_read(MPUREG_TEMP_OUT_H, d, 2)) {
- float temp = int16_val(d, 0);
- temp = temp/340 + 36.53;
- _temp_filtered = _temp_filter.apply(temp);
- _last_temp_read_ms = now;
- }
-}
-
bool AP_InertialSensor_MPU6000::_block_read(uint8_t reg, uint8_t *buf,
uint32_t size)
{
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
index 7bca86b9e3..a7d8e033e4 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
@@ -73,9 +73,6 @@ private:
/* Read samples from FIFO (FIFO enabled) */
void _read_fifo();
- // read temperature data
- void _read_temperature();
-
/* Check if there's data available by either reading DRDY pin or register */
bool _data_ready();
@@ -113,8 +110,8 @@ private:
// are we doing more than 1kHz sampling?
bool _fast_sampling;
- // last time we read temperature
- uint32_t _last_temp_read_ms;
+ // last temperature reading, used to detect FIFO errors
+ float _last_temp;
// buffer for fifo read
uint8_t *_fifo_buffer;
--
GitLab