Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: keep transfers nicely setup for DMA on stm32

Browse Source 
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -635,9 +635,24 @@ void AP_InertialSensor_MPU6000::_read_fifo()
while (n_samples > 0) { while (n_samples > 0) {
uint8_t n = MIN(n_samples, MPU_FIFO_BUFFER_LEN); uint8_t n = MIN(n_samples, MPU_FIFO_BUFFER_LEN);
if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE)) { if (!_dev->set_chip_select(true)) {
printf("MPU60x0: error in fifo read %u bytes\n", n * MPU_SAMPLE_SIZE); if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE)) {
goto check_registers; 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 (_fast_sampling) {

22
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -588,11 +588,25 @@ bool AP_InertialSensor_MPU9250::_read_sample()
while (n_samples > 0) { while (n_samples > 0) {
uint8_t n = MIN(MPU_FIFO_BUFFER_LEN, n_samples); uint8_t n = MIN(MPU_FIFO_BUFFER_LEN, n_samples);
if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE)) { if (!_dev->set_chip_select(true)) {
printf("MPU60x0: error in fifo read %u bytes\n", n * MPU_SAMPLE_SIZE); if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU_SAMPLE_SIZE)) {
goto check_registers; 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 (_fast_sampling) {
if (!_accumulate_fast_sampling(rx, n)) { if (!_accumulate_fast_sampling(rx, n)) {
debug("stop at %u of %u", n_samples, bytes_read/MPU_SAMPLE_SIZE); debug("stop at %u of %u", n_samples, bytes_read/MPU_SAMPLE_SIZE);