diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index 1c7b220f7f2..2f23d9ea3f5 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -5141,6 +5141,9 @@ const cliResourceValue_t resourceTable[] = { #endif DEFW( OWNER_GYRO_EXTI, PG_GYRO_DEVICE_CONFIG, gyroDeviceConfig_t, extiTag, MAX_GYRODEV_COUNT ), DEFW( OWNER_GYRO_CS, PG_GYRO_DEVICE_CONFIG, gyroDeviceConfig_t, csnTag, MAX_GYRODEV_COUNT ), +#if defined(USE_GYRO_CLKIN) + DEFW( OWNER_GYRO_CLKIN, PG_GYRO_DEVICE_CONFIG, gyroDeviceConfig_t, clkIn, MAX_GYRODEV_COUNT), +#endif #ifdef USE_USB_DETECT DEFS( OWNER_USB_DETECT, PG_USB_CONFIG, usbDev_t, detectPin ), #endif diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c index 1fe41f8ff35..5aa21566b14 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c @@ -42,8 +42,10 @@ #include "drivers/io.h" #include "drivers/sensor.h" #include "drivers/time.h" +#include "drivers/pwm_output.h" #include "sensors/gyro.h" +#include "pg/gyrodev.h" // Allows frequency to be set from the compile line EXTRA_FLAGS by adding e.g. // -D'ICM426XX_CLOCK=12000000'. If using the configurator this simply becomes @@ -56,6 +58,8 @@ #define ICM426XX_MAX_SPI_CLK_HZ ICM426XX_CLOCK #endif +#define ICM426XX_CLKIN_FREQ 32000 + #define ICM426XX_RA_REG_BANK_SEL 0x76 #define ICM426XX_BANK_SELECT0 0x00 #define ICM426XX_BANK_SELECT1 0x01 @@ -121,6 +125,12 @@ #define ICM426XX_UI_DRDY_INT1_EN_DISABLED (0 << 3) #define ICM426XX_UI_DRDY_INT1_EN_ENABLED (1 << 3) +// specific to CLKIN configuration +#define ICM426XX_INTF_CONFIG5 0x7B // User Bank 1 +#define ICM426XX_INTF_CONFIG1_CLKIN (1 << 2) +#define ICM426XX_INTF_CONFIG5_PIN9_FUNCTION_MASK (3 << 1) // PIN9 mode config +#define ICM426XX_INTF_CONFIG5_PIN9_FUNCTION_CLKIN (2 << 1) // PIN9 as CLKIN + typedef enum { ODR_CONFIG_8K = 0, ODR_CONFIG_4K, @@ -168,10 +178,85 @@ static aafConfig_t aafLUT42605[AAF_CONFIG_COUNT] = { // see table in section 5. [AAF_CONFIG_1962HZ] = { 63, 3968, 3 }, // 995 Hz is the max cutoff on the 42605 }; +static void setUserBank(const extDevice_t *dev, const uint8_t user_bank) +{ + spiWriteReg(dev, ICM426XX_RA_REG_BANK_SEL, user_bank & 7); +} + +#if defined(USE_GYRO_CLKIN) +static pwmOutputPort_t pwmGyroClk = {0}; + +static bool initExternalClock(const extDevice_t *dev) +{ + int cfg; + if (&gyro.gyroSensor1.gyroDev.dev == dev) { + cfg = 0; + } else if (&gyro.gyroSensor2.gyroDev.dev == dev) { + cfg = 1; + } else { + // only gyroSensor device supported + return false; + } + const ioTag_t tag = gyroDeviceConfig(cfg)->clkIn; + const IO_t io = IOGetByTag(tag); + if (pwmGyroClk.enabled) { + // pwm is already taken, but test for shared clkIn pin + return pwmGyroClk.io == io; + } + + const timerHardware_t *timer = timerAllocate(tag, OWNER_GYRO_CLKIN, RESOURCE_INDEX(cfg)); + if (!timer) { + // Error handling: failed to allocate timer + return false; + } + + pwmGyroClk.io = io; + pwmGyroClk.enabled = true; + + IOInit(io, OWNER_GYRO_CLKIN, RESOURCE_INDEX(cfg)); + IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction); + + const uint32_t clock = timerClock(timer->tim); // Get the timer clock frequency + const uint16_t period = clock / ICM426XX_CLKIN_FREQ; + + // Calculate duty cycle value for 50% + const uint16_t value = period / 2; + + // Configure PWM output + pwmOutConfig(&pwmGyroClk.channel, timer, clock, period - 1, value - 1, 0); + + // Set CCR value + *pwmGyroClk.channel.ccr = value - 1; + + return true; +} + +static void icm426xxEnableExternalClock(const extDevice_t *dev) +{ + if (initExternalClock(dev)) { + // Switch to Bank 1 and set bits 2:1 in INTF_CONFIG5 (0x7B) to enable CLKIN on PIN9 + setUserBank(dev, ICM426XX_BANK_SELECT1); + uint8_t intf_config5 = spiReadRegMsk(dev, ICM426XX_INTF_CONFIG5); + intf_config5 = (intf_config5 & ~ICM426XX_INTF_CONFIG5_PIN9_FUNCTION_MASK) | ICM426XX_INTF_CONFIG5_PIN9_FUNCTION_CLKIN; // Clear & set bits 2:1 to 0b10 for CLKIN + spiWriteReg(dev, ICM426XX_INTF_CONFIG5, intf_config5); + + // Switch to Bank 0 and set bit 2 in RTC_MODE (0x4D) to enable external CLK signal + setUserBank(dev, ICM426XX_BANK_SELECT0); + uint8_t rtc_mode = spiReadRegMsk(dev, ICM426XX_INTF_CONFIG1); + rtc_mode |= ICM426XX_INTF_CONFIG1_CLKIN; // Enable external CLK signal + spiWriteReg(dev, ICM426XX_INTF_CONFIG1, rtc_mode); + } +} +#endif + uint8_t icm426xxSpiDetect(const extDevice_t *dev) { spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, 0x00); +#if defined(USE_GYRO_CLKIN) + icm426xxEnableExternalClock(dev); +#endif + uint8_t icmDetected = MPU_NONE; uint8_t attemptsRemaining = 20; do { @@ -235,11 +320,6 @@ static void turnGyroAccOn(const extDevice_t *dev) delay(1); } -static void setUserBank(const extDevice_t *dev, const uint8_t user_bank) -{ - spiWriteReg(dev, ICM426XX_RA_REG_BANK_SEL, user_bank & 7); -} - void icm426xxGyroInit(gyroDev_t *gyro) { const extDevice_t *dev = &gyro->dev; diff --git a/src/main/drivers/resource.c b/src/main/drivers/resource.c index ece5eb91f48..24882a30650 100644 --- a/src/main/drivers/resource.c +++ b/src/main/drivers/resource.c @@ -115,4 +115,5 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = { "SOFTSERIAL_RX", "LPUART_TX", "LPUART_RX", + "GYRO_CLKIN", }; diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 1f583af67cf..ddb9720be68 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -113,6 +113,7 @@ typedef enum { OWNER_SOFTSERIAL_RX, OWNER_LPUART_TX, OWNER_LPUART_RX, + OWNER_GYRO_CLKIN, OWNER_TOTAL_COUNT } resourceOwner_e; diff --git a/src/main/pg/gyrodev.c b/src/main/pg/gyrodev.c index ffcffe272fb..16e2321e8d3 100644 --- a/src/main/pg/gyrodev.c +++ b/src/main/pg/gyrodev.c @@ -50,6 +50,14 @@ #define GYRO_2_EXTI_PIN NONE #endif +#ifndef GYRO_1_CLKIN_PIN +#define GYRO_1_CLKIN_PIN NONE +#endif + +#ifndef GYRO_2_CLKIN_PIN +#define GYRO_2_CLKIN_PIN NONE +#endif + #ifdef MPU_ADDRESS #define GYRO_I2C_ADDRESS MPU_ADDRESS #else @@ -68,7 +76,7 @@ #endif #if defined(USE_SPI_GYRO) && (defined(GYRO_1_SPI_INSTANCE) || defined(GYRO_2_SPI_INSTANCE)) -static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *instance, ioTag_t csnTag, ioTag_t extiTag, uint8_t alignment, sensorAlignment_t customAlignment) +static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *instance, ioTag_t csnTag, ioTag_t extiTag, ioTag_t clkInTag, uint8_t alignment, sensorAlignment_t customAlignment) { devconf->busType = BUS_TYPE_SPI; devconf->spiBus = SPI_DEV_TO_CFG(spiDeviceByInstance(instance)); @@ -76,6 +84,7 @@ static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *i devconf->extiTag = extiTag; devconf->alignment = alignment; devconf->customAlignment = customAlignment; + devconf->clkIn = clkInTag; } #endif @@ -91,7 +100,7 @@ static void gyroResetI2cDeviceConfig(gyroDeviceConfig_t *devconf, I2CDevice i2cb } #endif -PG_REGISTER_ARRAY_WITH_RESET_FN(gyroDeviceConfig_t, MAX_GYRODEV_COUNT, gyroDeviceConfig, PG_GYRO_DEVICE_CONFIG, 0); +PG_REGISTER_ARRAY_WITH_RESET_FN(gyroDeviceConfig_t, MAX_GYRODEV_COUNT, gyroDeviceConfig, PG_GYRO_DEVICE_CONFIG, 1); void pgResetFn_gyroDeviceConfig(gyroDeviceConfig_t *devconf) { @@ -106,7 +115,7 @@ void pgResetFn_gyroDeviceConfig(gyroDeviceConfig_t *devconf) // All multi-gyro boards use SPI based gyros. #ifdef USE_SPI_GYRO #ifdef GYRO_1_SPI_INSTANCE - gyroResetSpiDeviceConfig(&devconf[0], GYRO_1_SPI_INSTANCE, IO_TAG(GYRO_1_CS_PIN), IO_TAG(GYRO_1_EXTI_PIN), GYRO_1_ALIGN, customAlignment1); + gyroResetSpiDeviceConfig(&devconf[0], GYRO_1_SPI_INSTANCE, IO_TAG(GYRO_1_CS_PIN), IO_TAG(GYRO_1_EXTI_PIN), IO_TAG(GYRO_1_CLKIN_PIN), GYRO_1_ALIGN, customAlignment1); #else devconf[0].busType = BUS_TYPE_NONE; #endif @@ -120,7 +129,8 @@ void pgResetFn_gyroDeviceConfig(gyroDeviceConfig_t *devconf) #endif // GYRO_2_CUSTOM_ALIGN #ifdef GYRO_2_SPI_INSTANCE - gyroResetSpiDeviceConfig(&devconf[1], GYRO_2_SPI_INSTANCE, IO_TAG(GYRO_2_CS_PIN), IO_TAG(GYRO_2_EXTI_PIN), GYRO_2_ALIGN, customAlignment2); + // TODO: CLKIN gyro 2 on separate pin is not supported yet. need to implement it + gyroResetSpiDeviceConfig(&devconf[1], GYRO_2_SPI_INSTANCE, IO_TAG(GYRO_2_CS_PIN), IO_TAG(GYRO_2_EXTI_PIN), IO_TAG(GYRO_2_CLKIN_PIN), GYRO_2_ALIGN, customAlignment2); #else devconf[1].busType = BUS_TYPE_NONE; #endif diff --git a/src/main/pg/gyrodev.h b/src/main/pg/gyrodev.h index 289e579746c..af81b4b376a 100644 --- a/src/main/pg/gyrodev.h +++ b/src/main/pg/gyrodev.h @@ -45,6 +45,7 @@ typedef struct gyroDeviceConfig_s { ioTag_t extiTag; uint8_t alignment; // sensor_align_e sensorAlignment_t customAlignment; + ioTag_t clkIn; } gyroDeviceConfig_t; PG_DECLARE_ARRAY(gyroDeviceConfig_t, MAX_GYRODEV_COUNT, gyroDeviceConfig);