From c4974e1b42363ecf6c5e1f47de2ffba81d63e2a6 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 23 Sep 2024 11:37:11 +0100 Subject: [PATCH] AP_InertialSensor: optimize Invensense v3 FIF read --- .../AP_InertialSensor_Invensensev3.cpp | 45 ++++++++----------- 1 file changed, 18 insertions(+), 27 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index fc0bc9ecad79b..daf54ed64e7ec 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -185,12 +185,12 @@ AP_InertialSensor_Invensensev3::~AP_InertialSensor_Invensensev3() #if HAL_INS_HIGHRES_SAMPLE if (highres_sampling) { if (fifo_buffer != nullptr) { - hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE + 1, AP_HAL::Util::MEM_DMA_SAFE); } } else #endif if (fifo_buffer != nullptr) { - hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE + 1, AP_HAL::Util::MEM_DMA_SAFE); } } @@ -358,10 +358,10 @@ void AP_InertialSensor_Invensensev3::start() // allocate fifo buffer #if HAL_INS_HIGHRES_SAMPLE if (highres_sampling) { - fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE + 1, AP_HAL::Util::MEM_DMA_SAFE); } else #endif - fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE + 1, AP_HAL::Util::MEM_DMA_SAFE); if (fifo_buffer == nullptr) { AP_HAL::panic("Invensensev3: Unable to allocate FIFO buffer"); @@ -519,6 +519,8 @@ void AP_InertialSensor_Invensensev3::read_fifo() uint8_t reg_counth; uint8_t reg_data; + uint8_t* samples = nullptr; + uint8_t* tfr_buffer = (uint8_t*)fifo_buffer; switch (inv3_type) { case Invensensev3_Type::ICM45686: @@ -556,38 +558,27 @@ void AP_InertialSensor_Invensensev3::read_fifo() while (n_samples > 0) { uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN); - if (!dev->set_chip_select(true)) { - if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * fifo_sample_size)) { - goto check_registers; - } - } else { - // we don't use read_registers() here to ensure that the fifo buffer that we have allocated - // gets passed all the way down to the SPI DMA handling. This involves one transfer to send - // the register read and then another using the same buffer and length which is handled specially - // for the read - uint8_t reg = reg_data | BIT_READ_FLAG; - if (!dev->transfer(®, 1, nullptr, 0)) { - dev->set_chip_select(false); - goto check_registers; - } - // transfer will also be sending data, make sure that data is zero - memset((uint8_t*)fifo_buffer, 0, n * fifo_sample_size); - if (!dev->transfer((uint8_t*)fifo_buffer, n * fifo_sample_size, (uint8_t*)fifo_buffer, n * fifo_sample_size)) { - dev->set_chip_select(false); - goto check_registers; - } - dev->set_chip_select(false); + // we don't use read_registers() here to ensure that the fifo buffer that we have allocated + // gets passed all the way down to the SPI DMA handling. This involves one transfer to send + // the register read and then another using the same buffer and length which is handled specially + // for the read + tfr_buffer[0] = reg_data | BIT_READ_FLAG; + // transfer will also be sending data, make sure that data is zero + memset(tfr_buffer + 1, 0, n * fifo_sample_size); + if (!dev->transfer(tfr_buffer, n * fifo_sample_size + 1, tfr_buffer, n * fifo_sample_size + 1)) { + goto check_registers; } + samples = tfr_buffer + 1; #if HAL_INS_HIGHRES_SAMPLE if (highres_sampling) { - if (!accumulate_highres_samples((FIFODataHighRes*)fifo_buffer, n)) { + if (!accumulate_highres_samples((FIFODataHighRes*)samples, n)) { need_reset = true; break; } } else #endif - if (!accumulate_samples((FIFOData*)fifo_buffer, n)) { + if (!accumulate_samples((FIFOData*)samples, n)) { need_reset = true; break; }