IMU 측정, 커맨드 및 응답 추가

2026-04-12 18:42:54 +09:00
parent 8dcf4adf31
commit 6ac0490263
7 changed files with 250 additions and 1 deletions
@@ -0,0 +1,135 @@
+/*******************************************************************************
+ * @file    imu_i2c.c
+ * @brief   ICM42670P IMU 드라이버 (Zephyr I2C API)
+ *
+ * 기존 VesiScan-Basic(nRF5 SDK)의 imu_read_direct()를 Zephyr로 포팅.
+ *
+ * 동작 방식 (매 측정 시):
+ *   1) GYRO_CONFIG0  = 0x09 → ±2000dps, 100Hz ODR
+ *   2) ACCEL_CONFIG0 = 0x29 → ±4g, 100Hz ODR
+ *   3) PWR_MGMT0     = 0x0F → accel(low-noise) + gyro(low-noise) ON
+ *   4) 80ms 대기 (자이로 스타트업 최소 45ms + 여유)
+ *   5) 0x0B부터 12바이트 연속 읽기: accel XYZ(6B) + gyro XYZ(6B)
+ *   6) 빅엔디안 → int16_t 변환
+ *   7) PWR_MGMT0     = 0x00 → 슬립 (전력 절감)
+ *
+ * I2C 핀: SCL=P1.14, SDA=P1.15 (overlay에서 설정)
+ ******************************************************************************/
+
+#include <zephyr/kernel.h>
+#include <zephyr/drivers/i2c.h>
+#include <zephyr/devicetree.h>
+
+#include "imu_i2c.h"
+#include "debug_print.h"
+
+/*==============================================================================
+ * 디바이스트리 / I2C 설정
+ *============================================================================*/
+#define IMU_I2C_NODE    DT_NODELABEL(i2c0)
+#define IMU_I2C_ADDR    0x68   /* ICM42670P 기본 I2C 주소 (AD0=LOW) */
+
+/*==============================================================================
+ * ICM42670P 레지스터 주소
+ *============================================================================*/
+#define REG_PWR_MGMT0     0x1F  /* 전원 관리: accel/gyro 동작 모드 */
+#define REG_GYRO_CONFIG0  0x20  /* 자이로 FSR + ODR 설정 */
+#define REG_ACCEL_CONFIG0 0x21  /* 가속도 FSR + ODR 설정 */
+#define REG_ACCEL_DATA_X1 0x0B  /* 가속도 X축 상위 바이트 (시작 레지스터) */
+#define REG_WHO_AM_I      0x75  /* WHOAMI 레지스터 */
+#define ICM_WHOAMI        0x67  /* ICM42670P 식별값 */
+
+/* 자이로 스타트업 대기 시간 (ms) — 스펙 최소 45ms, 80ms로 여유 확보 */
+#define IMU_GYRO_STARTUP_MS   80
+
+static const struct device *i2c_bus;
+
+/*==============================================================================
+ * 내부 I2C 래퍼
+ *============================================================================*/
+
+/* 레지스터 1바이트 쓰기: [reg, val] 2바이트 TX */
+static int imu_write_reg(uint8_t reg, uint8_t val)
+{
+    uint8_t buf[2] = { reg, val };
+    return i2c_write(i2c_bus, buf, 2, IMU_I2C_ADDR);
+}
+
+/* 레지스터 연속 읽기: reg 주소 TX → data RX */
+static int imu_read_regs(uint8_t reg, uint8_t *data, uint8_t len)
+{
+    return i2c_write_read(i2c_bus, IMU_I2C_ADDR, &reg, 1, data, len);
+}
+
+/*==============================================================================
+ * 공개 API
+ *============================================================================*/
+
+int imu_init(void)
+{
+    i2c_bus = DEVICE_DT_GET(IMU_I2C_NODE);
+    if (!device_is_ready(i2c_bus)) {
+        DBG_PRINTF("[IMU] FAIL — I2C bus not ready\r\n");
+        return -1;
+    }
+
+    uint8_t who_am_i = 0;
+    if (imu_read_regs(REG_WHO_AM_I, &who_am_i, 1) != 0) {
+        DBG_PRINTF("[IMU] FAIL — WHOAMI read error (check SCL=P1.14, SDA=P1.15)\r\n");
+        return -2;
+    }
+
+    if (who_am_i != ICM_WHOAMI) {
+        DBG_PRINTF("[IMU] FAIL — WHOAMI mismatch (got=0x%02X, expected=0x%02X)\r\n",
+                   who_am_i, ICM_WHOAMI);
+        return -3;
+    }
+
+    DBG_PRINTF("[IMU] OK — ICM42670P detected (WHOAMI=0x%02X, addr=0x%02X)\r\n",
+               who_am_i, IMU_I2C_ADDR);
+    return 0;
+}
+
+int imu_read(int16_t accel[3], int16_t gyro[3])
+{
+    uint8_t raw[12];
+    int ret;
+
+    /* 자이로 설정: ±2000dps FSR, 100Hz ODR */
+    ret = imu_write_reg(REG_GYRO_CONFIG0, 0x09);
+    if (ret) { DBG_PRINTF("[IMU] FAIL — gyro config write (ret=%d)\r\n", ret); return -1; }
+
+    /* 가속도 설정: ±4g FSR, 100Hz ODR */
+    ret = imu_write_reg(REG_ACCEL_CONFIG0, 0x29);
+    if (ret) { DBG_PRINTF("[IMU] FAIL — accel config write (ret=%d)\r\n", ret); return -1; }
+
+    /* 전원 ON: accel(저잡음) + gyro(저잡음) 모두 활성화 */
+    ret = imu_write_reg(REG_PWR_MGMT0, 0x0F);
+    if (ret) { DBG_PRINTF("[IMU] FAIL — power on write (ret=%d)\r\n", ret); return -1; }
+
+    /* 자이로 스타트업 대기 */
+    k_msleep(IMU_GYRO_STARTUP_MS);
+
+    /* ACCEL_DATA_X1(0x0B)부터 12바이트 연속 읽기
+     * [0..5]  = accel X,Y,Z (MSB first)
+     * [6..11] = gyro  X,Y,Z (MSB first) */
+    ret = imu_read_regs(REG_ACCEL_DATA_X1, raw, 12);
+    if (ret) { DBG_PRINTF("[IMU] FAIL — data read (ret=%d)\r\n", ret); return -2; }
+
+    /* 빅엔디안 → int16_t 변환 */
+    accel[0] = (int16_t)((raw[0]  << 8) | raw[1]);
+    accel[1] = (int16_t)((raw[2]  << 8) | raw[3]);
+    accel[2] = (int16_t)((raw[4]  << 8) | raw[5]);
+    gyro[0]  = (int16_t)((raw[6]  << 8) | raw[7]);
+    gyro[1]  = (int16_t)((raw[8]  << 8) | raw[9]);
+    gyro[2]  = (int16_t)((raw[10] << 8) | raw[11]);
+
+    DBG_PRINTF("[IMU] msp: A=(%6d,%6d,%6d) G=(%6d,%6d,%6d)\r\n",
+               accel[0], accel[1], accel[2],
+               gyro[0],  gyro[1],  gyro[2]);
+
+    /* 슬립 모드: 전력 절감 */
+    imu_write_reg(REG_PWR_MGMT0, 0x00);
+
+    return 0;
+}