From 7da465ade1693e9acf5599b0fc273973ca510ece Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 26 May 2023 13:00:05 +0200 Subject: [PATCH] DSP based gyro operations --- cmake/at32.cmake | 2 ++ cmake/stm32.cmake | 2 ++ src/main/sensors/gyro.c | 10 +++------- src/main/sensors/gyro.h | 1 + 4 files changed, 8 insertions(+), 7 deletions(-) diff --git a/cmake/at32.cmake b/cmake/at32.cmake index 2722798669..aa902593e9 100644 --- a/cmake/at32.cmake +++ b/cmake/at32.cmake @@ -15,6 +15,8 @@ set(CMSIS_DSP_DIR "${MAIN_LIB_DIR}/main/CMSIS/DSP") set(CMSIS_DSP_INCLUDE_DIR "${CMSIS_DSP_DIR}/Include") set(CMSIS_DSP_SRC + BasicMathFunctions/arm_scale_f32.c + BasicMathFunctions/arm_sub_f32.c BasicMathFunctions/arm_mult_f32.c TransformFunctions/arm_rfft_fast_f32.c TransformFunctions/arm_cfft_f32.c diff --git a/cmake/stm32.cmake b/cmake/stm32.cmake index f852e44e0e..f02185e9af 100644 --- a/cmake/stm32.cmake +++ b/cmake/stm32.cmake @@ -16,6 +16,8 @@ set(CMSIS_DSP_DIR "${MAIN_LIB_DIR}/main/CMSIS/DSP") set(CMSIS_DSP_INCLUDE_DIR "${CMSIS_DSP_DIR}/Include") set(CMSIS_DSP_SRC + BasicMathFunctions/arm_scale_f32.c + BasicMathFunctions/arm_sub_f32.c BasicMathFunctions/arm_mult_f32.c TransformFunctions/arm_rfft_fast_f32.c TransformFunctions/arm_cfft_f32.c diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 982af0c1c5..a586ce6df6 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -411,19 +411,15 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC if (zeroCalibrationIsCompleteV(gyroCal)) { float gyroADCtmp[XYZ_AXIS_COUNT]; - // Copy gyro value into int32_t (to prevent overflow) and then apply calibration and alignment - gyroADCtmp[X] = gyroDev->gyroADCRaw[X] - (int32_t)gyroDev->gyroZero[X]; - gyroADCtmp[Y] = gyroDev->gyroADCRaw[Y] - (int32_t)gyroDev->gyroZero[Y]; - gyroADCtmp[Z] = gyroDev->gyroADCRaw[Z] - (int32_t)gyroDev->gyroZero[Z]; + //Apply zero calibration with CMSIS DSP + arm_sub_f32(gyroDev->gyroADCRaw, gyroDev->gyroZero, gyroADCtmp, 3); // Apply sensor alignment applySensorAlignment(gyroADCtmp, gyroADCtmp, gyroDev->gyroAlign); applyBoardAlignment(gyroADCtmp); // Convert to deg/s and store in unified data - gyroADCf[X] = (float)gyroADCtmp[X] * gyroDev->scale; - gyroADCf[Y] = (float)gyroADCtmp[Y] * gyroDev->scale; - gyroADCf[Z] = (float)gyroADCtmp[Z] * gyroDev->scale; + arm_scale_f32(gyroADCtmp, gyroDev->scale, gyroADCf, 3); return true; } else { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 66666d80e0..493096a83a 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -25,6 +25,7 @@ #include "drivers/sensor.h" #include "flight/dynamic_gyro_notch.h" #include "flight/secondary_dynamic_gyro_notch.h" +#include "arm_math.h" typedef enum { GYRO_NONE = 0,