diff --git a/cmake/at32.cmake b/cmake/at32.cmake
index 2722798669a..aa902593e94 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 f852e44e0e6..f02185e9aff 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/common/maths.c b/src/main/common/maths.c
index b7cc0167e02..ad976d9d50d 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -538,11 +538,43 @@ float fast_fsqrtf(const float value) {
 // function to calculate the normalization (pythagoras) of a 2-dimensional vector
 float NOINLINE calc_length_pythagorean_2D(const float firstElement, const float secondElement)
 {
-  return fast_fsqrtf(sq(firstElement) + sq(secondElement));
+    return fast_fsqrtf(sq(firstElement) + sq(secondElement));
 }
 
 // function to calculate the normalization (pythagoras) of a 3-dimensional vector
 float NOINLINE calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement)
 {
-  return fast_fsqrtf(sq(firstElement) + sq(secondElement) + sq(thirdElement));
-}
\ No newline at end of file
+    return fast_fsqrtf(sq(firstElement) + sq(secondElement) + sq(thirdElement));
+}
+
+#ifdef SITL_BUILD
+
+/**
+ * @brief Floating-point vector subtraction, equivalent of CMSIS arm_sub_f32.
+*/
+void arm_sub_f32(
+  float * pSrcA,
+  float * pSrcB,
+  float * pDst,
+  uint32_t blockSize)
+{
+    for (uint32_t i = 0; i < blockSize; i++) {
+        pDst[i] = pSrcA[i] - pSrcB[i];
+    }
+}
+
+/**
+ * @brief Floating-point vector scaling, equivalent of CMSIS arm_scale_f32.
+*/
+void arm_scale_f32(
+  float * pSrc,
+  float scale,
+  float * pDst,
+  uint32_t blockSize)
+{
+    for (uint32_t i = 0; i < blockSize; i++) {
+        pDst[i] = pSrc[i] * scale;
+    }
+}
+
+#endif
\ No newline at end of file
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index da417f33537..b701e480478 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -201,4 +201,9 @@ float calc_length_pythagorean_3D(const float firstElement, const float secondEle
  * The most significat byte is placed at the highest address
  * in other words, the most significant byte is "last", on odd indexes
  */
-#define int16_val_little_endian(v, idx) ((int16_t)(((uint8_t)v[2 * idx + 1] << 8) | v[2 * idx]))
\ No newline at end of file
+#define int16_val_little_endian(v, idx) ((int16_t)(((uint8_t)v[2 * idx + 1] << 8) | v[2 * idx]))
+
+#ifdef SITL_BUILD
+void arm_sub_f32(float * pSrcA, float * pSrcB, float * pDst, uint32_t blockSize);
+void arm_scale_f32(float * pSrc, float scale, float * pDst, uint32_t blockSize);
+#endif
\ No newline at end of file
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 982af0c1c56..a586ce6df6b 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 66666d80e0d..669f8fbee46 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -25,6 +25,11 @@
 #include "drivers/sensor.h"
 #include "flight/dynamic_gyro_notch.h"
 #include "flight/secondary_dynamic_gyro_notch.h"
+#if !defined(SITL_BUILD)
+#include "arm_math.h"
+#else
+#include <math.h>
+#endif
 
 typedef enum {
     GYRO_NONE = 0,
diff --git a/src/test/unit/target.h b/src/test/unit/target.h
index b95e1678c78..ac7617a63e2 100755
--- a/src/test/unit/target.h
+++ b/src/test/unit/target.h
@@ -17,6 +17,7 @@
 
 #pragma once
 
+#define SITL_BUILD
 #define USE_MAG
 #define USE_BARO
 #define USE_GPS