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:
parent
37a9a78725
commit
f9845c93b1
libraries/AP_InertialSensor
@ -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(®, 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) {
|
||||
|
@ -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(®, 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);
|
||||
|
Loading…
Reference in New Issue
Block a user