From 385dc0453495c6b8dc2090d9318657e02d0c9e71 Mon Sep 17 00:00:00 2001 From: r-u-t-r-A Date: Tue, 24 Feb 2026 13:22:56 +0100 Subject: [PATCH] adding icm42670p driver --- lib/Espfc/src/Device/GyroDevice.h | 2 + lib/Espfc/src/Device/GyroICM42760P.h | 129 +++++++++++++++++++++++++++ lib/Espfc/src/Hardware.cpp | 4 + 3 files changed, 135 insertions(+) create mode 100644 lib/Espfc/src/Device/GyroICM42760P.h diff --git a/lib/Espfc/src/Device/GyroDevice.h b/lib/Espfc/src/Device/GyroDevice.h index 0f49d5ee..2acf6816 100644 --- a/lib/Espfc/src/Device/GyroDevice.h +++ b/lib/Espfc/src/Device/GyroDevice.h @@ -16,9 +16,11 @@ enum GyroDeviceType { GYRO_LSM6DSO = 6, GYRO_ICM20602 = 7, GYRO_BMI160 = 8, + GYRO_ICM42670P = 9, GYRO_MAX }; + namespace Device { class GyroDevice: public BusAwareDevice diff --git a/lib/Espfc/src/Device/GyroICM42760P.h b/lib/Espfc/src/Device/GyroICM42760P.h new file mode 100644 index 00000000..5e0a82a9 --- /dev/null +++ b/lib/Espfc/src/Device/GyroICM42760P.h @@ -0,0 +1,129 @@ +#ifndef _ESPFC_DEVICE_GYRO_ICM42670P_H_ +#define _ESPFC_DEVICE_GYRO_ICM42670P_H_ + +#include "BusDevice.h" +#include "GyroDevice.h" +#include "helper_3dmath.h" +#include "Debug_Espfc.h" + +// Register Map for ICM-42670-P +#define ICM42670P_REG_MCLK_RDY 0x00 +#define ICM42670P_REG_DEVICE_CONFIG 0x11 +#define ICM42670P_REG_PWR_MGMT0 0x1F +#define ICM42670P_REG_GYRO_CONFIG0 0x20 +#define ICM42670P_REG_ACCEL_CONFIG0 0x21 +#define ICM42670P_REG_WHO_AM_I 0x75 +#define ICM42670P_REG_ACCEL_DATA_X1 0x0B +#define ICM42670P_REG_GYRO_DATA_X1 0x11 + +// Constants +#define ICM42670P_WHO_AM_I_CONST 0x67 +#define ICM42670P_RESET_BIT 0x01 + +// Power Management (Bits 3:2 = Gyro Mode, Bits 1:0 = Accel Mode) +// 11 = Low Noise (LN) Mode +#define ICM42670P_PWR_LN_MODE 0x0F + +// Config Values (FS=2000dps/16g, ODR=1.6kHz) +// FS_SEL (Bits 6:5): 00 = 2000dps / 16g +// ODR (Bits 3:0): 0101 (0x05) = 1.6kHz (LN) +#define ICM42670P_CONFIG_DEFAULT 0x05 + +namespace Espfc { + +namespace Device { + +class GyroICM42670P: public GyroDevice +{ + public: + int begin(BusDevice * bus) override + { + // Try default address first (0x68), then alternate (0x69) + return begin(bus, 0x68) ? 1 : begin(bus, 0x69); + } + + int begin(BusDevice * bus, uint8_t addr) override + { + setBus(bus, addr); + + if(!testConnection()) return 0; + + // 1. Soft Reset + _bus->writeByte(_addr, ICM42670P_REG_DEVICE_CONFIG, ICM42670P_RESET_BIT); + delay(100); + + // 2. Power Management (CRITICAL: Enable Gyro & Accel in Low Noise Mode) + // Unlike MPU6000, this sensor defaults to sleep. + _bus->writeByte(_addr, ICM42670P_REG_PWR_MGMT0, ICM42670P_PWR_LN_MODE); + delay(2); // Wait for PLL + + // 3. Configure Gyro (±2000dps, 1.6kHz) + _bus->writeByte(_addr, ICM42670P_REG_GYRO_CONFIG0, ICM42670P_CONFIG_DEFAULT); + + // 4. Configure Accel (±16g, 1.6kHz) + _bus->writeByte(_addr, ICM42670P_REG_ACCEL_CONFIG0, ICM42670P_CONFIG_DEFAULT); + + return 1; + } + + GyroDeviceType getType() const override + { + return GYRO_ICM42670P; + } + + int FAST_CODE_ATTR readGyro(VectorInt16& v) override + { + uint8_t buffer[6]; + // Read 6 bytes starting from GYRO_DATA_X1 (0x11) + _bus->readFast(_addr, ICM42670P_REG_GYRO_DATA_X1, 6, buffer); + + v.x = (((int16_t)buffer[0]) << 8) | buffer[1]; + v.y = (((int16_t)buffer[2]) << 8) | buffer[3]; + v.z = (((int16_t)buffer[4]) << 8) | buffer[5]; + + return 1; + } + + int readAccel(VectorInt16& v) override + { + uint8_t buffer[6]; + // Read 6 bytes starting from ACCEL_DATA_X1 (0x0B) + _bus->readFast(_addr, ICM42670P_REG_ACCEL_DATA_X1, 6, buffer); + + v.x = (((int16_t)buffer[0]) << 8) | buffer[1]; + v.y = (((int16_t)buffer[2]) << 8) | buffer[3]; + v.z = (((int16_t)buffer[4]) << 8) | buffer[5]; + + return 1; + } + + void setDLPFMode(uint8_t mode) override + { + // ICM-42670-P has fixed ODR/Filter presets in CONFIG0. + // For this simple implementation, we stick to the default initialized in begin(). + } + + int getRate() const override + { + return 1600; // We configured 1.6kHz in begin() + } + + void setRate(int rate) override + { + // Helper function to change ODR could go here, + // but 1.6kHz is standard for Betaflight-style loops. + } + + bool testConnection() override + { + uint8_t whoami = 0; + _bus->readByte(_addr, ICM42670P_REG_WHO_AM_I, &whoami); + return (whoami == ICM42670P_WHO_AM_I_CONST); + } +}; + +} + +} + +#endif \ No newline at end of file diff --git a/lib/Espfc/src/Hardware.cpp b/lib/Espfc/src/Hardware.cpp index f765426d..f3f75c21 100644 --- a/lib/Espfc/src/Hardware.cpp +++ b/lib/Espfc/src/Hardware.cpp @@ -6,6 +6,7 @@ #include "Device/GyroLSM6DSO.h" #include "Device/GyroICM20602.h" #include "Device/GyroBMI160.h" +#include "Device/GyroICM42760P.h" #include "Device/MagHMC5338L.h" #include "Device/MagQMC5338L.h" #include "Device/MagQMC5338P.h" @@ -39,6 +40,7 @@ namespace { static Espfc::Device::GyroLSM6DSO lsm6dso; static Espfc::Device::GyroICM20602 icm20602; static Espfc::Device::GyroBMI160 bmi160; + static Espfc::Device::GyroICM42670P icm42670p; static Espfc::Device::MagHMC5338L hmc5883l; static Espfc::Device::MagQMC5338L qmc5883l; static Espfc::Device::MagQMC5338P qmc5883p; @@ -95,6 +97,7 @@ void Hardware::detectGyro() if(!detectedGyro && detectDevice(icm20602, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &icm20602; if(!detectedGyro && detectDevice(bmi160, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &bmi160; if(!detectedGyro && detectDevice(lsm6dso, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &lsm6dso; + if(!detectedGyro && detectDevice(icm42670p, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &icm42670p; if(detectedGyro) gyroSlaveBus.begin(&spiBus, detectedGyro->getAddress()); } #endif @@ -107,6 +110,7 @@ void Hardware::detectGyro() if(!detectedGyro && detectDevice(bmi160, i2cBus)) detectedGyro = &bmi160; if(!detectedGyro && detectDevice(mpu6050, i2cBus)) detectedGyro = &mpu6050; if(!detectedGyro && detectDevice(lsm6dso, i2cBus)) detectedGyro = &lsm6dso; + if(!detectedGyro && detectDevice(icm42670p, i2cBus)) detectedGyro = &icm42670p; if(detectedGyro) gyroSlaveBus.begin(&i2cBus, detectedGyro->getAddress()); } #endif