Initial Commit
This commit is contained in:
+36
-49
@@ -1,16 +1,7 @@
|
||||
/*******************************************************************************
|
||||
* @file imu_i2c.c
|
||||
* @brief ICM42670P IMU Driver (Zephyr I2C API)
|
||||
*
|
||||
* 동작 방식 (매 측정 시):
|
||||
* 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에서 설정)
|
||||
******************************************************************************/
|
||||
|
||||
@@ -29,34 +20,34 @@
|
||||
* 디바이스트리 / I2C 설정
|
||||
*============================================================================*/
|
||||
#define IMU_I2C_NODE DT_NODELABEL(i2c0)
|
||||
#define IMU_I2C_ADDR 0x68 /* ICM42670P 기본 I2C 주소 (AD0=LOW) */
|
||||
#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_GYRO_CONFIG1 0x23 /* 자이로 LN 필터 BW */
|
||||
#define REG_ACCEL_CONFIG1 0x24 /* 가속도 LN 필터 BW */
|
||||
#define REG_FIFO_CONFIG1 0x28 /* FIFO bypass/mode */
|
||||
#define REG_FIFO_CONFIG2 0x29 /* FIFO watermark LSB */
|
||||
#define REG_FIFO_CONFIG3 0x2A /* FIFO watermark MSB */
|
||||
#define REG_INTF_CONFIG0 0x35 /* FIFO count/endian 설정 */
|
||||
#define REG_FIFO_COUNTH 0x3D /* FIFO count high */
|
||||
#define REG_FIFO_DATA 0x3F /* FIFO data pop */
|
||||
#define REG_BLK_SEL_W 0x79 /* MREG write block */
|
||||
#define REG_MADDR_W 0x7A /* MREG write addr */
|
||||
#define REG_M_W 0x7B /* MREG write data */
|
||||
#define REG_BLK_SEL_R 0x7C /* MREG read block */
|
||||
#define REG_MADDR_R 0x7D /* MREG read addr */
|
||||
#define REG_M_R 0x7E /* MREG read data */
|
||||
#define REG_TEMP_DATA1 0x09 /* IMU 내부 온도 상위 바이트 */
|
||||
#define REG_ACCEL_DATA_X1 0x0B /* 가속도 X축 상위 바이트 (시작 레지스터) */
|
||||
#define REG_WHO_AM_I 0x75 /* WHOAMI 레지스터 */
|
||||
#define REG_MCLK_RDY 0x00 /* MCLK ready */
|
||||
#define REG_SIGNAL_PATH_RESET 0x02 /* FIFO flush */
|
||||
#define ICM_WHOAMI 0x67 /* ICM42670P 식별값 */
|
||||
#define REG_PWR_MGMT0 0x1F // 전원 관리: accel/gyro 동작 모드
|
||||
#define REG_GYRO_CONFIG0 0x20 // 자이로 FSR + ODR 설정
|
||||
#define REG_ACCEL_CONFIG0 0x21 // 가속도 FSR + ODR 설정
|
||||
#define REG_GYRO_CONFIG1 0x23 // 자이로 LN 필터 BW
|
||||
#define REG_ACCEL_CONFIG1 0x24 // 가속도 LN 필터 BW
|
||||
#define REG_FIFO_CONFIG1 0x28 // FIFO bypass/mode
|
||||
#define REG_FIFO_CONFIG2 0x29 // FIFO watermark LSB
|
||||
#define REG_FIFO_CONFIG3 0x2A // FIFO watermark MSB
|
||||
#define REG_INTF_CONFIG0 0x35 // FIFO count/endian 설정
|
||||
#define REG_FIFO_COUNTH 0x3D // FIFO count high
|
||||
#define REG_FIFO_DATA 0x3F // FIFO data pop
|
||||
#define REG_BLK_SEL_W 0x79 // MREG write block
|
||||
#define REG_MADDR_W 0x7A // MREG write addr
|
||||
#define REG_M_W 0x7B // MREG write data
|
||||
#define REG_BLK_SEL_R 0x7C // MREG read block
|
||||
#define REG_MADDR_R 0x7D // MREG read addr
|
||||
#define REG_M_R 0x7E // MREG read data
|
||||
#define REG_TEMP_DATA1 0x09 // IMU 내부 온도 상위 바이트
|
||||
#define REG_ACCEL_DATA_X1 0x0B // 가속도 X축 상위 바이트 (시작 레지스터)
|
||||
#define REG_WHO_AM_I 0x75 // WHOAMI 레지스터
|
||||
#define REG_MCLK_RDY 0x00 // MCLK ready
|
||||
#define REG_SIGNAL_PATH_RESET 0x02 // FIFO flush
|
||||
#define ICM_WHOAMI 0x67 // ICM42670P 식별값
|
||||
|
||||
/* 자이로 스타트업 대기 시간 (ms) — 스펙 최소 45ms, 80ms로 여유 확보 */
|
||||
#define IMU_GYRO_STARTUP_MS 80
|
||||
@@ -95,9 +86,7 @@ static const struct device *i2c_bus;
|
||||
static bool fifo_active;
|
||||
static uint8_t fifo_raw[IMU_FIFO_PACKET_BYTES * IMU_FIFO_MAX_RECORDS];
|
||||
|
||||
/*==============================================================================
|
||||
* 내부 I2C 래퍼
|
||||
*============================================================================*/
|
||||
/* 내부 I2C 래퍼 */
|
||||
|
||||
/* 레지스터 1바이트 쓰기: [reg, val] 2바이트 TX */
|
||||
static int imu_write_reg(uint8_t reg, uint8_t val)
|
||||
@@ -375,9 +364,7 @@ static uint16_t imu_fifo_compact_records(uint16_t record_count)
|
||||
return write_idx;
|
||||
}
|
||||
|
||||
/*==============================================================================
|
||||
* 공개 API
|
||||
*============================================================================*/
|
||||
/* 공개 API */
|
||||
|
||||
int imu_init(void)
|
||||
{
|
||||
@@ -407,31 +394,31 @@ int imu_read(int16_t accel[3], int16_t gyro[3])
|
||||
uint8_t raw[12];
|
||||
int ret;
|
||||
|
||||
/* 자이로 설정: ±2000dps FSR, 100Hz ODR */
|
||||
// 자이로 설정: ±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 */
|
||||
// 가속도 설정: ±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; }
|
||||
|
||||
ret = imu_set_data_big_endian();
|
||||
if (ret) { DBG_PRINTF("[IMU] FAIL — endian config write (ret=%d)\r\n", ret); return -1; }
|
||||
|
||||
/* 전원 ON: accel(저잡음) + gyro(저잡음) 모두 활성화 */
|
||||
// 전원 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) */
|
||||
// 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 변환 */
|
||||
// 빅엔디안 → 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]);
|
||||
@@ -441,7 +428,7 @@ int imu_read(int16_t accel[3], int16_t gyro[3])
|
||||
|
||||
//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;
|
||||
|
||||
Reference in New Issue
Block a user