feat(v16): add gyro support for HelloRadio V16 (#7232)

Author: gismo2004

radio/src/drivers/icm42627.cpp

@@ -0,0 +1,208 @@
+/*
+ * 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 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 CONFIG_DELAY_MS = 1;
+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 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 gyro42627Init(etx_i2c_bus_t bus, uint16_t addr)
+{
+  s_i2c_bus = bus;
+  s_i2c_addr = addr;
+
+  TRACE("ICM42627: probing I2C address 0x%02X", addr);
+
+  if (i2c_init(s_i2c_bus) < 0) {
+    TRACE("ICM42627: i2c_init failed");
+    return -1;
+  }
+
+  if (i2c_dev_ready(s_i2c_bus, s_i2c_addr) < 0) {
+    TRACE("ICM42627: device did not acknowledge on I2C");
+    return -1;
+  }
+
+  uint8_t who_am_i = 0;
+  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: 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: failed to write reset command");
+  }
+
+  delay_ms(RESET_DELAY_MS);
+
+  if (wait_for_device_ready(READY_TIMEOUT_MS) < 0) {
+    TRACE("ICM42627: device did not become ready after reset");
+    return -1;
+  }
+
+  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_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_cmd(ICM42627_GYRO_CONFIG0_REG, ICM42627_GYRO_CONFIG) < 0) {
+    TRACE("ICM42627: failed to write GYRO_CONFIG0");
+    return -1;
+  }
+  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_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_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_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_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_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_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(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.
+  if (read_cmd(ICM42627_WHO_AM_I_REG, &who_am_i) < 0 ||
+      who_am_i != ICM42627_WHO_AM_I) {
+    TRACE("ICM42627: final WHO_AM_I check failed (0x%02X)", who_am_i);
+    return -1;
+  }
+
+  TRACE("ICM42627: init complete");
+  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: failed to read sensor sample block");
+    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;
+
+  return 0;
+}
+
+const etx_imu_driver_t imu_icm42627_driver = {
+  gyro42627Init,
+  gyro42627Read,
+  "ICM42627",
+};

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;

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

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

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