292 lines
9.2 KiB
C++
292 lines
9.2 KiB
C++
// icm20948.hpp — InvenSense ICM-20948 IMU driver
|
||
// Linux /dev/i2c-1 via I2C_RDWR ioctl — no external libs, no libgpiod.
|
||
//
|
||
// Hardware (THE TRUTH):
|
||
// SDA = GPIO2 (pin 3), SCL = GPIO3 (pin 5)
|
||
// VDD = 3.3V, GND = GND
|
||
// AD0 pin determines I2C address: GND → 0x68, VCC → 0x69
|
||
//
|
||
// If i2cdetect -y 1 shows nothing:
|
||
// 1. Enable I2C: sudo raspi-config → Interfaces → I2C → Yes → reboot
|
||
// 2. Check wiring — SDA/SCL must have pull-ups (Pi has built-in 1.8kΩ)
|
||
// 3. Verify AD0 pin state to confirm address (0x68 vs 0x69)
|
||
// 4. Try slower I2C: add i2c_arm_baudrate=50000 in /boot/config.txt
|
||
#pragma once
|
||
|
||
#include <chrono>
|
||
#include <cstdio>
|
||
#include <cstring>
|
||
#include <initializer_list>
|
||
#include <thread>
|
||
|
||
#include <cerrno>
|
||
#include <fcntl.h>
|
||
#include <linux/i2c-dev.h>
|
||
#include <linux/i2c.h>
|
||
#include <sys/ioctl.h>
|
||
#include <unistd.h>
|
||
|
||
namespace icm20948 {
|
||
|
||
// ---- Register map (Bank 0) -------------------------------------------------
|
||
constexpr uint8_t REG_WHO_AM_I = 0x00; // should read 0xEA
|
||
constexpr uint8_t REG_USER_CTRL = 0x03;
|
||
constexpr uint8_t REG_PWR_MGMT_1 = 0x06; // bit7=DEVICE_RESET, bits2:0=CLKSEL
|
||
constexpr uint8_t REG_PWR_MGMT_2 = 0x07; // bit5:3=disable_accel, bit2:0=disable_gyro
|
||
constexpr uint8_t REG_ACCEL_XOUT_H = 0x2D;
|
||
constexpr uint8_t REG_GYRO_XOUT_H = 0x33;
|
||
constexpr uint8_t REG_TEMP_OUT_H = 0x39;
|
||
constexpr uint8_t REG_INT_STATUS = 0x19;
|
||
constexpr uint8_t REG_INT_STATUS_1 = 0x1A; // bit0 = RAW_DATA_0_RDY_INT
|
||
constexpr uint8_t REG_BANK_SEL = 0x7F;
|
||
|
||
// ---- Register map (Bank 2) -------------------------------------------------
|
||
constexpr uint8_t B2_GYRO_SMPLRT_DIV = 0x00;
|
||
constexpr uint8_t B2_GYRO_CONFIG_1 = 0x01; // bits3:2=FS_SEL bits1:0=DLPF
|
||
constexpr uint8_t B2_ACCEL_SMPLRT_DIV_1 = 0x10;
|
||
constexpr uint8_t B2_ACCEL_SMPLRT_DIV_2 = 0x11;
|
||
constexpr uint8_t B2_ACCEL_CONFIG = 0x14; // bits3:2=FS_SEL bits1:0=DLPF
|
||
|
||
constexpr uint8_t WHO_AM_I_VAL = 0xEA;
|
||
|
||
// ---- Scale configuration ---------------------------------------------------
|
||
// Accel full-scale: 0=±2g 1=±4g 2=±8g 3=±16g
|
||
// Gyro full-scale: 0=±250 1=±500 2=±1000 3=±2000 dps
|
||
constexpr float ACCEL_SCALE[4] = { 2.0f/32768, 4.0f/32768, 8.0f/32768, 16.0f/32768 };
|
||
constexpr float GYRO_SCALE[4] = { 250.0f/32768, 500.0f/32768, 1000.0f/32768, 2000.0f/32768 };
|
||
|
||
// Raw 16-bit sensor sample
|
||
struct RawSample {
|
||
int16_t ax, ay, az; // accelerometer
|
||
int16_t gx, gy, gz; // gyroscope
|
||
int16_t temp_raw; // temperature raw
|
||
};
|
||
|
||
// Scaled sample with physical units
|
||
struct Sample {
|
||
float ax, ay, az; // g
|
||
float gx, gy, gz; // deg/s
|
||
float temp_c; // Celsius
|
||
};
|
||
|
||
struct Config {
|
||
const char *i2c_path = "/dev/i2c-1";
|
||
uint8_t accel_fs = 0; // 0=±2g, 1=±4g, 2=±8g, 3=±16g
|
||
uint8_t gyro_fs = 0; // 0=±250dps, 1=±500, 2=±1000, 3=±2000
|
||
bool verbose = false;
|
||
};
|
||
|
||
class Imu {
|
||
public:
|
||
bool begin(const Config &cfg);
|
||
void end();
|
||
|
||
// Returns true when new data is read.
|
||
bool read(RawSample &raw);
|
||
bool read(Sample &s);
|
||
|
||
// Returns the detected I2C address (0x68 or 0x69), or 0 if not found.
|
||
uint8_t address() const { return addr_; }
|
||
|
||
// Direct register access (for debugging / bank switching experiments).
|
||
bool writeReg(uint8_t reg, uint8_t val);
|
||
uint8_t readReg(uint8_t reg);
|
||
bool readRegs(uint8_t reg, uint8_t *buf, uint8_t n);
|
||
bool selectBank(uint8_t bank); // 0–3
|
||
|
||
private:
|
||
Config cfg_{};
|
||
int fd_ = -1;
|
||
uint8_t addr_ = 0;
|
||
uint8_t accel_fs_ = 0;
|
||
uint8_t gyro_fs_ = 0;
|
||
};
|
||
|
||
// ---- Implementation ---------------------------------------------------------
|
||
|
||
inline bool Imu::writeReg(uint8_t reg, uint8_t val)
|
||
{
|
||
uint8_t buf[2] = { reg, val };
|
||
i2c_msg msg{};
|
||
msg.addr = addr_;
|
||
msg.flags = 0;
|
||
msg.len = 2;
|
||
msg.buf = buf;
|
||
i2c_rdwr_ioctl_data io{};
|
||
io.msgs = &msg;
|
||
io.nmsgs = 1;
|
||
if (ioctl(fd_, I2C_RDWR, &io) == 1)
|
||
return true;
|
||
if (cfg_.verbose)
|
||
std::fprintf(stderr, "[icm20948] write 0x%02X=0x%02X failed at 0x%02X: %s\n",
|
||
reg, val, addr_, std::strerror(errno));
|
||
return false;
|
||
}
|
||
|
||
inline bool Imu::readRegs(uint8_t reg, uint8_t *buf, uint8_t n)
|
||
{
|
||
// Generates proper repeated-start: [START|ADDR+W|REG|RSTART|ADDR+R|DATA|STOP]
|
||
i2c_msg msgs[2]{};
|
||
msgs[0].addr = addr_;
|
||
msgs[0].flags = 0;
|
||
msgs[0].len = 1;
|
||
msgs[0].buf = ®
|
||
msgs[1].addr = addr_;
|
||
msgs[1].flags = I2C_M_RD;
|
||
msgs[1].len = n;
|
||
msgs[1].buf = buf;
|
||
i2c_rdwr_ioctl_data io{};
|
||
io.msgs = msgs;
|
||
io.nmsgs = 2;
|
||
if (ioctl(fd_, I2C_RDWR, &io) == 2)
|
||
return true;
|
||
if (cfg_.verbose)
|
||
std::fprintf(stderr, "[icm20948] read 0x%02X (%u B) failed at 0x%02X: %s\n",
|
||
reg, n, addr_, std::strerror(errno));
|
||
return false;
|
||
}
|
||
|
||
inline uint8_t Imu::readReg(uint8_t reg)
|
||
{
|
||
uint8_t v = 0;
|
||
if (!readRegs(reg, &v, 1) && cfg_.verbose)
|
||
std::fprintf(stderr, "[icm20948] readReg(0x%02X) → no data (check NACK / wiring)\n", reg);
|
||
return v;
|
||
}
|
||
|
||
inline bool Imu::selectBank(uint8_t bank)
|
||
{
|
||
return writeReg(REG_BANK_SEL, (uint8_t)(bank << 4));
|
||
}
|
||
|
||
inline bool Imu::begin(const Config &c)
|
||
{
|
||
cfg_ = c;
|
||
|
||
fd_ = ::open(c.i2c_path, O_RDWR);
|
||
if (fd_ < 0) {
|
||
if (c.verbose)
|
||
std::fprintf(stderr, "[icm20948] cannot open %s: %m\n", c.i2c_path);
|
||
return false;
|
||
}
|
||
|
||
// Auto-detect I2C address (AD0 low → 0x68, AD0 high → 0x69)
|
||
bool found = false;
|
||
for (uint8_t a : {(uint8_t)0x68, (uint8_t)0x69}) {
|
||
addr_ = a;
|
||
// Ensure bank 0 before reading WHO_AM_I
|
||
if (!writeReg(REG_BANK_SEL, 0x00))
|
||
continue;
|
||
uint8_t id = 0;
|
||
if (!readRegs(REG_WHO_AM_I, &id, 1))
|
||
continue;
|
||
if (c.verbose)
|
||
std::fprintf(stderr, "[icm20948] probe 0x%02X → WHO_AM_I = 0x%02X\n", a, id);
|
||
if (id == WHO_AM_I_VAL) { found = true; break; }
|
||
}
|
||
if (!found) {
|
||
if (c.verbose)
|
||
std::fprintf(stderr, "[icm20948] chip not found on %s at 0x68 or 0x69\n"
|
||
" check wiring, I2C enabled, pull-ups\n",
|
||
c.i2c_path);
|
||
::close(fd_); fd_ = -1;
|
||
return false;
|
||
}
|
||
if (c.verbose)
|
||
std::fprintf(stderr, "[icm20948] found at 0x%02X\n", addr_);
|
||
|
||
// Device reset — sets all registers to default values.
|
||
selectBank(0);
|
||
writeReg(REG_PWR_MGMT_1, 0x80); // DEVICE_RESET bit
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||
|
||
// Wait until reset is complete (DEVICE_RESET bit self-clears).
|
||
for (int i = 0; i < 50; ++i) {
|
||
if (!(readReg(REG_PWR_MGMT_1) & 0x80)) break;
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||
}
|
||
|
||
// Verify still alive after reset.
|
||
if (readReg(REG_WHO_AM_I) != WHO_AM_I_VAL) {
|
||
if (c.verbose)
|
||
std::fprintf(stderr, "[icm20948] WHO_AM_I mismatch after reset\n");
|
||
::close(fd_); fd_ = -1;
|
||
return false;
|
||
}
|
||
|
||
// Disable internal I2C master (mag interface) — matches Adafruit begin_I2C().
|
||
writeReg(REG_USER_CTRL, 0x00);
|
||
|
||
// Wake up: auto-select clock source (best practice per datasheet).
|
||
writeReg(REG_PWR_MGMT_1, 0x01);
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
||
|
||
// Enable all accel + gyro axes.
|
||
writeReg(REG_PWR_MGMT_2, 0x00);
|
||
|
||
// Configure full-scale in Bank 2.
|
||
accel_fs_ = c.accel_fs & 0x03;
|
||
gyro_fs_ = c.gyro_fs & 0x03;
|
||
selectBank(2);
|
||
writeReg(B2_ACCEL_CONFIG, (uint8_t)(accel_fs_ << 2));
|
||
writeReg(B2_GYRO_CONFIG_1, (uint8_t)(gyro_fs_ << 2));
|
||
|
||
selectBank(0); // leave in bank 0 for normal operation
|
||
|
||
if (c.verbose) {
|
||
static const unsigned gyro_dps[] = { 250u, 500u, 1000u, 2000u };
|
||
std::fprintf(stderr, "[icm20948] init OK: accel ±%ug gyro ±%udps\n",
|
||
(unsigned)(2u << accel_fs_), gyro_dps[gyro_fs_]);
|
||
}
|
||
return true;
|
||
}
|
||
|
||
inline void Imu::end()
|
||
{
|
||
if (fd_ >= 0) { ::close(fd_); fd_ = -1; }
|
||
}
|
||
|
||
inline bool Imu::read(RawSample &raw)
|
||
{
|
||
selectBank(0);
|
||
// Wait for fresh accel/gyro/temp (datasheet: INT_STATUS_1 bit0 RAW_DATA_0_RDY_INT).
|
||
for (int i = 0; i < 50; ++i) {
|
||
if (readReg(REG_INT_STATUS_1) & 0x01) break;
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||
}
|
||
uint8_t buf[14]{};
|
||
if (!readRegs(REG_ACCEL_XOUT_H, buf, 14)) return false;
|
||
|
||
auto s16 = [](uint8_t hi, uint8_t lo) -> int16_t {
|
||
return (int16_t)((uint16_t)hi << 8 | lo);
|
||
};
|
||
// Burst from 0x2D: accel (6) → gyro (6) → temp (2)
|
||
raw.ax = s16(buf[0], buf[1]);
|
||
raw.ay = s16(buf[2], buf[3]);
|
||
raw.az = s16(buf[4], buf[5]);
|
||
raw.gx = s16(buf[6], buf[7]);
|
||
raw.gy = s16(buf[8], buf[9]);
|
||
raw.gz = s16(buf[10], buf[11]);
|
||
raw.temp_raw = s16(buf[12], buf[13]);
|
||
return true;
|
||
}
|
||
|
||
inline bool Imu::read(Sample &s)
|
||
{
|
||
RawSample raw{};
|
||
if (!read(raw)) return false;
|
||
float as = ACCEL_SCALE[accel_fs_];
|
||
float gs = GYRO_SCALE[gyro_fs_];
|
||
s.ax = raw.ax * as;
|
||
s.ay = raw.ay * as;
|
||
s.az = raw.az * as;
|
||
s.gx = raw.gx * gs;
|
||
s.gy = raw.gy * gs;
|
||
s.gz = raw.gz * gs;
|
||
// Temp formula from ICM-20948 datasheet: T°C = TEMP_OUT / 333.87 + 21.0
|
||
s.temp_c = (float)raw.temp_raw / 333.87f + 21.0f;
|
||
return true;
|
||
}
|
||
|
||
} // namespace icm20948
|