diff --git a/radio/src/drivers/icm42627.cpp b/radio/src/drivers/icm42627.cpp new file mode 100644 index 00000000000..ad796fd8a82 --- /dev/null +++ b/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", +}; diff --git a/radio/src/drivers/icm42627.h b/radio/src/drivers/icm42627.h new file mode 100644 index 00000000000..70c9395a6aa --- /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; 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