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,