AP_InertialSensor: optimize Invensense v3 FIF read

andyp1per · Sep 23, 2024 · f484a23 · f484a23
1 parent a56660e
commit f484a23
Showing 1 changed file with 18 additions and 27 deletions.
diff --git 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");
@@ -532,6 +532,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:
@@ -571,38 +573,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(&reg, 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;
         }