5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00

AP_InertialSensor: keep transfers nicely setup for DMA on stm32

this avoids using stack based bounce buffers which may not support DMA
on stm32
This commit is contained in:
Andrew Tridgell 2016-11-27 10:39:55 +11:00
parent 37a9a78725
commit f9845c93b1
2 changed files with 36 additions and 7 deletions

View File

@ -635,9 +635,24 @@ void AP_InertialSensor_MPU6000::_read_fifo()
while (n_samples > 0) {
uint8_t n = MIN(n_samples, MPU_FIFO_BUFFER_LEN);
if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE)) {
printf("MPU60x0: error in fifo read %u bytes\n", n * MPU_SAMPLE_SIZE);
goto check_registers;
if (!_dev->set_chip_select(true)) {
if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE)) {
goto check_registers;
}
} else {
// this ensures we keep things nicely setup for DMA
uint8_t reg = MPUREG_FIFO_R_W | 0x80;
if (!_dev->transfer(&reg, 1, nullptr, 0)) {
_dev->set_chip_select(false);
goto check_registers;
}
memset(rx, 0, n * MPU_SAMPLE_SIZE);
if (!_dev->transfer(rx, n * MPU_SAMPLE_SIZE, rx, n * MPU_SAMPLE_SIZE)) {
hal.console->printf("MPU60x0: error in fifo read %u bytes\n", n * MPU_SAMPLE_SIZE);
_dev->set_chip_select(false);
goto check_registers;
}
_dev->set_chip_select(false);
}
if (_fast_sampling) {

View File

@ -588,11 +588,25 @@ bool AP_InertialSensor_MPU9250::_read_sample()
while (n_samples > 0) {
uint8_t n = MIN(MPU_FIFO_BUFFER_LEN, n_samples);
if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE)) {
printf("MPU60x0: error in fifo read %u bytes\n", n * MPU_SAMPLE_SIZE);
goto check_registers;
if (!_dev->set_chip_select(true)) {
if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE)) {
goto check_registers;
}
} else {
// this ensures we keep things nicely setup for DMA
uint8_t reg = MPUREG_FIFO_R_W | 0x80;
if (!_dev->transfer(&reg, 1, nullptr, 0)) {
_dev->set_chip_select(false);
goto check_registers;
}
memset(rx, 0, n * MPU_SAMPLE_SIZE);
if (!_dev->transfer(rx, n * MPU_SAMPLE_SIZE, rx, n * MPU_SAMPLE_SIZE)) {
hal.console->printf("MPU60x0: error in fifo read %u bytes\n", n * MPU_SAMPLE_SIZE);
_dev->set_chip_select(false);
goto check_registers;
}
_dev->set_chip_select(false);
}
if (_fast_sampling) {
if (!_accumulate_fast_sampling(rx, n)) {
debug("stop at %u of %u", n_samples, bytes_read/MPU_SAMPLE_SIZE);