diff --git a/radio/src/drivers/icm42627.h b/radio/src/drivers/icm42627.h index 70c9395a6aa..1b9df0f26b7 100644 --- a/radio/src/drivers/icm42627.h +++ b/radio/src/drivers/icm42627.h @@ -23,6 +23,8 @@ #include "hal/imu.h" +#define ICM42627_I2C_BASE_ADDR 0x68 + #define ICM42627_WHO_AM_I_REG 0x75 #define ICM42627_WHO_AM_I 0x20 diff --git a/radio/src/targets/horus/CMakeLists.txt b/radio/src/targets/horus/CMakeLists.txt index 80c04f93a8f..16a0e29ffa2 100644 --- a/radio/src/targets/horus/CMakeLists.txt +++ b/radio/src/targets/horus/CMakeLists.txt @@ -116,6 +116,8 @@ if (PCB STREQUAL X10) set(AUX2_SERIAL OFF) set(CSD203_SENSOR ON) set(FLYSKY_GIMBAL ON) + add_definitions(-DIMU_ICM42627) + add_definitions(-DIMU_SC7U22) if (BLUETOOTH) set(AUX_SERIAL OFF) endif() @@ -296,6 +298,10 @@ add_library(board OBJECT EXCLUDE_FROM_ALL ) set(FIRMWARE_SRC ${FIRMWARE_SRC} $) +if(FLAVOUR STREQUAL v16) + target_sources(board PRIVATE drivers/sc7u22.cpp) +endif() + if(BLUETOOTH) target_sources(board PRIVATE targets/common/arm/stm32/bluetooth_driver.cpp) endif() diff --git a/radio/src/targets/horus/board.cpp b/radio/src/targets/horus/board.cpp index 04308fd289e..c4f967984d1 100644 --- a/radio/src/targets/horus/board.cpp +++ b/radio/src/targets/horus/board.cpp @@ -52,7 +52,8 @@ #include "csd203_sensor.h" #endif -#if defined(IMU) && defined(IMU_I2C_BUS) && defined(IMU_I2C_ADDRESS) +#if defined(IMU) && defined(IMU_I2C_BUS) && \ + (defined(RADIO_V16) || defined(IMU_I2C_ADDRESS)) #define HAS_IMU #endif @@ -137,21 +138,40 @@ void audioInit() #include "stm32_i2c_driver.h" #include "drivers/icm42627.h" #include "drivers/lsm6ds.h" +#if defined(IMU_SC7U22) +#include "drivers/sc7u22.h" +#endif + +#if defined(RADIO_V16) + +static const etx_imu_t _imu_candidates[] = { +#if defined(IMU_ICM42627) + { &imu_icm42627_driver, IMU_I2C_BUS, ICM42627_I2C_BASE_ADDR }, + { &imu_icm42627_driver, IMU_I2C_BUS, ICM42627_I2C_BASE_ADDR + 1 }, +#endif +#if defined(IMU_SC7U22) + { &imu_sc7u22_driver, IMU_I2C_BUS, SC7U22_I2C_BASE_ADDR }, + { &imu_sc7u22_driver, IMU_I2C_BUS, SC7U22_I2C_BASE_ADDR + 1 }, +#endif +}; static void gyroInit() { -#if defined(RADIO_V16) - const etx_imu_driver_t *driver = &imu_icm42627_driver; + gyroStart(imuDetect(_imu_candidates, DIM(_imu_candidates))); +} + #else - const etx_imu_driver_t *driver = &imu_lsm6ds_driver; -#endif +static void gyroInit() +{ const etx_imu_t candidates[] = { - { driver, IMU_I2C_BUS, IMU_I2C_ADDRESS }, + { &imu_lsm6ds_driver, IMU_I2C_BUS, IMU_I2C_ADDRESS }, }; gyroStart(imuDetect(candidates, DIM(candidates))); } + +#endif #endif void boardInit() diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h index 4ba08f6f501..098dc11143b 100644 --- a/radio/src/targets/horus/hal.h +++ b/radio/src/targets/horus/hal.h @@ -493,7 +493,6 @@ #define IMU_I2C_ADDRESS 0x6A #elif defined(RADIO_V16) #define IMU_I2C_BUS I2C_Bus_1 - #define IMU_I2C_ADDRESS 0x69 #elif !defined(AUX_SERIAL) && defined(IMU_LSM6DS33) #define IMU_I2C_BUS I2C_Bus_2 #define IMU_I2C_ADDRESS 0x6A