diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board
index 9d6594a5a7..4190588fa7 100644
--- a/boards/bitcraze/crazyflie21/default.px4board
+++ b/boards/bitcraze/crazyflie21/default.px4board
@@ -6,7 +6,7 @@ CONFIG_BOARD_COMPILE_DEFINITIONS="-Wno-narrowing"
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
 CONFIG_DRIVERS_GPS=y
-CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
+CONFIG_DRIVERS_IMU_BOSCH_BMI088_I2C=y
 CONFIG_DRIVERS_OPTICAL_FLOW_PMW3901=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
diff --git a/boards/freefly/can-rtk-gps/default.px4board b/boards/freefly/can-rtk-gps/default.px4board
index b3d974db3e..83619d320a 100644
--- a/boards/freefly/can-rtk-gps/default.px4board
+++ b/boards/freefly/can-rtk-gps/default.px4board
@@ -4,9 +4,9 @@ CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_IMU_ST=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
-CONFIG_DRIVERS_MAGNETOMETER_LSM9DS1_MAG=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
 CONFIG_MODULES_LOAD_MON=y
diff --git a/boards/holybro/durandal-v1/default.px4board b/boards/holybro/durandal-v1/default.px4board
index 996bf4ac2d..b525c2195c 100644
--- a/boards/holybro/durandal-v1/default.px4board
+++ b/boards/holybro/durandal-v1/default.px4board
@@ -89,6 +89,7 @@ CONFIG_SYSTEMCMDS_PWM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SERIAL_TEST=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
diff --git a/boards/px4/fmu-v5/optimized.px4board b/boards/px4/fmu-v5/optimized.px4board
index 08adfe32cb..e429f27c82 100644
--- a/boards/px4/fmu-v5/optimized.px4board
+++ b/boards/px4/fmu-v5/optimized.px4board
@@ -1,6 +1,7 @@
 # CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
 CONFIG_COMMON_MAGNETOMETER=n
 CONFIG_COMMON_OPTICAL_FLOW=n
+CONFIG_COMMON_TELEMETRY=n
 CONFIG_DRIVERS_HEATER=n
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
@@ -12,7 +13,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=n
 CONFIG_DRIVERS_PWM_INPUT=n
 CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_RPM=n
-CONFIG_DRIVERS_TELEMETRY=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_ESC_BATTERY=n
diff --git a/cmake/kconfig.cmake b/cmake/kconfig.cmake
index b06a7b3f8d..f584921f88 100644
--- a/cmake/kconfig.cmake
+++ b/cmake/kconfig.cmake
@@ -106,6 +106,11 @@ if(EXISTS ${BOARD_DEFCONFIG})
             string(REGEX REPLACE "(^[a-z]+_[a-z0-9]+)_([a-z_0-9]+).*$" "\\1" driver_p4_folder ${driver})
             string(REGEX REPLACE "(^[a-z]+_[a-z0-9]+)_([a-z_0-9]+).*$" "\\2" driver_p4_subfolder ${driver})
 
+            # Pattern 5 XXXXXX / XXXXXX / XXX_XXX
+            string(REGEX REPLACE "(^[a-z]+)_([a-z0-9]+[a-z0-9]+)_([a-z0-9]+_[a-z0-9]+).*$" "\\1" driver_p5_folder ${driver})
+            string(REGEX REPLACE "(^[a-z]+)_([a-z0-9]+[a-z0-9]+)_([a-z0-9]+_[a-z0-9]+).*$" "\\2" driver_p5_subfolder ${driver})
+            string(REGEX REPLACE "(^[a-z]+)_([a-z0-9]+[a-z0-9]+)_([a-z0-9]+_[a-z0-9]+).*$" "\\3" driver_p5_subsubfolder ${driver})
+
             # Trick circumvent PX4 src naming problem with underscores and slashes
             if(EXISTS ${PX4_SOURCE_DIR}/src/drivers/${driver})
                 list(APPEND config_module_list drivers/${driver})
@@ -119,6 +124,8 @@ if(EXISTS ${BOARD_DEFCONFIG})
                 list(APPEND config_module_list drivers/${driver_p4_folder}/${driver_p4_subfolder})
             elseif(EXISTS ${PX4_SOURCE_DIR}/src/drivers/${driver_p2_folder}/${driver_p2_subfolder})
                 list(APPEND config_module_list drivers/${driver_p2_folder}/${driver_p2_subfolder})
+            elseif(EXISTS ${PX4_SOURCE_DIR}/src/drivers/${driver_p5_folder}/${driver_p5_subfolder}/${driver_p5_subsubfolder})
+                list(APPEND config_module_list drivers/${driver_p5_folder}/${driver_p5_subfolder}/${driver_p5_subsubfolder})
             else()
                 message(FATAL_ERROR "Couldn't find path for ${driver}")
             endif()
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088.cpp
similarity index 100%
rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.cpp
rename to src/drivers/imu/bosch/bmi088_i2c/BMI088.cpp
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp
similarity index 100%
rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088.hpp
rename to src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp
similarity index 100%
rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.cpp
rename to src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp
similarity index 100%
rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Accelerometer.hpp
rename to src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.hpp
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp
similarity index 100%
rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.cpp
rename to src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp
similarity index 100%
rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/BMI088_Gyroscope.hpp
rename to src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.hpp
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp b/src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp
similarity index 100%
rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp
rename to src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Accelerometer_Registers.hpp
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp b/src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp
similarity index 100%
rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp
rename to src/drivers/imu/bosch/bmi088_i2c/Bosch_BMI088_Gyroscope_Registers.hpp
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/CMakeLists.txt b/src/drivers/imu/bosch/bmi088_i2c/CMakeLists.txt
similarity index 100%
rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/CMakeLists.txt
rename to src/drivers/imu/bosch/bmi088_i2c/CMakeLists.txt
diff --git a/src/drivers/imu/bosch/bmi088/bmi088_i2c/bmi088_i2c_main.cpp b/src/drivers/imu/bosch/bmi088_i2c/bmi088_i2c_main.cpp
similarity index 100%
rename from src/drivers/imu/bosch/bmi088/bmi088_i2c/bmi088_i2c_main.cpp
rename to src/drivers/imu/bosch/bmi088_i2c/bmi088_i2c_main.cpp