From 5fef8e3abc59302d81c8e6172750a6c0dead248f Mon Sep 17 00:00:00 2001 From: gismo2004 Date: Sun, 29 Mar 2026 15:28:14 +0200 Subject: [PATCH 1/8] feat(radio): add gyro support for HelloRadio V16 --- radio/src/drivers/icm42627.cpp | 283 +++++++++++++++++++++++++ radio/src/drivers/icm42627.h | 54 +++++ radio/src/targets/horus/CMakeLists.txt | 3 +- radio/src/targets/horus/board.cpp | 13 +- radio/src/targets/horus/hal.h | 9 +- 5 files changed, 356 insertions(+), 6 deletions(-) create mode 100644 radio/src/drivers/icm42627.cpp create mode 100644 radio/src/drivers/icm42627.h diff --git a/radio/src/drivers/icm42627.cpp b/radio/src/drivers/icm42627.cpp new file mode 100644 index 00000000000..5eae3c1106b --- /dev/null +++ b/radio/src/drivers/icm42627.cpp @@ -0,0 +1,283 @@ +/* + * Copyright (C) EdgeTX + * + * Based on code named + * opentx - https://github.com/opentx/opentx + * th9x - http://code.google.com/p/th9x + * er9x - http://code.google.com/p/er9x + * gruvin9x - http://code.google.com/p/gruvin9x + * + * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include "hal/i2c_driver.h" + +#include "delays_driver.h" + +#include "icm42627.h" + +#include "debug.h" + +static etx_i2c_bus_t s_i2c_bus; +static uint16_t s_i2c_addr; +static bool s_trace_first_sample; + +static constexpr uint32_t RESET_DELAY_MS = 150; +static constexpr uint32_t READY_TIMEOUT_MS = 200; +static constexpr uint32_t POWER_DELAY_MS = 20; +static constexpr uint32_t VERIFY_DELAY_MS = 10; +static constexpr uint32_t CONFIG_DELAY_US = 100; +static constexpr uint8_t BURST_READ_LEN = 14; + +static int write_cmd(uint8_t reg, uint8_t value) +{ + return i2c_write(s_i2c_bus, s_i2c_addr, reg, 1, &value, 1); +} + +static int read_cmd(uint8_t reg, uint8_t *value) +{ + return i2c_read(s_i2c_bus, s_i2c_addr, reg, 1, value, 1); +} + +static int read_who_am_i(uint8_t *value) +{ + return read_cmd(ICM42627_WHO_AM_I_REG, value); +} + +static int write_cmd_retry(uint8_t reg, uint8_t value) +{ + if (write_cmd(reg, value) >= 0) { + return 0; + } + + return write_cmd(reg, value); +} + +static int select_bank(uint8_t bank) +{ + return write_cmd_retry(ICM42627_BANK_SEL_REG, bank); +} + +static int write_cmd_delay_ms(uint8_t reg, uint8_t value, uint32_t delay, + const char *error) +{ + if (write_cmd_retry(reg, value) < 0) { + TRACE("%s", error); + return -1; + } + + delay_ms(delay); + return 0; +} + +static int write_cmd_delay_us(uint8_t reg, uint8_t value, uint32_t delay, + const char *error) +{ + if (write_cmd_retry(reg, value) < 0) { + TRACE("%s", error); + return -1; + } + + delay_us(delay); + return 0; +} + +static int write_cmd_twice(uint8_t reg, uint8_t value, const char *first_error, + const char *second_error) +{ + if (write_cmd_delay_us(reg, value, CONFIG_DELAY_US, first_error) < 0) { + return -1; + } + + return write_cmd_delay_us(reg, value, CONFIG_DELAY_US, second_error); +} + +static int wait_for_device_ready(uint32_t timeout_ms) +{ + while (timeout_ms > 0) { + if (i2c_dev_ready(s_i2c_bus, s_i2c_addr) >= 0) { + return 0; + } + + delay_ms(10); + if (timeout_ms < 10) { + break; + } + timeout_ms -= 10; + } + + return -1; +} + +static int reset_device(const char *write_warning, const char *ready_error) +{ + if (write_cmd_retry(ICM42627_DEVICE_CONFIG_REG, ICM42627_RESET) < 0) { + TRACE("%s", write_warning); + } + + delay_ms(RESET_DELAY_MS); + + if (wait_for_device_ready(READY_TIMEOUT_MS) < 0) { + TRACE("%s", ready_error); + return -1; + } + + return 0; +} + +static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) +{ + s_i2c_bus = bus; + s_i2c_addr = addr; + s_trace_first_sample = true; + + TRACE("ICM42627 I2C Init at address 0x%x", addr); + + if (i2c_init(s_i2c_bus) < 0) { + TRACE("ICM42627 ERROR: i2c_init bus error"); + return -1; + } + + if (i2c_dev_ready(s_i2c_bus, s_i2c_addr) < 0) { + TRACE("ICM42627 device init error"); + return -1; + } + + uint8_t who_am_i = 0; + if (read_who_am_i(&who_am_i) < 0) { + TRACE("ICM42627 ERROR: WHO_AM_I read error"); + return -1; + } + + if (who_am_i != ICM42627_WHO_AM_I) { + TRACE("ICM42627 ERROR: unexpected WHO_AM_I 0x%02X", who_am_i); + return -1; + } + + if (reset_device("ICM42627 WARN: reset write failed", + "ICM42627 ERROR: device not ready after reset") < 0) { + return -1; + } + + if (reset_device("ICM42627 WARN: second reset write failed", + "ICM42627 ERROR: device not ready after second reset") < 0) { + return -1; + } + + if (write_cmd_delay_ms(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE1, + POWER_DELAY_MS, + "ICM42627 ERROR: PWR_MGMT0 stage1 failed") < 0) { + return -1; + } + + if (write_cmd_delay_ms(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE2, + POWER_DELAY_MS, + "ICM42627 ERROR: PWR_MGMT0 stage2 failed") < 0) { + return -1; + } + + if (write_cmd_twice(ICM42627_GYRO_CONFIG0_REG, ICM42627_GYRO_CONFIG, + "ICM42627 ERROR: GYRO_CONFIG0_REG error", + "ICM42627 ERROR: GYRO_CONFIG0_REG retry error") < 0) { + return -1; + } + + if (write_cmd_twice(ICM42627_ACCEL_CONFIG0_REG, ICM42627_ACCEL_CONFIG, + "ICM42627 ERROR: ACCEL_CONFIG0_REG error", + "ICM42627 ERROR: ACCEL_CONFIG0_REG retry error") < 0) { + return -1; + } + + if (write_cmd_delay_us(ICM42627_GYRO_CONFIG1_REG, ICM42627_GYRO_FILTER, + CONFIG_DELAY_US, + "ICM42627 ERROR: GYRO_CONFIG1_REG error") < 0) { + return -1; + } + + if (write_cmd_delay_us(ICM42627_ACCEL_GYRO_BW_REG, ICM42627_ACCEL_GYRO_BW, + CONFIG_DELAY_US, + "ICM42627 ERROR: ACCEL_GYRO_BW_REG error") < 0) { + return -1; + } + + if (write_cmd_delay_us(ICM42627_ACCEL_CONFIG1_REG, ICM42627_ACCEL_FILTER, + CONFIG_DELAY_US, + "ICM42627 ERROR: ACCEL_CONFIG1_REG error") < 0) { + return -1; + } + + if (select_bank(ICM42627_BANK1) < 0) { + TRACE("ICM42627 ERROR: bank1 select failed"); + return -1; + } + delay_us(CONFIG_DELAY_US); + + if (write_cmd_delay_us(ICM42627_GYRO_STATIC2_REG, ICM42627_GYRO_STATIC2, + CONFIG_DELAY_US, + "ICM42627 ERROR: GYRO_STATIC2_REG error") < 0) { + return -1; + } + + if (select_bank(ICM42627_BANK0) < 0) { + TRACE("ICM42627 ERROR: bank0 select failed"); + return -1; + } + delay_ms(VERIFY_DELAY_MS); + + if (read_who_am_i(&who_am_i) < 0 || + who_am_i != ICM42627_WHO_AM_I) { + TRACE("ICM42627 ERROR: WHO_AM_I verify failed (0x%02X)", who_am_i); + return -1; + } + + TRACE("ICM42627 succeeded"); + return 0; +} + +static int gyro42627Read(etx_imu_data_t *data) +{ + uint8_t buf[BURST_READ_LEN] = {0}; + + if (i2c_read(s_i2c_bus, s_i2c_addr, ICM42627_DATA_REG, 1, buf, sizeof(buf)) < 0) { + TRACE("ICM42627 ERROR: burst read failed"); + return -1; + } + + const int16_t accel_x = (int16_t)((buf[2] << 8) | buf[3]); + const int16_t accel_y = (int16_t)((buf[4] << 8) | buf[5]); + const int16_t accel_z = (int16_t)((buf[6] << 8) | buf[7]); + const int16_t gyro_x = (int16_t)((buf[8] << 8) | buf[9]); + const int16_t gyro_y = (int16_t)((buf[10] << 8) | buf[11]); + const int16_t gyro_z = (int16_t)((buf[12] << 8) | buf[13]); + + data->accel_x = accel_y; + data->accel_y = accel_x; + data->accel_z = -accel_z; + data->gyro_x = gyro_y; + data->gyro_y = gyro_x; + data->gyro_z = -gyro_z; + + if (s_trace_first_sample) { + s_trace_first_sample = false; + TRACE("ICM42627 first sample: G=(%d,%d,%d) A=(%d,%d,%d)", + (int)data->gyro_x, (int)data->gyro_y, (int)data->gyro_z, + (int)data->accel_x, (int)data->accel_y, (int)data->accel_z); + } + + return 0; +} + +const etx_imu_driver_t imu_icm42627_driver = { + gyro42627Init, + gyro42627Read, + "ICM42627", +}; \ No newline at end of file diff --git a/radio/src/drivers/icm42627.h b/radio/src/drivers/icm42627.h new file mode 100644 index 00000000000..e59346133c9 --- /dev/null +++ b/radio/src/drivers/icm42627.h @@ -0,0 +1,54 @@ +/* + * Copyright (C) EdgeTX + * + * Based on code named + * opentx - https://github.com/opentx/opentx + * th9x - http://code.google.com/p/th9x + * er9x - http://code.google.com/p/er9x + * gruvin9x - http://code.google.com/p/gruvin9x + * + * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#pragma once + +#include "hal/imu.h" + +#define ICM42627_WHO_AM_I_REG 0x75 +#define ICM42627_WHO_AM_I 0x20 + +#define ICM42627_DEVICE_CONFIG_REG 0x11 +#define ICM42627_PWR_MGMT0_REG 0x4E +#define ICM42627_GYRO_CONFIG0_REG 0x4F +#define ICM42627_ACCEL_CONFIG0_REG 0x50 +#define ICM42627_GYRO_CONFIG1_REG 0x51 +#define ICM42627_ACCEL_GYRO_BW_REG 0x52 +#define ICM42627_ACCEL_CONFIG1_REG 0x53 +#define ICM42627_BANK_SEL_REG 0x76 + +#define ICM42627_GYRO_STATIC2_REG 0x0B +#define ICM42627_DATA_REG 0x1D + +#define ICM42627_BANK0 0x00 +#define ICM42627_BANK1 0x01 + +#define ICM42627_RESET 0x01 +#define ICM42627_PWR_STAGE1 0x0F +#define ICM42627_PWR_STAGE2 0x2F +#define ICM42627_GYRO_CONFIG 0x07 +#define ICM42627_ACCEL_CONFIG 0x47 +#define ICM42627_GYRO_FILTER 0x0A +#define ICM42627_ACCEL_GYRO_BW 0x22 +#define ICM42627_ACCEL_FILTER 0x14 +#define ICM42627_GYRO_STATIC2 0x00 + +extern const etx_imu_driver_t imu_icm42627_driver; \ No newline at end of file diff --git a/radio/src/targets/horus/CMakeLists.txt b/radio/src/targets/horus/CMakeLists.txt index a86a1744d3e..7d823811b73 100644 --- a/radio/src/targets/horus/CMakeLists.txt +++ b/radio/src/targets/horus/CMakeLists.txt @@ -147,7 +147,7 @@ elseif (PCB STREQUAL X12S) add_definitions(-DMANUFACTURER_FRSKY) endif() -# enable GPS support if there is AUX port available unless otherwise disabled +# enable GPS support if there is AUX port available unless otherwise disabled if(AUX_SERIAL) option(INTERNAL_GPS "Support for internal GPS" ON) set(INTERNAL_GPS_BAUDRATE "9600" CACHE STRING "Baud rate for internal GPS") @@ -284,6 +284,7 @@ add_library(board OBJECT EXCLUDE_FROM_ALL targets/common/arm/stm32/delays_driver.cpp targets/common/arm/stm32/heartbeat_driver.cpp drivers/lsm6ds.cpp + drivers/icm42627.cpp targets/common/arm/stm32/mixer_scheduler_driver.cpp targets/common/arm/stm32/module_timer_driver.cpp targets/common/arm/stm32/sticks_pwm_driver.cpp diff --git a/radio/src/targets/horus/board.cpp b/radio/src/targets/horus/board.cpp index c5dbcc4dfee..04308fd289e 100644 --- a/radio/src/targets/horus/board.cpp +++ b/radio/src/targets/horus/board.cpp @@ -133,14 +133,23 @@ void audioInit() #endif #if defined(HAS_IMU) -#include "drivers/lsm6ds.h" +#include "gyro.h" #include "stm32_i2c_driver.h" +#include "drivers/icm42627.h" +#include "drivers/lsm6ds.h" static void gyroInit() { +#if defined(RADIO_V16) + const etx_imu_driver_t *driver = &imu_icm42627_driver; +#else + const etx_imu_driver_t *driver = &imu_lsm6ds_driver; +#endif + const etx_imu_t candidates[] = { - { &imu_lsm6ds_driver, IMU_I2C_BUS, IMU_I2C_ADDRESS }, + { driver, IMU_I2C_BUS, IMU_I2C_ADDRESS }, }; + gyroStart(imuDetect(candidates, DIM(candidates))); } #endif diff --git a/radio/src/targets/horus/hal.h b/radio/src/targets/horus/hal.h index 94ab9696efd..4ba08f6f501 100644 --- a/radio/src/targets/horus/hal.h +++ b/radio/src/targets/horus/hal.h @@ -90,7 +90,7 @@ #define DEFAULT_6POS_CALIB {3, 12, 21, 30, 38} #define DEFAULT_6POS_IDX 5 #endif - + // Power #if defined(RADIO_T18) #define PWR_ON_GPIO GPIO_PIN(GPIOJ, 1) // PJ.01 @@ -355,7 +355,7 @@ #define STORAGE_USE_SDIO // Use SD card for storage with SDIO driver //#define STORAGE_USE_SPI_FLASH // Use SPI flash for storage instead of SD card -// SPI NOR Flash +// SPI NOR Flash #if defined(PCBX12S) && PCBREV >= 13 #define FLASH_SPI SPI1 #define FLASH_SPI_CS_GPIO GPIO_PIN(GPIOA, 15) // PA.15 @@ -374,7 +374,7 @@ // #define FLASH_SPI_TX_DMA_IRQHandler DMA2_Stream3_IRQHandler // #define FLASH_SPI_TX_DMA_FLAG_TC DMA_IT_TCIF3 // #define FLASH_SPI_TX_DMA_STATUS_REG HISR - // SPI1_RX: DMA2 Stream 0 / Stream 2 + // SPI1_RX: DMA2 Stream 0 / Stream 2 // #define FLASH_SPI_RX_DMA_CHANNEL DMA_Channel_3 // #define FLASH_SPI_RX_DMA_STREAM DMA2_Stream5 // #define FLASH_SPI_RX_DMA_IRQn DMA2_Stream5_IRQn @@ -491,6 +491,9 @@ #if defined(PCBX12S) #define IMU_I2C_BUS I2C_Bus_1 #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 From e523f5dcad21f3f2a8d20f96deef88c7a86bcfeb Mon Sep 17 00:00:00 2001 From: gismo2004 Date: Sun, 29 Mar 2026 15:52:19 +0200 Subject: [PATCH 2/8] try a verified write --- radio/src/drivers/icm42627.cpp | 83 ++++++++++++++++++---------------- 1 file changed, 44 insertions(+), 39 deletions(-) diff --git a/radio/src/drivers/icm42627.cpp b/radio/src/drivers/icm42627.cpp index 5eae3c1106b..7df9319abef 100644 --- a/radio/src/drivers/icm42627.cpp +++ b/radio/src/drivers/icm42627.cpp @@ -67,38 +67,43 @@ static int select_bank(uint8_t bank) return write_cmd_retry(ICM42627_BANK_SEL_REG, bank); } -static int write_cmd_delay_ms(uint8_t reg, uint8_t value, uint32_t delay, - const char *error) +static int write_checked(uint8_t reg, uint8_t value, const char *error) { if (write_cmd_retry(reg, value) < 0) { TRACE("%s", error); return -1; } - delay_ms(delay); return 0; } -static int write_cmd_delay_us(uint8_t reg, uint8_t value, uint32_t delay, - const char *error) +static int write_verified(uint8_t reg, uint8_t value, const char *write_error, + const char *verify_error) { - if (write_cmd_retry(reg, value) < 0) { - TRACE("%s", error); + uint8_t readback = 0; + + if (write_checked(reg, value, write_error) < 0) { return -1; } - delay_us(delay); - return 0; -} + delay_us(CONFIG_DELAY_US); -static int write_cmd_twice(uint8_t reg, uint8_t value, const char *first_error, - const char *second_error) -{ - if (write_cmd_delay_us(reg, value, CONFIG_DELAY_US, first_error) < 0) { + if (read_cmd(reg, &readback) >= 0 && readback == value) { + return 0; + } + + if (write_checked(reg, value, write_error) < 0) { return -1; } - return write_cmd_delay_us(reg, value, CONFIG_DELAY_US, second_error); + delay_us(CONFIG_DELAY_US); + + if (read_cmd(reg, &readback) < 0 || readback != value) { + TRACE("%s", verify_error); + return -1; + } + + return 0; } static int wait_for_device_ready(uint32_t timeout_ms) @@ -173,47 +178,47 @@ static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) return -1; } - if (write_cmd_delay_ms(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE1, - POWER_DELAY_MS, - "ICM42627 ERROR: PWR_MGMT0 stage1 failed") < 0) { + if (write_checked(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE1, + "ICM42627 ERROR: PWR_MGMT0 stage1 failed") < 0) { return -1; } + delay_ms(POWER_DELAY_MS); - if (write_cmd_delay_ms(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE2, - POWER_DELAY_MS, - "ICM42627 ERROR: PWR_MGMT0 stage2 failed") < 0) { + if (write_checked(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE2, + "ICM42627 ERROR: PWR_MGMT0 stage2 failed") < 0) { return -1; } + delay_ms(POWER_DELAY_MS); - if (write_cmd_twice(ICM42627_GYRO_CONFIG0_REG, ICM42627_GYRO_CONFIG, - "ICM42627 ERROR: GYRO_CONFIG0_REG error", - "ICM42627 ERROR: GYRO_CONFIG0_REG retry error") < 0) { + if (write_verified(ICM42627_GYRO_CONFIG0_REG, ICM42627_GYRO_CONFIG, + "ICM42627 ERROR: GYRO_CONFIG0_REG write failed", + "ICM42627 ERROR: GYRO_CONFIG0_REG verify failed") < 0) { return -1; } - if (write_cmd_twice(ICM42627_ACCEL_CONFIG0_REG, ICM42627_ACCEL_CONFIG, - "ICM42627 ERROR: ACCEL_CONFIG0_REG error", - "ICM42627 ERROR: ACCEL_CONFIG0_REG retry error") < 0) { + if (write_verified(ICM42627_ACCEL_CONFIG0_REG, ICM42627_ACCEL_CONFIG, + "ICM42627 ERROR: ACCEL_CONFIG0_REG write failed", + "ICM42627 ERROR: ACCEL_CONFIG0_REG verify failed") < 0) { return -1; } - if (write_cmd_delay_us(ICM42627_GYRO_CONFIG1_REG, ICM42627_GYRO_FILTER, - CONFIG_DELAY_US, - "ICM42627 ERROR: GYRO_CONFIG1_REG error") < 0) { + if (write_checked(ICM42627_GYRO_CONFIG1_REG, ICM42627_GYRO_FILTER, + "ICM42627 ERROR: GYRO_CONFIG1_REG error") < 0) { return -1; } + delay_us(CONFIG_DELAY_US); - if (write_cmd_delay_us(ICM42627_ACCEL_GYRO_BW_REG, ICM42627_ACCEL_GYRO_BW, - CONFIG_DELAY_US, - "ICM42627 ERROR: ACCEL_GYRO_BW_REG error") < 0) { + if (write_checked(ICM42627_ACCEL_GYRO_BW_REG, ICM42627_ACCEL_GYRO_BW, + "ICM42627 ERROR: ACCEL_GYRO_BW_REG error") < 0) { return -1; } + delay_us(CONFIG_DELAY_US); - if (write_cmd_delay_us(ICM42627_ACCEL_CONFIG1_REG, ICM42627_ACCEL_FILTER, - CONFIG_DELAY_US, - "ICM42627 ERROR: ACCEL_CONFIG1_REG error") < 0) { + if (write_checked(ICM42627_ACCEL_CONFIG1_REG, ICM42627_ACCEL_FILTER, + "ICM42627 ERROR: ACCEL_CONFIG1_REG error") < 0) { return -1; } + delay_us(CONFIG_DELAY_US); if (select_bank(ICM42627_BANK1) < 0) { TRACE("ICM42627 ERROR: bank1 select failed"); @@ -221,11 +226,11 @@ static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) } delay_us(CONFIG_DELAY_US); - if (write_cmd_delay_us(ICM42627_GYRO_STATIC2_REG, ICM42627_GYRO_STATIC2, - CONFIG_DELAY_US, - "ICM42627 ERROR: GYRO_STATIC2_REG error") < 0) { + if (write_checked(ICM42627_GYRO_STATIC2_REG, ICM42627_GYRO_STATIC2, + "ICM42627 ERROR: GYRO_STATIC2_REG error") < 0) { return -1; } + delay_us(CONFIG_DELAY_US); if (select_bank(ICM42627_BANK0) < 0) { TRACE("ICM42627 ERROR: bank0 select failed"); From f1dbab83a3d585e7f8c82dc48658ddf3ac311b3c Mon Sep 17 00:00:00 2001 From: gismo2004 Date: Sun, 29 Mar 2026 15:54:57 +0200 Subject: [PATCH 3/8] new line --- radio/src/drivers/icm42627.cpp | 2 +- radio/src/drivers/icm42627.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/radio/src/drivers/icm42627.cpp b/radio/src/drivers/icm42627.cpp index 7df9319abef..d9ae2e90eb9 100644 --- a/radio/src/drivers/icm42627.cpp +++ b/radio/src/drivers/icm42627.cpp @@ -285,4 +285,4 @@ const etx_imu_driver_t imu_icm42627_driver = { gyro42627Init, gyro42627Read, "ICM42627", -}; \ No newline at end of file +}; diff --git a/radio/src/drivers/icm42627.h b/radio/src/drivers/icm42627.h index e59346133c9..70c9395a6aa 100644 --- a/radio/src/drivers/icm42627.h +++ b/radio/src/drivers/icm42627.h @@ -51,4 +51,4 @@ #define ICM42627_ACCEL_FILTER 0x14 #define ICM42627_GYRO_STATIC2 0x00 -extern const etx_imu_driver_t imu_icm42627_driver; \ No newline at end of file +extern const etx_imu_driver_t imu_icm42627_driver; From 6674bd62cadd1d834553c415ee8105c85e0781c5 Mon Sep 17 00:00:00 2001 From: gismo2004 Date: Sun, 29 Mar 2026 16:02:43 +0200 Subject: [PATCH 4/8] remove double call --- radio/src/drivers/icm42627.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/radio/src/drivers/icm42627.cpp b/radio/src/drivers/icm42627.cpp index d9ae2e90eb9..92bd954e72f 100644 --- a/radio/src/drivers/icm42627.cpp +++ b/radio/src/drivers/icm42627.cpp @@ -173,11 +173,6 @@ static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) return -1; } - if (reset_device("ICM42627 WARN: second reset write failed", - "ICM42627 ERROR: device not ready after second reset") < 0) { - return -1; - } - if (write_checked(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE1, "ICM42627 ERROR: PWR_MGMT0 stage1 failed") < 0) { return -1; From a9d7408d7ebc7adf53d6b27fc223d843955c9cd7 Mon Sep 17 00:00:00 2001 From: gismo2004 Date: Sun, 29 Mar 2026 16:26:17 +0200 Subject: [PATCH 5/8] remove double calls --- radio/src/drivers/icm42627.cpp | 65 ++++++---------------------------- 1 file changed, 11 insertions(+), 54 deletions(-) diff --git a/radio/src/drivers/icm42627.cpp b/radio/src/drivers/icm42627.cpp index 92bd954e72f..0b4ebbdf94c 100644 --- a/radio/src/drivers/icm42627.cpp +++ b/radio/src/drivers/icm42627.cpp @@ -29,7 +29,6 @@ static etx_i2c_bus_t s_i2c_bus; static uint16_t s_i2c_addr; -static bool s_trace_first_sample; static constexpr uint32_t RESET_DELAY_MS = 150; static constexpr uint32_t READY_TIMEOUT_MS = 200; @@ -53,23 +52,9 @@ static int read_who_am_i(uint8_t *value) return read_cmd(ICM42627_WHO_AM_I_REG, value); } -static int write_cmd_retry(uint8_t reg, uint8_t value) -{ - if (write_cmd(reg, value) >= 0) { - return 0; - } - - return write_cmd(reg, value); -} - -static int select_bank(uint8_t bank) -{ - return write_cmd_retry(ICM42627_BANK_SEL_REG, bank); -} - static int write_checked(uint8_t reg, uint8_t value, const char *error) { - if (write_cmd_retry(reg, value) < 0) { + if (write_cmd(reg, value) < 0) { TRACE("%s", error); return -1; } @@ -88,16 +73,6 @@ static int write_verified(uint8_t reg, uint8_t value, const char *write_error, delay_us(CONFIG_DELAY_US); - if (read_cmd(reg, &readback) >= 0 && readback == value) { - return 0; - } - - if (write_checked(reg, value, write_error) < 0) { - return -1; - } - - delay_us(CONFIG_DELAY_US); - if (read_cmd(reg, &readback) < 0 || readback != value) { TRACE("%s", verify_error); return -1; @@ -123,27 +98,10 @@ static int wait_for_device_ready(uint32_t timeout_ms) return -1; } -static int reset_device(const char *write_warning, const char *ready_error) -{ - if (write_cmd_retry(ICM42627_DEVICE_CONFIG_REG, ICM42627_RESET) < 0) { - TRACE("%s", write_warning); - } - - delay_ms(RESET_DELAY_MS); - - if (wait_for_device_ready(READY_TIMEOUT_MS) < 0) { - TRACE("%s", ready_error); - return -1; - } - - return 0; -} - static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) { s_i2c_bus = bus; s_i2c_addr = addr; - s_trace_first_sample = true; TRACE("ICM42627 I2C Init at address 0x%x", addr); @@ -168,8 +126,14 @@ static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) return -1; } - if (reset_device("ICM42627 WARN: reset write failed", - "ICM42627 ERROR: device not ready after reset") < 0) { + if (write_cmd(ICM42627_DEVICE_CONFIG_REG, ICM42627_RESET) < 0) { + TRACE("ICM42627 WARN: reset write failed"); + } + + delay_ms(RESET_DELAY_MS); + + if (wait_for_device_ready(READY_TIMEOUT_MS) < 0) { + TRACE("ICM42627 ERROR: device not ready after reset"); return -1; } @@ -215,7 +179,7 @@ static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) } delay_us(CONFIG_DELAY_US); - if (select_bank(ICM42627_BANK1) < 0) { + if (write_cmd(ICM42627_BANK_SEL_REG, ICM42627_BANK1) < 0) { TRACE("ICM42627 ERROR: bank1 select failed"); return -1; } @@ -227,7 +191,7 @@ static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) } delay_us(CONFIG_DELAY_US); - if (select_bank(ICM42627_BANK0) < 0) { + if (write_cmd(ICM42627_BANK_SEL_REG, ICM42627_BANK0) < 0) { TRACE("ICM42627 ERROR: bank0 select failed"); return -1; } @@ -266,13 +230,6 @@ static int gyro42627Read(etx_imu_data_t *data) data->gyro_y = gyro_x; data->gyro_z = -gyro_z; - if (s_trace_first_sample) { - s_trace_first_sample = false; - TRACE("ICM42627 first sample: G=(%d,%d,%d) A=(%d,%d,%d)", - (int)data->gyro_x, (int)data->gyro_y, (int)data->gyro_z, - (int)data->accel_x, (int)data->accel_y, (int)data->accel_z); - } - return 0; } From 4cb6f7fdcc8dee627fa124c15f7fd695554308d6 Mon Sep 17 00:00:00 2001 From: gismo2004 Date: Sun, 29 Mar 2026 16:33:37 +0200 Subject: [PATCH 6/8] clean log output --- radio/src/drivers/icm42627.cpp | 56 ++++++++++++++++------------------ 1 file changed, 27 insertions(+), 29 deletions(-) diff --git a/radio/src/drivers/icm42627.cpp b/radio/src/drivers/icm42627.cpp index 0b4ebbdf94c..cd72178ddeb 100644 --- a/radio/src/drivers/icm42627.cpp +++ b/radio/src/drivers/icm42627.cpp @@ -47,11 +47,6 @@ static int read_cmd(uint8_t reg, uint8_t *value) return i2c_read(s_i2c_bus, s_i2c_addr, reg, 1, value, 1); } -static int read_who_am_i(uint8_t *value) -{ - return read_cmd(ICM42627_WHO_AM_I_REG, value); -} - static int write_checked(uint8_t reg, uint8_t value, const char *error) { if (write_cmd(reg, value) < 0) { @@ -103,107 +98,110 @@ static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) s_i2c_bus = bus; s_i2c_addr = addr; - TRACE("ICM42627 I2C Init at address 0x%x", addr); + TRACE("ICM42627: probing I2C address 0x%02X", addr); if (i2c_init(s_i2c_bus) < 0) { - TRACE("ICM42627 ERROR: i2c_init bus error"); + TRACE("ICM42627: i2c_init failed"); return -1; } if (i2c_dev_ready(s_i2c_bus, s_i2c_addr) < 0) { - TRACE("ICM42627 device init error"); + TRACE("ICM42627: device did not acknowledge on I2C"); return -1; } uint8_t who_am_i = 0; - if (read_who_am_i(&who_am_i) < 0) { - TRACE("ICM42627 ERROR: WHO_AM_I read error"); + if (read_cmd(ICM42627_WHO_AM_I_REG, &who_am_i) < 0) { + TRACE("ICM42627: failed to read WHO_AM_I"); return -1; } if (who_am_i != ICM42627_WHO_AM_I) { - TRACE("ICM42627 ERROR: unexpected WHO_AM_I 0x%02X", who_am_i); + TRACE("ICM42627: unexpected WHO_AM_I 0x%02X, expected 0x%02X", who_am_i, + ICM42627_WHO_AM_I); return -1; } if (write_cmd(ICM42627_DEVICE_CONFIG_REG, ICM42627_RESET) < 0) { - TRACE("ICM42627 WARN: reset write failed"); + TRACE("ICM42627: failed to write reset command"); } delay_ms(RESET_DELAY_MS); if (wait_for_device_ready(READY_TIMEOUT_MS) < 0) { - TRACE("ICM42627 ERROR: device not ready after reset"); + TRACE("ICM42627: device did not become ready after reset"); return -1; } if (write_checked(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE1, - "ICM42627 ERROR: PWR_MGMT0 stage1 failed") < 0) { + "ICM42627: failed to write PWR_MGMT0 stage 1") < 0) { return -1; } delay_ms(POWER_DELAY_MS); if (write_checked(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE2, - "ICM42627 ERROR: PWR_MGMT0 stage2 failed") < 0) { + "ICM42627: failed to write PWR_MGMT0 stage 2") < 0) { return -1; } delay_ms(POWER_DELAY_MS); if (write_verified(ICM42627_GYRO_CONFIG0_REG, ICM42627_GYRO_CONFIG, - "ICM42627 ERROR: GYRO_CONFIG0_REG write failed", - "ICM42627 ERROR: GYRO_CONFIG0_REG verify failed") < 0) { + "ICM42627: failed to write GYRO_CONFIG0", + "ICM42627: GYRO_CONFIG0 readback mismatch") < 0) { return -1; } if (write_verified(ICM42627_ACCEL_CONFIG0_REG, ICM42627_ACCEL_CONFIG, - "ICM42627 ERROR: ACCEL_CONFIG0_REG write failed", - "ICM42627 ERROR: ACCEL_CONFIG0_REG verify failed") < 0) { + "ICM42627: failed to write ACCEL_CONFIG0", + "ICM42627: ACCEL_CONFIG0 readback mismatch") < 0) { return -1; } if (write_checked(ICM42627_GYRO_CONFIG1_REG, ICM42627_GYRO_FILTER, - "ICM42627 ERROR: GYRO_CONFIG1_REG error") < 0) { + "ICM42627: failed to write GYRO_CONFIG1") < 0) { return -1; } delay_us(CONFIG_DELAY_US); if (write_checked(ICM42627_ACCEL_GYRO_BW_REG, ICM42627_ACCEL_GYRO_BW, - "ICM42627 ERROR: ACCEL_GYRO_BW_REG error") < 0) { + "ICM42627: failed to write ACCEL_GYRO_BW") < 0) { return -1; } delay_us(CONFIG_DELAY_US); if (write_checked(ICM42627_ACCEL_CONFIG1_REG, ICM42627_ACCEL_FILTER, - "ICM42627 ERROR: ACCEL_CONFIG1_REG error") < 0) { + "ICM42627: failed to write ACCEL_CONFIG1") < 0) { return -1; } delay_us(CONFIG_DELAY_US); if (write_cmd(ICM42627_BANK_SEL_REG, ICM42627_BANK1) < 0) { - TRACE("ICM42627 ERROR: bank1 select failed"); + TRACE("ICM42627: failed to select register bank 1"); return -1; } delay_us(CONFIG_DELAY_US); if (write_checked(ICM42627_GYRO_STATIC2_REG, ICM42627_GYRO_STATIC2, - "ICM42627 ERROR: GYRO_STATIC2_REG error") < 0) { + "ICM42627: failed to write GYRO_STATIC2") < 0) { return -1; } delay_us(CONFIG_DELAY_US); if (write_cmd(ICM42627_BANK_SEL_REG, ICM42627_BANK0) < 0) { - TRACE("ICM42627 ERROR: bank0 select failed"); + TRACE("ICM42627: failed to restore register bank 0"); return -1; } delay_ms(VERIFY_DELAY_MS); - if (read_who_am_i(&who_am_i) < 0 || + // Re-read WHO_AM_I after reset and banked configuration to catch a device + // that stopped responding even though the initial probe succeeded. + if (read_cmd(ICM42627_WHO_AM_I_REG, &who_am_i) < 0 || who_am_i != ICM42627_WHO_AM_I) { - TRACE("ICM42627 ERROR: WHO_AM_I verify failed (0x%02X)", who_am_i); + TRACE("ICM42627: final WHO_AM_I check failed (0x%02X)", who_am_i); return -1; } - TRACE("ICM42627 succeeded"); + TRACE("ICM42627: init complete"); return 0; } @@ -212,7 +210,7 @@ static int gyro42627Read(etx_imu_data_t *data) uint8_t buf[BURST_READ_LEN] = {0}; if (i2c_read(s_i2c_bus, s_i2c_addr, ICM42627_DATA_REG, 1, buf, sizeof(buf)) < 0) { - TRACE("ICM42627 ERROR: burst read failed"); + TRACE("ICM42627: failed to read sensor sample block"); return -1; } From 8d47b345af15504f88113dfbc72fcb938c992ba2 Mon Sep 17 00:00:00 2001 From: gismo2004 Date: Sun, 29 Mar 2026 16:58:36 +0200 Subject: [PATCH 7/8] remove all the double checks. icm42607C does also not use them --- radio/src/drivers/icm42627.cpp | 71 ++++++++++++---------------------- 1 file changed, 24 insertions(+), 47 deletions(-) diff --git a/radio/src/drivers/icm42627.cpp b/radio/src/drivers/icm42627.cpp index cd72178ddeb..d6be4cd17b6 100644 --- a/radio/src/drivers/icm42627.cpp +++ b/radio/src/drivers/icm42627.cpp @@ -47,35 +47,6 @@ static int read_cmd(uint8_t reg, uint8_t *value) return i2c_read(s_i2c_bus, s_i2c_addr, reg, 1, value, 1); } -static int write_checked(uint8_t reg, uint8_t value, const char *error) -{ - if (write_cmd(reg, value) < 0) { - TRACE("%s", error); - return -1; - } - - return 0; -} - -static int write_verified(uint8_t reg, uint8_t value, const char *write_error, - const char *verify_error) -{ - uint8_t readback = 0; - - if (write_checked(reg, value, write_error) < 0) { - return -1; - } - - delay_us(CONFIG_DELAY_US); - - if (read_cmd(reg, &readback) < 0 || readback != value) { - TRACE("%s", verify_error); - return -1; - } - - return 0; -} - static int wait_for_device_ready(uint32_t timeout_ms) { while (timeout_ms > 0) { @@ -133,44 +104,50 @@ static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) return -1; } - if (write_checked(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE1, - "ICM42627: failed to write PWR_MGMT0 stage 1") < 0) { + if (write_cmd(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE1) < 0) { + TRACE("ICM42627: failed to write PWR_MGMT0 stage 1"); return -1; } delay_ms(POWER_DELAY_MS); - if (write_checked(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE2, - "ICM42627: failed to write PWR_MGMT0 stage 2") < 0) { + if (write_cmd(ICM42627_PWR_MGMT0_REG, ICM42627_PWR_STAGE2) < 0) { + TRACE("ICM42627: failed to write PWR_MGMT0 stage 2"); return -1; } delay_ms(POWER_DELAY_MS); - if (write_verified(ICM42627_GYRO_CONFIG0_REG, ICM42627_GYRO_CONFIG, - "ICM42627: failed to write GYRO_CONFIG0", - "ICM42627: GYRO_CONFIG0 readback mismatch") < 0) { + if (write_cmd(ICM42627_GYRO_CONFIG0_REG, ICM42627_GYRO_CONFIG) < 0) { + TRACE("ICM42627: failed to write GYRO_CONFIG0"); + return -1; + } + delay_us(CONFIG_DELAY_US); + + if (write_cmd(ICM42627_ACCEL_CONFIG0_REG, ICM42627_ACCEL_CONFIG) < 0) { + TRACE("ICM42627: failed to write ACCEL_CONFIG0"); return -1; } + delay_us(CONFIG_DELAY_US); - if (write_verified(ICM42627_ACCEL_CONFIG0_REG, ICM42627_ACCEL_CONFIG, - "ICM42627: failed to write ACCEL_CONFIG0", - "ICM42627: ACCEL_CONFIG0 readback mismatch") < 0) { + if (read_cmd(ICM42627_ACCEL_CONFIG0_REG, &who_am_i) < 0 || + who_am_i != ICM42627_ACCEL_CONFIG) { + TRACE("ICM42627: ACCEL_CONFIG0 readback mismatch"); return -1; } - if (write_checked(ICM42627_GYRO_CONFIG1_REG, ICM42627_GYRO_FILTER, - "ICM42627: failed to write GYRO_CONFIG1") < 0) { + if (write_cmd(ICM42627_GYRO_CONFIG1_REG, ICM42627_GYRO_FILTER) < 0) { + TRACE("ICM42627: failed to write GYRO_CONFIG1"); return -1; } delay_us(CONFIG_DELAY_US); - if (write_checked(ICM42627_ACCEL_GYRO_BW_REG, ICM42627_ACCEL_GYRO_BW, - "ICM42627: failed to write ACCEL_GYRO_BW") < 0) { + if (write_cmd(ICM42627_ACCEL_GYRO_BW_REG, ICM42627_ACCEL_GYRO_BW) < 0) { + TRACE("ICM42627: failed to write ACCEL_GYRO_BW"); return -1; } delay_us(CONFIG_DELAY_US); - if (write_checked(ICM42627_ACCEL_CONFIG1_REG, ICM42627_ACCEL_FILTER, - "ICM42627: failed to write ACCEL_CONFIG1") < 0) { + if (write_cmd(ICM42627_ACCEL_CONFIG1_REG, ICM42627_ACCEL_FILTER) < 0) { + TRACE("ICM42627: failed to write ACCEL_CONFIG1"); return -1; } delay_us(CONFIG_DELAY_US); @@ -181,8 +158,8 @@ static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) } delay_us(CONFIG_DELAY_US); - if (write_checked(ICM42627_GYRO_STATIC2_REG, ICM42627_GYRO_STATIC2, - "ICM42627: failed to write GYRO_STATIC2") < 0) { + if (write_cmd(ICM42627_GYRO_STATIC2_REG, ICM42627_GYRO_STATIC2) < 0) { + TRACE("ICM42627: failed to write GYRO_STATIC2"); return -1; } delay_us(CONFIG_DELAY_US); From 5abaeab478e91fd59a0b7b77e9e46cd594b942be Mon Sep 17 00:00:00 2001 From: gismo2004 Date: Sun, 29 Mar 2026 17:09:51 +0200 Subject: [PATCH 8/8] remove delay_us --- radio/src/drivers/icm42627.cpp | 25 +++++++++---------------- 1 file changed, 9 insertions(+), 16 deletions(-) diff --git a/radio/src/drivers/icm42627.cpp b/radio/src/drivers/icm42627.cpp index d6be4cd17b6..ad796fd8a82 100644 --- a/radio/src/drivers/icm42627.cpp +++ b/radio/src/drivers/icm42627.cpp @@ -33,8 +33,7 @@ static uint16_t s_i2c_addr; static constexpr uint32_t RESET_DELAY_MS = 150; static constexpr uint32_t READY_TIMEOUT_MS = 200; static constexpr uint32_t POWER_DELAY_MS = 20; -static constexpr uint32_t VERIFY_DELAY_MS = 10; -static constexpr uint32_t CONFIG_DELAY_US = 100; +static constexpr uint32_t CONFIG_DELAY_MS = 1; static constexpr uint8_t BURST_READ_LEN = 14; static int write_cmd(uint8_t reg, uint8_t value) @@ -120,55 +119,49 @@ static int gyro42627Init(etx_i2c_bus_t bus, uint16_t addr) TRACE("ICM42627: failed to write GYRO_CONFIG0"); return -1; } - delay_us(CONFIG_DELAY_US); + delay_ms(CONFIG_DELAY_MS); if (write_cmd(ICM42627_ACCEL_CONFIG0_REG, ICM42627_ACCEL_CONFIG) < 0) { TRACE("ICM42627: failed to write ACCEL_CONFIG0"); return -1; } - delay_us(CONFIG_DELAY_US); - - if (read_cmd(ICM42627_ACCEL_CONFIG0_REG, &who_am_i) < 0 || - who_am_i != ICM42627_ACCEL_CONFIG) { - TRACE("ICM42627: ACCEL_CONFIG0 readback mismatch"); - return -1; - } + delay_ms(CONFIG_DELAY_MS); if (write_cmd(ICM42627_GYRO_CONFIG1_REG, ICM42627_GYRO_FILTER) < 0) { TRACE("ICM42627: failed to write GYRO_CONFIG1"); return -1; } - delay_us(CONFIG_DELAY_US); + delay_ms(CONFIG_DELAY_MS); if (write_cmd(ICM42627_ACCEL_GYRO_BW_REG, ICM42627_ACCEL_GYRO_BW) < 0) { TRACE("ICM42627: failed to write ACCEL_GYRO_BW"); return -1; } - delay_us(CONFIG_DELAY_US); + delay_ms(CONFIG_DELAY_MS); if (write_cmd(ICM42627_ACCEL_CONFIG1_REG, ICM42627_ACCEL_FILTER) < 0) { TRACE("ICM42627: failed to write ACCEL_CONFIG1"); return -1; } - delay_us(CONFIG_DELAY_US); + delay_ms(CONFIG_DELAY_MS); if (write_cmd(ICM42627_BANK_SEL_REG, ICM42627_BANK1) < 0) { TRACE("ICM42627: failed to select register bank 1"); return -1; } - delay_us(CONFIG_DELAY_US); + delay_ms(CONFIG_DELAY_MS); if (write_cmd(ICM42627_GYRO_STATIC2_REG, ICM42627_GYRO_STATIC2) < 0) { TRACE("ICM42627: failed to write GYRO_STATIC2"); return -1; } - delay_us(CONFIG_DELAY_US); + delay_ms(CONFIG_DELAY_MS); if (write_cmd(ICM42627_BANK_SEL_REG, ICM42627_BANK0) < 0) { TRACE("ICM42627: failed to restore register bank 0"); return -1; } - delay_ms(VERIFY_DELAY_MS); + delay_ms(CONFIG_DELAY_MS); // Re-read WHO_AM_I after reset and banked configuration to catch a device // that stopped responding even though the initial probe succeeded.