Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: optimize Invensense v3 FIF read

This commit is contained in:
Andy Piper 2024-09-23 11:37:11 +01:00 committed by Andrew Tridgell
parent d76132ec63
commit c0ce5e5ed0
1 changed files with 18 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -185,12 +185,12 @@ AP_InertialSensor_Invensensev3::~AP_InertialSensor_Invensensev3()
#if HAL_INS_HIGHRES_SAMPLE #if HAL_INS_HIGHRES_SAMPLE
if (highres_sampling) { if (highres_sampling) {
if (fifo_buffer != nullptr) { 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 } else
#endif #endif
if (fifo_buffer != nullptr) { 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 // allocate fifo buffer
#if HAL_INS_HIGHRES_SAMPLE #if HAL_INS_HIGHRES_SAMPLE
if (highres_sampling) { 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 } else
#endif #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) { if (fifo_buffer == nullptr) {
AP_HAL::panic("Invensensev3: Unable to allocate FIFO buffer"); 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_counth;
uint8_t reg_data; uint8_t reg_data;
uint8_t* samples = nullptr;
uint8_t* tfr_buffer = (uint8_t*)fifo_buffer;
switch (inv3_type) { switch (inv3_type) {
case Invensensev3_Type::ICM45686: case Invensensev3_Type::ICM45686:
@ -556,38 +558,27 @@ void AP_InertialSensor_Invensensev3::read_fifo()
while (n_samples > 0) { while (n_samples > 0) {
uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN); uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN);
if (!dev->set_chip_select(true)) { // we don't use read_registers() here to ensure that the fifo buffer that we have allocated
if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * fifo_sample_size)) { // gets passed all the way down to the SPI DMA handling. This involves one transfer to send
goto check_registers; // the register read and then another using the same buffer and length which is handled specially
} // for the read
} else { tfr_buffer[0] = reg_data | BIT_READ_FLAG;
// we don't use read_registers() here to ensure that the fifo buffer that we have allocated // transfer will also be sending data, make sure that data is zero
// gets passed all the way down to the SPI DMA handling. This involves one transfer to send memset(tfr_buffer + 1, 0, n * fifo_sample_size);
// the register read and then another using the same buffer and length which is handled specially if (!dev->transfer(tfr_buffer, n * fifo_sample_size + 1, tfr_buffer, n * fifo_sample_size + 1)) {
// for the read goto check_registers;
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);
} }
samples = tfr_buffer + 1;
#if HAL_INS_HIGHRES_SAMPLE #if HAL_INS_HIGHRES_SAMPLE
if (highres_sampling) { if (highres_sampling) {
if (!accumulate_highres_samples((FIFODataHighRes*)fifo_buffer, n)) { if (!accumulate_highres_samples((FIFODataHighRes*)samples, n)) {
need_reset = true; need_reset = true;
break; break;
} }
} else } else
#endif #endif
if (!accumulate_samples((FIFOData*)fifo_buffer, n)) { if (!accumulate_samples((FIFOData*)samples, n)) {
need_reset = true; need_reset = true;
break; break;
} }