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

AP_InertialSensor: use smaller FIFO buffers

Browse Source 
this allows for reading multiple chunks from the fifo per callback
This commit is contained in:
Andrew Tridgell 2016-11-12 16:43:29 +11:00
parent 5b411aef65
commit c794ad9ee8
2 changed files with 26 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -226,7 +226,7 @@ extern const AP_HAL::HAL& hal;
#define MPU6000_REV_D9 0x59 // 0101 1001 #define MPU6000_REV_D9 0x59 // 0101 1001
#define MPU6000_SAMPLE_SIZE 14 #define MPU6000_SAMPLE_SIZE 14
#define MPU6000_MAX_FIFO_SAMPLES 20 #define MPU6000_MAX_FIFO_SAMPLES 8
#define MAX_DATA_READ (MPU6000_MAX_FIFO_SAMPLES * MPU6000_SAMPLE_SIZE) #define MAX_DATA_READ (MPU6000_MAX_FIFO_SAMPLES * MPU6000_SAMPLE_SIZE)
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
@ -600,25 +600,19 @@ void AP_InertialSensor_MPU6000::_read_fifo()
goto check_registers; goto check_registers;
} }
if (n_samples > MPU6000_MAX_FIFO_SAMPLES) { while (n_samples > 0) {
printf("bytes_read = %u, n_samples %u > %u, dropping samples\n", uint8_t n = MIN(n_samples, MPU6000_MAX_FIFO_SAMPLES);
bytes_read, n_samples, MPU6000_MAX_FIFO_SAMPLES); if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU6000_SAMPLE_SIZE)) {
printf("MPU60x0: error in fifo read %u bytes\n", n * MPU6000_SAMPLE_SIZE);
goto check_registers;
}
/* Too many samples, do a FIFO RESET */ if (_fast_sampling) {
_fifo_reset(); _accumulate_fast_sampling(rx, n);
goto check_registers; } else {
} _accumulate(rx, n);
}
if (!_block_read(MPUREG_FIFO_R_W, rx, n_samples * MPU6000_SAMPLE_SIZE)) { n_samples -= n;
printf("MPU60x0: error in fifo read %u bytes\n",
n_samples * MPU6000_SAMPLE_SIZE);
goto check_registers;
}
if (_fast_sampling) {
_accumulate_fast_sampling(rx, n_samples);
} else {
_accumulate(rx, n_samples);
} }
if (_temp_counter++ == 255) { if (_temp_counter++ == 255) {

32
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -185,7 +185,7 @@ extern const AP_HAL::HAL &hal;
#define BITS_DLPF_FCHOICE_B 0x08 #define BITS_DLPF_FCHOICE_B 0x08
#define MPU9250_SAMPLE_SIZE 14 #define MPU9250_SAMPLE_SIZE 14
#define MPU9250_MAX_FIFO_SAMPLES 20 #define MPU9250_MAX_FIFO_SAMPLES 8
#define MAX_DATA_READ (MPU9250_MAX_FIFO_SAMPLES * MPU9250_SAMPLE_SIZE) #define MAX_DATA_READ (MPU9250_MAX_FIFO_SAMPLES * MPU9250_SAMPLE_SIZE)
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
@ -533,25 +533,19 @@ bool AP_InertialSensor_MPU9250::_read_sample()
goto check_registers; goto check_registers;
} }
if (n_samples > MPU9250_MAX_FIFO_SAMPLES) { while (n_samples > 0) {
printf("bytes_read = %u, n_samples %u > %u, dropping samples\n", uint8_t n = MIN(MPU9250_MAX_FIFO_SAMPLES, n_samples);
bytes_read, n_samples, MPU9250_MAX_FIFO_SAMPLES); if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU9250_SAMPLE_SIZE)) {
printf("MPU60x0: error in fifo read %u bytes\n", n * MPU9250_SAMPLE_SIZE);
goto check_registers;
}
/* Too many samples, do a FIFO RESET */ if (_fast_sampling) {
_fifo_reset(); _accumulate_fast_sampling(rx, n);
goto check_registers; } else {
} _accumulate_fast_sampling(rx, n);
}
if (!_block_read(MPUREG_FIFO_R_W, rx, n_samples * MPU9250_SAMPLE_SIZE)) { n_samples -= n;
printf("MPU60x0: error in fifo read %u bytes\n",
n_samples * MPU9250_SAMPLE_SIZE);
goto check_registers;
}
if (_fast_sampling) {
_accumulate_fast_sampling(rx, n_samples);
} else {
_accumulate_fast_sampling(rx, n_samples);
} }
if (_temp_counter++ == 255) { if (_temp_counter++ == 255) {