Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: reduced number of SPI transfers 

use cached copy of temperature in MPU6000 and MPU9250 to detect FIFO
error when possible
This commit is contained in:
Andrew Tridgell 2016-11-26 19:02:22 +11:00
parent fdc94ec28a
commit 771cedca3d
4 changed files with 61 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -483,7 +483,7 @@ bool AP_InertialSensor_MPU6000::_poll_data()
return true;
}
bool AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp)
bool AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
{
for (uint8_t i = 0; i < n_samples; i++) {
const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
@ -500,8 +500,8 @@ bool AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples,
accel *= _accel_scale;
int16_t t2 = int16_val(data, 3);
if (abs(t2 - raw_temp) > 400) {
debug("temp reset %d %d", raw_temp, t2);
if (!_check_raw_temp(t2)) {
debug("temp reset %d %d", _raw_temp, t2);
_fifo_reset();
return false;
}
@ -531,7 +531,7 @@ bool AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples,
gives very good aliasing rejection at frequencies well above what
can be handled with 1kHz sample rates.
*/
bool AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp)
bool AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
{
int32_t tsum = 0;
const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / _accel_scale;
@ -543,8 +543,8 @@ bool AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
// use temperatue to detect FIFO corruption
int16_t t2 = int16_val(data, 3);
if (abs(t2 - raw_temp) > 400) {
debug("temp reset %d %d %d", raw_temp, t2, raw_temp - t2);
if (!_check_raw_temp(t2)) {
debug("temp reset %d %d", _raw_temp, t2);
_fifo_reset();
ret = false;
break;
@ -607,8 +607,6 @@ void AP_InertialSensor_MPU6000::_read_fifo()
uint8_t n_samples;
uint16_t bytes_read;
uint8_t *rx = _fifo_buffer;
int16_t raw_temp;
uint8_t trx[2];
bool need_reset = false;
if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) {
@ -623,14 +621,6 @@ void AP_InertialSensor_MPU6000::_read_fifo()
goto check_registers;
}
/*
fetch temperature in order to detect FIFO sync errors
*/
if (!_block_read(MPUREG_TEMP_OUT_H, trx, 2)) {
return;
}
raw_temp = int16_val(trx, 0);
/*
testing has shown that if we have more than 32 samples in the
FIFO then some of those samples will be corrupt. It always is
@ -651,12 +641,12 @@ void AP_InertialSensor_MPU6000::_read_fifo()
}
if (_fast_sampling) {
if (!_accumulate_fast_sampling(rx, n, raw_temp)) {
if (!_accumulate_fast_sampling(rx, n)) {
debug("stop at %u of %u", n_samples, bytes_read/MPU_SAMPLE_SIZE);
break;
}
} else {
if (!_accumulate(rx, n, raw_temp)) {
if (!_accumulate(rx, n)) {
break;
}
}
@ -678,6 +668,22 @@ check_registers:
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
}
/*
fetch temperature in order to detect FIFO sync errors
*/
bool AP_InertialSensor_MPU6000::_check_raw_temp(int16_t t2)
{
if (abs(t2 - _raw_temp) < 400) {
// cached copy OK
return true;
}
uint8_t trx[2];
if (_block_read(MPUREG_TEMP_OUT_H, trx, 2)) {
_raw_temp = int16_val(trx, 0);
}
return (abs(t2 - _raw_temp) < 400);
}
bool AP_InertialSensor_MPU6000::_block_read(uint8_t reg, uint8_t *buf,
uint32_t size)
{

8
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -78,9 +78,13 @@ private:
uint8_t _register_read(uint8_t reg);
void _register_write(uint8_t reg, uint8_t val, bool checked=false);
bool _accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp);
bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp);
bool _accumulate(uint8_t *samples, uint8_t n_samples);
bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples);
bool _check_raw_temp(int16_t t2);
int16_t _raw_temp;
// instance numbers of accel and gyro data
uint8_t _gyro_instance;
uint8_t _accel_instance;

43
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -421,7 +421,7 @@ bool AP_InertialSensor_MPU9250::_data_ready(uint8_t int_status)
}
bool AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp)
bool AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples)
{
for (uint8_t i = 0; i < n_samples; i++) {
const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
@ -433,8 +433,8 @@ bool AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples,
accel *= MPU9250_ACCEL_SCALE_1G;
int16_t t2 = int16_val(data, 3);
if (abs(t2 - raw_temp) > 400) {
debug("temp reset %d %d %d", raw_temp, t2, raw_temp-t2);
if (!_check_raw_temp(t2)) {
debug("temp reset %d %d %d", _raw_temp, t2, _raw_temp-t2);
_fifo_reset();
return false;
}
@ -464,7 +464,7 @@ bool AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples,
gives very good aliasing rejection at frequencies well above what
can be handled with 1kHz sample rates.
*/
bool AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp)
bool AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
{
int32_t tsum = 0;
const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / MPU9250_ACCEL_SCALE_1G;
@ -476,8 +476,8 @@ bool AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
// use temperatue to detect FIFO corruption
int16_t t2 = int16_val(data, 3);
if (abs(t2 - raw_temp) > 400) {
debug("temp reset %d %d %d", raw_temp, t2, raw_temp - t2);
if (!_check_raw_temp(t2)) {
debug("temp reset %d %d %d", _raw_temp, t2, _raw_temp - t2);
_fifo_reset();
ret = false;
break;
@ -535,6 +535,23 @@ bool AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
}
/*
fetch temperature in order to detect FIFO sync errors
*/
bool AP_InertialSensor_MPU9250::_check_raw_temp(int16_t t2)
{
if (abs(t2 - _raw_temp) < 400) {
// cached copy OK
return true;
}
uint8_t trx[2];
if (_block_read(MPUREG_TEMP_OUT_H, trx, 2)) {
_raw_temp = int16_val(trx, 0);
}
return (abs(t2 - _raw_temp) < 400);
}
/*
* read from the data registers and update filtered data
*/
@ -543,8 +560,6 @@ bool AP_InertialSensor_MPU9250::_read_sample()
uint8_t n_samples;
uint16_t bytes_read;
uint8_t *rx = _fifo_buffer;
int16_t raw_temp;
uint8_t trx[2];
bool need_reset = false;
if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) {
@ -559,14 +574,6 @@ bool AP_InertialSensor_MPU9250::_read_sample()
goto check_registers;
}
/*
fetch temperature in order to detect FIFO sync errors
*/
if (!_block_read(MPUREG_TEMP_OUT_H, trx, 2)) {
return true;
}
raw_temp = int16_val(trx, 0);
/*
testing has shown that if we have more than 32 samples in the
FIFO then some of those samples will be corrupt. It always is
@ -587,12 +594,12 @@ bool AP_InertialSensor_MPU9250::_read_sample()
}
if (_fast_sampling) {
if (!_accumulate_fast_sampling(rx, n, raw_temp)) {
if (!_accumulate_fast_sampling(rx, n)) {
debug("stop at %u of %u", n_samples, bytes_read/MPU_SAMPLE_SIZE);
break;
}
} else {
if (!_accumulate(rx, n, raw_temp)) {
if (!_accumulate(rx, n)) {
break;
}
}

8
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -73,9 +73,11 @@ private:
uint8_t _register_read(uint8_t reg);
void _register_write(uint8_t reg, uint8_t val, bool checked=false);
bool _accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp);
bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp);
bool _accumulate(uint8_t *samples, uint8_t n_samples);
bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples);
bool _check_raw_temp(int16_t t2);
// instance numbers of accel and gyro data
uint8_t _gyro_instance;
uint8_t _accel_instance;
@ -97,6 +99,8 @@ private:
// buffer for fifo read
uint8_t *_fifo_buffer;
int16_t _raw_temp;
/*
accumulators for fast sampling
See description in _accumulate_fast_sampling()