From 3db6203fb6884f04b7ce4431c20cd81bdfa92e39 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 17 Dec 2020 22:02:53 +0000 Subject: [PATCH] AP_InertialSensor: reduce FIFO buffer size to avoid DMA contention --- libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp | 2 +- libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index 5f000814f5..06fcc59d19 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -55,7 +55,7 @@ extern const AP_HAL::HAL& hal; #include "AP_InertialSensor_Invensense_registers.h" #define MPU_SAMPLE_SIZE 14 -#define MPU_FIFO_BUFFER_LEN 16 +#define MPU_FIFO_BUFFER_LEN 8 #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) #define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp index b1c9faeac0..2cc71e8038 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp @@ -51,7 +51,7 @@ static const float GYRO_SCALE = (0.0174532f / 16.4f); #include "AP_InertialSensor_Invensensev2_registers.h" #define INV2_SAMPLE_SIZE 14 -#define INV2_FIFO_BUFFER_LEN 16 +#define INV2_FIFO_BUFFER_LEN 8 #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) #define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])