Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 11:29:56 +03:00
Code Issues Wiki Activity

DSP based gyro operations

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2023-05-26 13:00:05 +02:00
parent 26145ec17c
commit 7da465ade1
4 changed files with 8 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

2
cmake/at32.cmake
View file

@ -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

2
cmake/stm32.cmake
View file

@ -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

10
src/main/sensors/gyro.c
View file

@ -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 {

1
src/main/sensors/gyro.h
View file

@ -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,