Skip to content

adding icm42670p driver #201

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
r-u-t-r-A wants to merge 1 commit into
base: master
Choose a base branch
from
Open

adding icm42670p driver #201

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions lib/Espfc/src/Device/GyroDevice.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,11 @@ enum GyroDeviceType {
GYRO_LSM6DSO = 6,
GYRO_ICM20602 = 7,
GYRO_BMI160 = 8,
GYRO_ICM42670P = 9,
GYRO_MAX

@rtlopez rtlopez May 20, 2026 •
edited
Loading

Copy link
Copy Markdown
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think You missed this place
https://github.com/r-u-t-r-A/esp-fc/blob/385dc0453495c6b8dc2090d9318657e02d0c9e71/lib/Espfc/src/Device/GyroDevice.cpp#L9

lib/Espfc/src/Device/GyroDevice.cpp:9

static const char* devChoices[] = { PSTR("AUTO"), PSTR("NONE"), PSTR("MPU6000"), PSTR("MPU6050"), PSTR("MPU6500"), PSTR("MPU9250"), PSTR("LSM6DSO"), PSTR("ICM20602"),PSTR("BMI160"), NULL };

};


namespace Device {

class GyroDevice: public BusAwareDevice
Expand Down
129 changes: 129 additions & 0 deletions lib/Espfc/src/Device/GyroICM42760P.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,129 @@
#ifndef _ESPFC_DEVICE_GYRO_ICM42670P_H_
#define _ESPFC_DEVICE_GYRO_ICM42670P_H_
Comment on lines +1 to +2

@coderabbitai coderabbitai Bot May 18, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue | 🟡 Minor | ⚡ Quick win

Filename typo: GyroICM42760P.h should be GyroICM42670P.h.

The file is named GyroICM42760P.h but the device is ICM-42670-P (note: 42670, not 42760). The include guard and class name correctly use 42670P. Rename the file to GyroICM42670P.h for consistency, and update the include in Hardware.cpp accordingly.

🤖 Prompt for AI Agents 
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.

In `@lib/Espfc/src/Device/GyroICM42760P.h` around lines 1 - 2, The header file is
misnamed GyroICM42760P.h but the device and symbols use ICM42670P; rename the
file to GyroICM42670P.h to match the include guard/class naming and ensure
consistency, then update any includes that reference the old name (notably the
include in Hardware.cpp) to the new filename GyroICM42670P.h so compilation and
symbol resolution remain correct.


#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
4 changes: 4 additions & 0 deletions lib/Espfc/src/Hardware.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
#include "Device/GyroLSM6DSO.h"
#include "Device/GyroICM20602.h"
#include "Device/GyroBMI160.h"
#include "Device/GyroICM42760P.h"

@coderabbitai coderabbitai Bot May 18, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ Potential issue | 🟡 Minor | ⚡ Quick win

Update include path when filename is corrected.

This include path references the typo'd filename (GyroICM42760P.h). When the filename is corrected to GyroICM42670P.h, update this include accordingly.

🤖 Prompt for AI Agents 
Verify each finding against current code. Fix only still-valid issues, skip the
rest with a brief reason, keep changes minimal, and validate.

In `@lib/Espfc/src/Hardware.cpp` at line 9, Update the include in Hardware.cpp to
reference the corrected header name: replace the old include of
"Device/GyroICM42760P.h" with the corrected "Device/GyroICM42670P.h" so that the
compilation uses the fixed GyroICM42670P.h header (search for the include line
containing GyroICM42760P and change the filename to GyroICM42670P).

#include "Device/MagHMC5338L.h"
#include "Device/MagQMC5338L.h"
#include "Device/MagQMC5338P.h"
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down