diff --git a/.gitmodules b/.gitmodules
index c944f41c59..d2d8aa2f8d 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,6 +1,6 @@
 [submodule "vendor/CMSIS"]
 	path = vendor/CMSIS
-	url = https://github.com/ARM-software/CMSIS.git
+	url = https://github.com/ARM-software/CMSIS_5.git
 [submodule "vendor/FreeRTOS"]
 	path = vendor/FreeRTOS
 	url = https://github.com/FreeRTOS/FreeRTOS-Kernel.git
diff --git a/Makefile b/Makefile
index b617bda8ce..df08dcbc46 100644
--- a/Makefile
+++ b/Makefile
@@ -284,24 +284,25 @@ SIZE = $(CROSS_COMPILE)size
 OBJCOPY = $(CROSS_COMPILE)objcopy
 GDB = $(CROSS_COMPILE)gdb
 
-INCLUDES += -I$(FREERTOS)/include -I$(PORT) -I$(CRAZYFLIE_BASE)/src
-INCLUDES += -I$(CRAZYFLIE_BASE)/src/config -I$(CRAZYFLIE_BASE)/src/hal/interface
-INCLUDES += -I$(CRAZYFLIE_BASE)/src/modules/interface -I$(CRAZYFLIE_BASE)/src/modules/interface/lighthouse -I$(CRAZYFLIE_BASE)/src/modules/interface/kalman_core
-INCLUDES += -I$(CRAZYFLIE_BASE)/src/utils/interface -I$(CRAZYFLIE_BASE)/src/drivers/interface -I$(CRAZYFLIE_BASE)/src/platform
-INCLUDES += -I$(CRAZYFLIE_BASE)/vendor/CMSIS/CMSIS/Include -I$(CRAZYFLIE_BASE)/src/drivers/bosch/interface
+INCLUDES += -I$(CRAZYFLIE_BASE)/vendor/CMSIS/CMSIS/Core/Include -I$(CRAZYFLIE_BASE)/vendor/CMSIS/CMSIS/DSP/Include
+INCLUDES += -I$(CRAZYFLIE_BASE)/vendor/libdw1000/inc
+INCLUDES += -I$(FREERTOS)/include -I$(PORT)
 
-INCLUDES += -I$(LIB)/STM32F4xx_StdPeriph_Driver/inc
+INCLUDES += -I$(CRAZYFLIE_BASE)/src/config
+INCLUDES += -I$(CRAZYFLIE_BASE)/src/platform
+
+INCLUDES += -I$(CRAZYFLIE_BASE)/src/deck/interface -I$(CRAZYFLIE_BASE)/src/deck/drivers/interface
+INCLUDES += -I$(CRAZYFLIE_BASE)/src/drivers/interface -I$(CRAZYFLIE_BASE)/src/drivers/bosch/interface
+INCLUDES += -I$(CRAZYFLIE_BASE)/src/hal/interface
+INCLUDES += -I$(CRAZYFLIE_BASE)/src/modules/interface -I$(CRAZYFLIE_BASE)/src/modules/interface/kalman_core -I$(CRAZYFLIE_BASE)/src/modules/interface/lighthouse
+INCLUDES += -I$(CRAZYFLIE_BASE)/src/utils/interface -I$(CRAZYFLIE_BASE)/src/utils/interface/kve -I$(CRAZYFLIE_BASE)/src/utils/interface/lighthouse -I$(CRAZYFLIE_BASE)/src/utils/interface/tdoa
+
+INCLUDES += -I$(LIB)/FatFS
 INCLUDES += -I$(LIB)/CMSIS/STM32F4xx/Include
 INCLUDES += -I$(LIB)/STM32_USB_Device_Library/Core/inc
 INCLUDES += -I$(LIB)/STM32_USB_OTG_Driver/inc
-INCLUDES += -I$(CRAZYFLIE_BASE)/src/deck/interface -I$(CRAZYFLIE_BASE)/src/deck/drivers/interface
-INCLUDES += -I$(CRAZYFLIE_BASE)/src/utils/interface/clockCorrection
-INCLUDES += -I$(CRAZYFLIE_BASE)/src/utils/interface/tdoa
-INCLUDES += -I$(CRAZYFLIE_BASE)/src/utils/interface/lighthouse
-INCLUDES += -I$(CRAZYFLIE_BASE)/vendor/libdw1000/inc
-INCLUDES += -I$(LIB)/FatFS
-INCLUDES += -I$(LIB)/vl53l1
-INCLUDES += -I$(LIB)/vl53l1/core/inc
+INCLUDES += -I$(LIB)/STM32F4xx_StdPeriph_Driver/inc
+INCLUDES += -I$(LIB)/vl53l1 -I$(LIB)/vl53l1/core/inc
 
 CFLAGS += -g3
 ifeq ($(DEBUG), 1)
diff --git a/test/deck/drivers/src/test_lps_twr_tag.c b/test/deck/drivers/src/test_lps_twr_tag.c
index f5a98198ad..721e457bb0 100644
--- a/test/deck/drivers/src/test_lps_twr_tag.c
+++ b/test/deck/drivers/src/test_lps_twr_tag.c
@@ -17,8 +17,8 @@
 // The mocking FW can not handle the cf_math.h/arm_math.h file, it crashes while parsing it. We have to use manual mocks instead.
 // Temporarily fix to make tests pass, add test code for the estimator part of rxcallback()
 #include "cf_math.h"
-void arm_std_f32( float32_t * pSrc, uint32_t blockSize, float32_t * pResult) { *pResult = 0.0; }
-void arm_mean_f32( float32_t * pSrc, uint32_t blockSize, float32_t * pResult) { *pResult = 0.0; }
+void arm_std_f32(const float32_t * pSrc, uint32_t blockSize, float32_t * pResult) { *pResult = 0.0; }
+void arm_mean_f32(const float32_t * pSrc, uint32_t blockSize, float32_t * pResult) { *pResult = 0.0; }
 
 #include "mock_estimator.h"
 
diff --git a/tools/make/cmsis_dsp/Makefile b/tools/make/cmsis_dsp/Makefile
index 58c87c99aa..d0894f0be8 100644
--- a/tools/make/cmsis_dsp/Makefile
+++ b/tools/make/cmsis_dsp/Makefile
@@ -2,8 +2,9 @@ include obj.mk
 
 BIN=$(PROJ_ROOT)/bin/vendor
 PROJ_BIN=$(PROJ_ROOT)/bin
-DSP_SRC=$(CRAZYFLIE_BASE)/vendor/CMSIS/CMSIS/DSP_Lib/Source
-DSP_INC=$(CRAZYFLIE_BASE)/vendor/CMSIS/CMSIS/Include
+CMSIS_INC=$(CRAZYFLIE_BASE)/vendor/CMSIS/CMSIS/Core/Include
+DSP_INC=$(CRAZYFLIE_BASE)/vendor/CMSIS/CMSIS/DSP/Include
+DSP_SRC=$(CRAZYFLIE_BASE)/vendor/CMSIS/CMSIS/DSP/Source
 
 VPATH += $(BIN)
 VPATH += $(DSP_SRC)/BasicMathFunctions/
@@ -20,18 +21,13 @@ VPATH += $(DSP_SRC)/TransformFunctions/
 
 LIB=$(PROJ_BIN)/libarm_math.a
 
-CSFLAGS = -O2 -I$(DSP_INC) -std=c11 -DARM_MATH_CM4 -D__FPU_PRESENT=1 -D__TARGET_FPU_VFP  -mfloat-abi=hard -mfpu=fpv4-sp-d16
-CSFLAGS += -mcpu=cortex-m4 -mthumb -ffunction-sections -fdata-sections
-CFLAGS=$(CSFLAGS) -include stdint.h
+CFLAGS = -O2 -I$(CMSIS_INC) -I$(DSP_INC) -std=c11
+CFLAGS += -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16
+CFLAGS += -DARM_MATH_CM4 -D__FPU_PRESENT=1 -ffunction-sections -fdata-sections
 
 CROSS_COMPILE     ?= arm-none-eabi-
-AS = $(CROSS_COMPILE)as
 CC = $(CROSS_COMPILE)gcc
-LD = $(CROSS_COMPILE)gcc
 AR = $(CROSS_COMPILE)ar
-SIZE = $(CROSS_COMPILE)size
-OBJCOPY = $(CROSS_COMPILE)objcopy
-GDB = $(CROSS_COMPILE)gdb
 
 all: $(LIB)
 
diff --git a/tools/make/cmsis_dsp/obj.mk b/tools/make/cmsis_dsp/obj.mk
index bc30bd6f66..cd7a85719e 100644
--- a/tools/make/cmsis_dsp/obj.mk
+++ b/tools/make/cmsis_dsp/obj.mk
@@ -1,273 +1,272 @@
-DSP_OBJ += arm_iir_lattice_init_q31.o
-DSP_OBJ += arm_fir_interpolate_init_q31.o
-DSP_OBJ += arm_fir_q7.o
-DSP_OBJ += arm_fir_lattice_init_q15.o
-DSP_OBJ += arm_lms_norm_q15.o
+DSP_OBJ += arm_abs_f32.o
+DSP_OBJ += arm_abs_q15.o
+DSP_OBJ += arm_abs_q31.o
+DSP_OBJ += arm_abs_q7.o
+DSP_OBJ += arm_add_f32.o
+DSP_OBJ += arm_add_q15.o
+DSP_OBJ += arm_add_q31.o
+DSP_OBJ += arm_add_q7.o
 DSP_OBJ += arm_biquad_cascade_df1_32x64_init_q31.o
+DSP_OBJ += arm_biquad_cascade_df1_32x64_q31.o
+DSP_OBJ += arm_biquad_cascade_df1_f32.o
+DSP_OBJ += arm_biquad_cascade_df1_fast_q15.o
+DSP_OBJ += arm_biquad_cascade_df1_fast_q31.o
+DSP_OBJ += arm_biquad_cascade_df1_init_f32.o
 DSP_OBJ += arm_biquad_cascade_df1_init_q15.o
-DSP_OBJ += arm_biquad_cascade_df2T_init_f32.o
-DSP_OBJ += arm_correlate_q15.o
-DSP_OBJ += arm_conv_partial_q31.o
-DSP_OBJ += arm_biquad_cascade_stereo_df2T_f32.o
-DSP_OBJ += arm_fir_decimate_init_q31.o
-DSP_OBJ += arm_correlate_opt_q7.o
-DSP_OBJ += arm_fir_sparse_q15.o
-DSP_OBJ += arm_conv_partial_q15.o
-DSP_OBJ += arm_conv_q7.o
 DSP_OBJ += arm_biquad_cascade_df1_init_q31.o
+DSP_OBJ += arm_biquad_cascade_df1_q15.o
 DSP_OBJ += arm_biquad_cascade_df1_q31.o
-DSP_OBJ += arm_fir_lattice_f32.o
-DSP_OBJ += arm_fir_decimate_fast_q31.o
+DSP_OBJ += arm_biquad_cascade_df2T_f32.o
+DSP_OBJ += arm_biquad_cascade_df2T_f64.o
+DSP_OBJ += arm_biquad_cascade_df2T_init_f32.o
+DSP_OBJ += arm_biquad_cascade_df2T_init_f64.o
+DSP_OBJ += arm_biquad_cascade_stereo_df2T_f32.o
+DSP_OBJ += arm_biquad_cascade_stereo_df2T_init_f32.o
+DSP_OBJ += arm_bitreversal.o
+DSP_OBJ += arm_bitreversal2.o
+DSP_OBJ += arm_cfft_f32.o
+DSP_OBJ += arm_cfft_q15.o
+DSP_OBJ += arm_cfft_q31.o
+DSP_OBJ += arm_cfft_radix2_f32.o
+DSP_OBJ += arm_cfft_radix2_init_f32.o
+DSP_OBJ += arm_cfft_radix2_init_q15.o
+DSP_OBJ += arm_cfft_radix2_init_q31.o
+DSP_OBJ += arm_cfft_radix2_q15.o
+DSP_OBJ += arm_cfft_radix2_q31.o
+DSP_OBJ += arm_cfft_radix4_f32.o
+DSP_OBJ += arm_cfft_radix4_init_f32.o
+DSP_OBJ += arm_cfft_radix4_init_q15.o
+DSP_OBJ += arm_cfft_radix4_init_q31.o
+DSP_OBJ += arm_cfft_radix4_q15.o
+DSP_OBJ += arm_cfft_radix4_q31.o
+DSP_OBJ += arm_cfft_radix8_f32.o
+DSP_OBJ += arm_cmplx_conj_f32.o
+DSP_OBJ += arm_cmplx_conj_q15.o
+DSP_OBJ += arm_cmplx_conj_q31.o
+DSP_OBJ += arm_cmplx_dot_prod_f32.o
+DSP_OBJ += arm_cmplx_dot_prod_q15.o
+DSP_OBJ += arm_cmplx_dot_prod_q31.o
+DSP_OBJ += arm_cmplx_mag_f32.o
+DSP_OBJ += arm_cmplx_mag_q15.o
+DSP_OBJ += arm_cmplx_mag_q31.o
+DSP_OBJ += arm_cmplx_mag_squared_f32.o
+DSP_OBJ += arm_cmplx_mag_squared_q15.o
+DSP_OBJ += arm_cmplx_mag_squared_q31.o
+DSP_OBJ += arm_cmplx_mult_cmplx_f32.o
+DSP_OBJ += arm_cmplx_mult_cmplx_q15.o
+DSP_OBJ += arm_cmplx_mult_cmplx_q31.o
+DSP_OBJ += arm_cmplx_mult_real_f32.o
+DSP_OBJ += arm_cmplx_mult_real_q15.o
+DSP_OBJ += arm_cmplx_mult_real_q31.o
+DSP_OBJ += arm_common_tables.o
+DSP_OBJ += arm_const_structs.o
+DSP_OBJ += arm_conv_f32.o
+DSP_OBJ += arm_conv_fast_opt_q15.o
+DSP_OBJ += arm_conv_fast_q15.o
+DSP_OBJ += arm_conv_fast_q31.o
+DSP_OBJ += arm_conv_opt_q15.o
+DSP_OBJ += arm_conv_opt_q7.o
+DSP_OBJ += arm_conv_partial_f32.o
+DSP_OBJ += arm_conv_partial_fast_opt_q15.o
 DSP_OBJ += arm_conv_partial_fast_q15.o
+DSP_OBJ += arm_conv_partial_fast_q31.o
+DSP_OBJ += arm_conv_partial_opt_q15.o
 DSP_OBJ += arm_conv_partial_opt_q7.o
-DSP_OBJ += arm_fir_interpolate_q15.o
-DSP_OBJ += arm_correlate_q31.o
-DSP_OBJ += arm_fir_decimate_q15.o
-DSP_OBJ += arm_fir_fast_q15.o
-DSP_OBJ += arm_fir_sparse_q31.o
+DSP_OBJ += arm_conv_partial_q15.o
+DSP_OBJ += arm_conv_partial_q31.o
+DSP_OBJ += arm_conv_partial_q7.o
 DSP_OBJ += arm_conv_q15.o
-DSP_OBJ += arm_conv_opt_q15.o
-DSP_OBJ += arm_fir_decimate_init_q15.o
-DSP_OBJ += arm_fir_sparse_init_q31.o
-DSP_OBJ += arm_conv_partial_fast_opt_q15.o
-DSP_OBJ += arm_iir_lattice_q15.o
-DSP_OBJ += arm_lms_init_f32.o
+DSP_OBJ += arm_conv_q31.o
+DSP_OBJ += arm_conv_q7.o
+DSP_OBJ += arm_copy_f32.o
+DSP_OBJ += arm_copy_q15.o
+DSP_OBJ += arm_copy_q31.o
+DSP_OBJ += arm_copy_q7.o
+DSP_OBJ += arm_correlate_f32.o
 DSP_OBJ += arm_correlate_fast_opt_q15.o
-DSP_OBJ += arm_lms_norm_f32.o
-DSP_OBJ += arm_conv_partial_q7.o
-DSP_OBJ += arm_fir_sparse_init_q15.o
-DSP_OBJ += arm_lms_init_q31.o
-DSP_OBJ += arm_iir_lattice_init_q15.o
-DSP_OBJ += arm_fir_q31.o
 DSP_OBJ += arm_correlate_fast_q15.o
-DSP_OBJ += arm_biquad_cascade_stereo_df2T_init_f32.o
-DSP_OBJ += arm_fir_sparse_init_q7.o
-DSP_OBJ += arm_fir_sparse_q7.o
-DSP_OBJ += arm_conv_q31.o
-DSP_OBJ += arm_biquad_cascade_df1_fast_q31.o
+DSP_OBJ += arm_correlate_fast_q31.o
+DSP_OBJ += arm_correlate_opt_q15.o
+DSP_OBJ += arm_correlate_opt_q7.o
+DSP_OBJ += arm_correlate_q15.o
+DSP_OBJ += arm_correlate_q31.o
+DSP_OBJ += arm_correlate_q7.o
+DSP_OBJ += arm_cos_f32.o
+DSP_OBJ += arm_cos_q15.o
+DSP_OBJ += arm_cos_q31.o
+DSP_OBJ += arm_dct4_f32.o
+DSP_OBJ += arm_dct4_init_f32.o
+DSP_OBJ += arm_dct4_init_q15.o
+DSP_OBJ += arm_dct4_init_q31.o
+DSP_OBJ += arm_dct4_q15.o
+DSP_OBJ += arm_dct4_q31.o
+DSP_OBJ += arm_dot_prod_f32.o
+DSP_OBJ += arm_dot_prod_q15.o
+DSP_OBJ += arm_dot_prod_q31.o
+DSP_OBJ += arm_dot_prod_q7.o
+DSP_OBJ += arm_fill_f32.o
+DSP_OBJ += arm_fill_q15.o
+DSP_OBJ += arm_fill_q31.o
+DSP_OBJ += arm_fill_q7.o
+DSP_OBJ += arm_fir_decimate_f32.o
+DSP_OBJ += arm_fir_decimate_fast_q15.o
+DSP_OBJ += arm_fir_decimate_fast_q31.o
+DSP_OBJ += arm_fir_decimate_init_f32.o
+DSP_OBJ += arm_fir_decimate_init_q15.o
+DSP_OBJ += arm_fir_decimate_init_q31.o
+DSP_OBJ += arm_fir_decimate_q15.o
+DSP_OBJ += arm_fir_decimate_q31.o
+DSP_OBJ += arm_fir_f32.o
+DSP_OBJ += arm_fir_fast_q15.o
+DSP_OBJ += arm_fir_fast_q31.o
+DSP_OBJ += arm_fir_init_f32.o
+DSP_OBJ += arm_fir_init_q15.o
+DSP_OBJ += arm_fir_init_q31.o
+DSP_OBJ += arm_fir_init_q7.o
+DSP_OBJ += arm_fir_interpolate_f32.o
+DSP_OBJ += arm_fir_interpolate_init_f32.o
+DSP_OBJ += arm_fir_interpolate_init_q15.o
+DSP_OBJ += arm_fir_interpolate_init_q31.o
+DSP_OBJ += arm_fir_interpolate_q15.o
 DSP_OBJ += arm_fir_interpolate_q31.o
-DSP_OBJ += arm_biquad_cascade_df1_32x64_q31.o
+DSP_OBJ += arm_fir_lattice_f32.o
 DSP_OBJ += arm_fir_lattice_init_f32.o
-DSP_OBJ += arm_lms_norm_init_q31.o
-DSP_OBJ += arm_fir_init_q7.o
-DSP_OBJ += arm_biquad_cascade_df2T_init_f64.o
+DSP_OBJ += arm_fir_lattice_init_q15.o
 DSP_OBJ += arm_fir_lattice_init_q31.o
-DSP_OBJ += arm_biquad_cascade_df1_f32.o
-DSP_OBJ += arm_correlate_f32.o
-DSP_OBJ += arm_fir_decimate_init_f32.o
-DSP_OBJ += arm_biquad_cascade_df1_init_f32.o
-DSP_OBJ += arm_lms_q31.o
 DSP_OBJ += arm_fir_lattice_q15.o
-DSP_OBJ += arm_lms_f32.o
-DSP_OBJ += arm_biquad_cascade_df2T_f32.o
-DSP_OBJ += arm_conv_fast_q15.o
-DSP_OBJ += arm_iir_lattice_init_f32.o
-DSP_OBJ += arm_correlate_q7.o
-DSP_OBJ += arm_fir_decimate_fast_q15.o
-DSP_OBJ += arm_correlate_fast_q31.o
-DSP_OBJ += arm_biquad_cascade_df2T_f64.o
-DSP_OBJ += arm_iir_lattice_q31.o
-DSP_OBJ += arm_conv_partial_opt_q15.o
-DSP_OBJ += arm_lms_q15.o
-DSP_OBJ += arm_fir_init_f32.o
-DSP_OBJ += arm_conv_partial_fast_q31.o
+DSP_OBJ += arm_fir_lattice_q31.o
+DSP_OBJ += arm_fir_q15.o
+DSP_OBJ += arm_fir_q31.o
+DSP_OBJ += arm_fir_q7.o
 DSP_OBJ += arm_fir_sparse_f32.o
-DSP_OBJ += arm_lms_norm_q31.o
-DSP_OBJ += arm_fir_f32.o
+DSP_OBJ += arm_fir_sparse_init_f32.o
+DSP_OBJ += arm_fir_sparse_init_q15.o
+DSP_OBJ += arm_fir_sparse_init_q31.o
+DSP_OBJ += arm_fir_sparse_init_q7.o
+DSP_OBJ += arm_fir_sparse_q15.o
+DSP_OBJ += arm_fir_sparse_q31.o
+DSP_OBJ += arm_fir_sparse_q7.o
+DSP_OBJ += arm_float_to_q15.o
+DSP_OBJ += arm_float_to_q31.o
+DSP_OBJ += arm_float_to_q7.o
 DSP_OBJ += arm_iir_lattice_f32.o
-DSP_OBJ += arm_correlate_opt_q15.o
-DSP_OBJ += arm_fir_interpolate_init_f32.o
+DSP_OBJ += arm_iir_lattice_init_f32.o
+DSP_OBJ += arm_iir_lattice_init_q15.o
+DSP_OBJ += arm_iir_lattice_init_q31.o
+DSP_OBJ += arm_iir_lattice_q15.o
+DSP_OBJ += arm_iir_lattice_q31.o
+DSP_OBJ += arm_lms_f32.o
+DSP_OBJ += arm_lms_init_f32.o
+DSP_OBJ += arm_lms_init_q15.o
+DSP_OBJ += arm_lms_init_q31.o
+DSP_OBJ += arm_lms_norm_f32.o
 DSP_OBJ += arm_lms_norm_init_f32.o
-DSP_OBJ += arm_conv_f32.o
-DSP_OBJ += arm_fir_decimate_q31.o
-DSP_OBJ += arm_fir_decimate_f32.o
-DSP_OBJ += arm_biquad_cascade_df1_fast_q15.o
-DSP_OBJ += arm_biquad_cascade_df1_q15.o
 DSP_OBJ += arm_lms_norm_init_q15.o
-DSP_OBJ += arm_fir_interpolate_f32.o
-DSP_OBJ += arm_conv_opt_q7.o
-DSP_OBJ += arm_conv_partial_f32.o
-DSP_OBJ += arm_conv_fast_opt_q15.o
-DSP_OBJ += arm_fir_sparse_init_f32.o
-DSP_OBJ += arm_fir_init_q15.o
-DSP_OBJ += arm_fir_lattice_q31.o
-DSP_OBJ += arm_fir_fast_q31.o
-DSP_OBJ += arm_fir_q15.o
-DSP_OBJ += arm_lms_init_q15.o
-DSP_OBJ += arm_fir_interpolate_init_q15.o
-DSP_OBJ += arm_fir_init_q31.o
-DSP_OBJ += arm_conv_fast_q31.o
-DSP_OBJ += arm_cmplx_mult_real_q15.o
-DSP_OBJ += arm_cmplx_mag_squared_q15.o
-DSP_OBJ += arm_cmplx_mult_cmplx_q15.o
-DSP_OBJ += arm_cmplx_mag_q31.o
-DSP_OBJ += arm_cmplx_mag_squared_q31.o
-DSP_OBJ += arm_cmplx_mult_cmplx_f32.o
-DSP_OBJ += arm_cmplx_conj_q31.o
-DSP_OBJ += arm_cmplx_dot_prod_q15.o
-DSP_OBJ += arm_cmplx_conj_q15.o
-DSP_OBJ += arm_cmplx_mag_q15.o
-DSP_OBJ += arm_cmplx_conj_f32.o
-DSP_OBJ += arm_cmplx_dot_prod_f32.o
-DSP_OBJ += arm_cmplx_mag_f32.o
-DSP_OBJ += arm_cmplx_mult_cmplx_q31.o
-DSP_OBJ += arm_cmplx_dot_prod_q31.o
-DSP_OBJ += arm_cmplx_mult_real_q31.o
-DSP_OBJ += arm_cmplx_mag_squared_f32.o
-DSP_OBJ += arm_cmplx_mult_real_f32.o
-DSP_OBJ += arm_shift_q7.o
-DSP_OBJ += arm_offset_q15.o
+DSP_OBJ += arm_lms_norm_init_q31.o
+DSP_OBJ += arm_lms_norm_q15.o
+DSP_OBJ += arm_lms_norm_q31.o
+DSP_OBJ += arm_lms_q15.o
+DSP_OBJ += arm_lms_q31.o
+DSP_OBJ += arm_mat_add_f32.o
+DSP_OBJ += arm_mat_add_q15.o
+DSP_OBJ += arm_mat_add_q31.o
+DSP_OBJ += arm_mat_cmplx_mult_f32.o
+DSP_OBJ += arm_mat_cmplx_mult_q15.o
+DSP_OBJ += arm_mat_cmplx_mult_q31.o
+DSP_OBJ += arm_mat_init_f32.o
+DSP_OBJ += arm_mat_init_q15.o
+DSP_OBJ += arm_mat_init_q31.o
+DSP_OBJ += arm_mat_inverse_f32.o
+DSP_OBJ += arm_mat_inverse_f64.o
+DSP_OBJ += arm_mat_mult_f32.o
+DSP_OBJ += arm_mat_mult_fast_q15.o
+DSP_OBJ += arm_mat_mult_fast_q31.o
+DSP_OBJ += arm_mat_mult_q15.o
+DSP_OBJ += arm_mat_mult_q31.o
+DSP_OBJ += arm_mat_scale_f32.o
+DSP_OBJ += arm_mat_scale_q15.o
+DSP_OBJ += arm_mat_scale_q31.o
+DSP_OBJ += arm_mat_sub_f32.o
+DSP_OBJ += arm_mat_sub_q15.o
+DSP_OBJ += arm_mat_sub_q31.o
+DSP_OBJ += arm_mat_trans_f32.o
+DSP_OBJ += arm_mat_trans_q15.o
+DSP_OBJ += arm_mat_trans_q31.o
+DSP_OBJ += arm_max_f32.o
+DSP_OBJ += arm_max_q15.o
+DSP_OBJ += arm_max_q31.o
+DSP_OBJ += arm_max_q7.o
+DSP_OBJ += arm_mean_f32.o
+DSP_OBJ += arm_mean_q15.o
+DSP_OBJ += arm_mean_q31.o
+DSP_OBJ += arm_mean_q7.o
+DSP_OBJ += arm_min_f32.o
+DSP_OBJ += arm_min_q15.o
+DSP_OBJ += arm_min_q31.o
+DSP_OBJ += arm_min_q7.o
+DSP_OBJ += arm_mult_f32.o
+DSP_OBJ += arm_mult_q15.o
+DSP_OBJ += arm_mult_q31.o
+DSP_OBJ += arm_mult_q7.o
+DSP_OBJ += arm_negate_f32.o
+DSP_OBJ += arm_negate_q15.o
 DSP_OBJ += arm_negate_q31.o
-DSP_OBJ += arm_offset_f32.o
-DSP_OBJ += arm_abs_f32.o
-DSP_OBJ += arm_sub_q15.o
-DSP_OBJ += arm_scale_q31.o
 DSP_OBJ += arm_negate_q7.o
-DSP_OBJ += arm_negate_q15.o
+DSP_OBJ += arm_offset_f32.o
+DSP_OBJ += arm_offset_q15.o
 DSP_OBJ += arm_offset_q31.o
-DSP_OBJ += arm_add_q31.o
-DSP_OBJ += arm_abs_q7.o
-DSP_OBJ += arm_abs_q31.o
-DSP_OBJ += arm_shift_q15.o
-DSP_OBJ += arm_shift_q31.o
-DSP_OBJ += arm_dot_prod_q15.o
-DSP_OBJ += arm_mult_q7.o
-DSP_OBJ += arm_scale_q15.o
-DSP_OBJ += arm_dot_prod_f32.o
 DSP_OBJ += arm_offset_q7.o
-DSP_OBJ += arm_sub_f32.o
-DSP_OBJ += arm_mult_q31.o
-DSP_OBJ += arm_dot_prod_q7.o
-DSP_OBJ += arm_mult_f32.o
-DSP_OBJ += arm_scale_q7.o
-DSP_OBJ += arm_scale_f32.o
-DSP_OBJ += arm_add_q15.o
-DSP_OBJ += arm_sub_q31.o
-DSP_OBJ += arm_negate_f32.o
-DSP_OBJ += arm_mult_q15.o
-DSP_OBJ += arm_add_q7.o
-DSP_OBJ += arm_sub_q7.o
-DSP_OBJ += arm_abs_q15.o
-DSP_OBJ += arm_dot_prod_q31.o
-DSP_OBJ += arm_add_f32.o
-DSP_OBJ += arm_dct4_q15.o
-DSP_OBJ += arm_cfft_radix4_init_f32.o
+DSP_OBJ += arm_pid_init_f32.o
+DSP_OBJ += arm_pid_init_q15.o
+DSP_OBJ += arm_pid_init_q31.o
+DSP_OBJ += arm_pid_reset_f32.o
+DSP_OBJ += arm_pid_reset_q15.o
+DSP_OBJ += arm_pid_reset_q31.o
+DSP_OBJ += arm_power_f32.o
+DSP_OBJ += arm_power_q15.o
+DSP_OBJ += arm_power_q31.o
+DSP_OBJ += arm_power_q7.o
+DSP_OBJ += arm_q15_to_float.o
+DSP_OBJ += arm_q15_to_q31.o
+DSP_OBJ += arm_q15_to_q7.o
+DSP_OBJ += arm_q31_to_float.o
+DSP_OBJ += arm_q31_to_q15.o
+DSP_OBJ += arm_q31_to_q7.o
+DSP_OBJ += arm_q7_to_float.o
+DSP_OBJ += arm_q7_to_q15.o
+DSP_OBJ += arm_q7_to_q31.o
 DSP_OBJ += arm_rfft_f32.o
-DSP_OBJ += arm_cfft_radix4_init_q15.o
-DSP_OBJ += arm_cfft_radix8_f32.o
-DSP_OBJ += arm_cfft_radix4_q31.o
-DSP_OBJ += arm_dct4_init_q15.o
-DSP_OBJ += arm_cfft_radix2_init_q15.o
-DSP_OBJ += arm_cfft_q31.o
-DSP_OBJ += arm_cfft_radix2_q15.o
-DSP_OBJ += arm_dct4_init_f32.o
-DSP_OBJ += arm_dct4_init_q31.o
-DSP_OBJ += arm_cfft_radix4_q15.o
-DSP_OBJ += arm_rfft_init_q31.o
-DSP_OBJ += arm_cfft_f32.o
-DSP_OBJ += arm_cfft_radix2_init_q31.o
-DSP_OBJ += arm_cfft_q15.o
-DSP_OBJ += arm_rfft_q15.o
-DSP_OBJ += arm_cfft_radix2_init_f32.o
-DSP_OBJ += arm_cfft_radix2_f32.o
-DSP_OBJ += arm_dct4_f32.o
+DSP_OBJ += arm_rfft_fast_f32.o
 DSP_OBJ += arm_rfft_fast_init_f32.o
-DSP_OBJ += arm_cfft_radix4_f32.o
-DSP_OBJ += arm_rfft_init_q15.o
-DSP_OBJ += arm_cfft_radix2_q31.o
 DSP_OBJ += arm_rfft_init_f32.o
-DSP_OBJ += arm_bitreversal.o
-DSP_OBJ += arm_rfft_fast_f32.o
-DSP_OBJ += arm_dct4_q31.o
+DSP_OBJ += arm_rfft_init_q15.o
+DSP_OBJ += arm_rfft_init_q31.o
+DSP_OBJ += arm_rfft_q15.o
 DSP_OBJ += arm_rfft_q31.o
-DSP_OBJ += arm_cfft_radix4_init_q31.o
-DSP_OBJ += arm_std_q31.o
-DSP_OBJ += arm_mean_q7.o
-DSP_OBJ += arm_mean_f32.o
-DSP_OBJ += arm_max_q7.o
-DSP_OBJ += arm_mean_q15.o
-DSP_OBJ += arm_var_q15.o
-DSP_OBJ += arm_power_f32.o
-DSP_OBJ += arm_min_f32.o
-DSP_OBJ += arm_power_q7.o
-DSP_OBJ += arm_var_f32.o
-DSP_OBJ += arm_max_q15.o
-DSP_OBJ += arm_max_q31.o
-DSP_OBJ += arm_max_f32.o
-DSP_OBJ += arm_min_q7.o
-DSP_OBJ += arm_std_f32.o
-DSP_OBJ += arm_power_q15.o
-DSP_OBJ += arm_rms_q15.o
-DSP_OBJ += arm_power_q31.o
 DSP_OBJ += arm_rms_f32.o
-DSP_OBJ += arm_mean_q31.o
-DSP_OBJ += arm_var_q31.o
+DSP_OBJ += arm_rms_q15.o
 DSP_OBJ += arm_rms_q31.o
-DSP_OBJ += arm_min_q31.o
-DSP_OBJ += arm_min_q15.o
-DSP_OBJ += arm_std_q15.o
+DSP_OBJ += arm_scale_f32.o
+DSP_OBJ += arm_scale_q15.o
+DSP_OBJ += arm_scale_q31.o
+DSP_OBJ += arm_scale_q7.o
+DSP_OBJ += arm_shift_q15.o
+DSP_OBJ += arm_shift_q31.o
+DSP_OBJ += arm_shift_q7.o
 DSP_OBJ += arm_sin_cos_f32.o
-DSP_OBJ += arm_pid_reset_f32.o
-DSP_OBJ += arm_pid_reset_q31.o
-DSP_OBJ += arm_pid_init_q15.o
-DSP_OBJ += arm_pid_init_q31.o
-DSP_OBJ += arm_pid_reset_q15.o
-DSP_OBJ += arm_pid_init_f32.o
 DSP_OBJ += arm_sin_cos_q31.o
-DSP_OBJ += arm_common_tables.o
-DSP_OBJ += arm_const_structs.o
-DSP_OBJ += arm_copy_f32.o
-DSP_OBJ += arm_fill_f32.o
-DSP_OBJ += arm_q15_to_float.o
-DSP_OBJ += arm_float_to_q15.o
-DSP_OBJ += arm_q31_to_q7.o
-DSP_OBJ += arm_copy_q31.o
-DSP_OBJ += arm_float_to_q31.o
-DSP_OBJ += arm_float_to_q7.o
-DSP_OBJ += arm_fill_q7.o
-DSP_OBJ += arm_fill_q15.o
-DSP_OBJ += arm_q7_to_q31.o
-DSP_OBJ += arm_q31_to_float.o
-DSP_OBJ += arm_fill_q31.o
-DSP_OBJ += arm_q7_to_q15.o
-DSP_OBJ += arm_copy_q7.o
-DSP_OBJ += arm_q31_to_q15.o
-DSP_OBJ += arm_copy_q15.o
-DSP_OBJ += arm_q7_to_float.o
-DSP_OBJ += arm_q15_to_q7.o
-DSP_OBJ += arm_q15_to_q31.o
-DSP_OBJ += arm_mat_mult_fast_q31.o
-DSP_OBJ += arm_mat_init_f32.o
-DSP_OBJ += arm_mat_mult_q31.o
-DSP_OBJ += arm_mat_add_q15.o
-DSP_OBJ += arm_mat_cmplx_mult_f32.o
-DSP_OBJ += arm_mat_add_f32.o
-DSP_OBJ += arm_mat_init_q31.o
-DSP_OBJ += arm_mat_trans_q15.o
-DSP_OBJ += arm_mat_cmplx_mult_q15.o
-DSP_OBJ += arm_mat_add_q31.o
-DSP_OBJ += arm_mat_mult_q15.o
-DSP_OBJ += arm_mat_sub_q15.o
-DSP_OBJ += arm_mat_sub_q31.o
-DSP_OBJ += arm_mat_init_q15.o
-DSP_OBJ += arm_mat_mult_f32.o
-DSP_OBJ += arm_mat_scale_f32.o
-DSP_OBJ += arm_mat_sub_f32.o
-DSP_OBJ += arm_mat_inverse_f32.o
-DSP_OBJ += arm_mat_cmplx_mult_q31.o
-DSP_OBJ += arm_mat_inverse_f64.o
-DSP_OBJ += arm_mat_scale_q31.o
-DSP_OBJ += arm_mat_scale_q15.o
-DSP_OBJ += arm_mat_trans_q31.o
-DSP_OBJ += arm_mat_mult_fast_q15.o
-DSP_OBJ += arm_mat_trans_f32.o
+DSP_OBJ += arm_sin_f32.o
+DSP_OBJ += arm_sin_q15.o
 DSP_OBJ += arm_sin_q31.o
 DSP_OBJ += arm_sqrt_q15.o
 DSP_OBJ += arm_sqrt_q31.o
-DSP_OBJ += arm_cos_q15.o
-DSP_OBJ += arm_cos_f32.o
-DSP_OBJ += arm_cos_q31.o
-DSP_OBJ += arm_sin_f32.o
-DSP_OBJ += arm_sin_q15.o
-
-DSP_OBJ += arm_bitreversal2.o
+DSP_OBJ += arm_std_f32.o
+DSP_OBJ += arm_std_q15.o
+DSP_OBJ += arm_std_q31.o
+DSP_OBJ += arm_sub_f32.o
+DSP_OBJ += arm_sub_q15.o
+DSP_OBJ += arm_sub_q31.o
+DSP_OBJ += arm_sub_q7.o
+DSP_OBJ += arm_var_f32.o
+DSP_OBJ += arm_var_q15.o
+DSP_OBJ += arm_var_q31.o
diff --git a/tools/test/gcc.yml b/tools/test/gcc.yml
index ffbe9de9e8..877090bb91 100644
--- a/tools/test/gcc.yml
+++ b/tools/test/gcc.yml
@@ -23,34 +23,35 @@ compiler:
       - *source_path
       - *unit_tests_path
       - *mocks_path
-      - 'vendor/unity/src/'
-      - 'vendor/cmock/src/'
-      - 'vendor/libdw1000/inc/'
+      - 'src/config'
       - 'src/deck/drivers/interface/'
       - 'src/deck/drivers/src/'
-      - 'src/utils/interface/'
-      - 'src/utils/interface/tdoa/'
-      - 'src/utils/interface/lighthouse/'
-      - 'src/utils/src/'
-      - 'src/utils/src/tdoa/'
-      - 'src/utils/src/lighthouse/'
-      - 'vendor/FreeRTOS/include/'
-      - 'src/config/'
       - 'src/drivers/interface/'
-      - 'src/drivers/src/'
+      - 'src/drivers/src'
+      - 'src/hal/interface/'
+      - 'src/lib/CMSIS/STM32F4xx/Include'
+      - 'src/lib/STM32F4xx_StdPeriph_Driver/inc'
       - 'src/modules/interface/'
-      - 'src/modules/interface/lighthouse/'
       - 'src/modules/interface/kalman_core/'
+      - 'src/modules/interface/lighthouse/'
       - 'src/modules/src/'
-      - 'src/modules/src/lighthouse/'
       - 'src/modules/src/kalman_core/'
+      - 'src/modules/src/lighthouse/'
       - 'src/platform/'
-      - 'vendor/FreeRTOS/portable/GCC/ARM_CM4F/'
-      - 'src/hal/interface/'
+      - 'src/utils/interface/'
+      - 'src/utils/interface/lighthouse/'
+      - 'src/utils/interface/tdoa'
+      - 'src/utils/src/'
+      - 'src/utils/src/lighthouse/'
+      - 'src/utils/src/tdoa/'
       - 'test/testSupport/'
-      - 'vendor/CMSIS/CMSIS/Include/'
-      - 'src/lib/CMSIS/STM32F4xx/Include'
-      - 'src/lib/STM32F4xx_StdPeriph_Driver/inc'
+      - 'vendor/CMSIS/CMSIS/Core/Include'
+      - 'vendor/CMSIS/CMSIS/DSP/Include'
+      - 'vendor/FreeRTOS/include'
+      - 'vendor/FreeRTOS/portable/GCC/ARM_CM4F'
+      - 'vendor/cmock/src/'
+      - 'vendor/libdw1000/inc/'
+      - 'vendor/unity/src/'
   defines:
     prefix: '-D'
     items:
@@ -68,15 +69,15 @@ compiler:
   libs:
     ARM_DSP_MATH:
       files:
-        - 'vendor/CMSIS/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_add_f32.c'
-        - 'vendor/CMSIS/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_sub_f32.c'
-        - 'vendor/CMSIS/CMSIS/DSP_Lib/Source/StatisticsFunctions/arm_power_f32.c'
-        - 'vendor/CMSIS/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_sin_f32.c'
-        - 'vendor/CMSIS/CMSIS/DSP_Lib/Source/FastMathFunctions/arm_cos_f32.c'
-        - 'vendor/CMSIS/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_dot_prod_f32.c'
-        - 'vendor/CMSIS/CMSIS/DSP_Lib/Source/MatrixFunctions/arm_mat_mult_f32.c'
-        - 'vendor/CMSIS/CMSIS/DSP_Lib/Source/BasicMathFunctions/arm_scale_f32.c'
-        - 'vendor/CMSIS/CMSIS/DSP_Lib/Source/CommonTables/arm_common_tables.c'
+        - 'vendor/CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_add_f32.c'
+        - 'vendor/CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_dot_prod_f32.c'
+        - 'vendor/CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_scale_f32.c'
+        - 'vendor/CMSIS/CMSIS/DSP/Source/BasicMathFunctions/arm_sub_f32.c'
+        - 'vendor/CMSIS/CMSIS/DSP/Source/CommonTables/arm_common_tables.c'
+        - 'vendor/CMSIS/CMSIS/DSP/Source/FastMathFunctions/arm_cos_f32.c'
+        - 'vendor/CMSIS/CMSIS/DSP/Source/FastMathFunctions/arm_sin_f32.c'
+        - 'vendor/CMSIS/CMSIS/DSP/Source/MatrixFunctions/arm_mat_mult_f32.c'
+        - 'vendor/CMSIS/CMSIS/DSP/Source/StatisticsFunctions/arm_power_f32.c'
       extra_options:
         - '-Wno-overflow'
 
diff --git a/vendor/CMSIS b/vendor/CMSIS
index 0156c6f40e..a65b7c9a3e 160000
--- a/vendor/CMSIS
+++ b/vendor/CMSIS
@@ -1 +1 @@
-Subproject commit 0156c6f40ec9556bf46b2d4a947ae3ef872cd1bd
+Subproject commit a65b7c9a3e6502127fdb80eb288d8cbdf251a6f4