mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 19:40:31 +03:00
Add support external clock for gyro ICM42688P (#13912)
This commit is contained in:
cvetaevvitaliy
GitHub
parent
866191dfad
commit
715c1671a8
6 changed files with 105 additions and 9 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<n> 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;
|
||||
|
|
|
|||
|
|
@ -115,4 +115,5 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
|
|||
"SOFTSERIAL_RX",
|
||||
"LPUART_TX",
|
||||
"LPUART_RX",
|
||||
"GYRO_CLKIN",
|
||||
};
|
||||
|
|
|
|||
|
|
@ -113,6 +113,7 @@ typedef enum {
|
|||
OWNER_SOFTSERIAL_RX,
|
||||
OWNER_LPUART_TX,
|
||||
OWNER_LPUART_RX,
|
||||
OWNER_GYRO_CLKIN,
|
||||
OWNER_TOTAL_COUNT
|
||||
} resourceOwner_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue