diff --git a/src/drivers/imu/imu_i2c.c b/src/drivers/imu/imu_i2c.c index f609fb6..e0f8378 100644 --- a/src/drivers/imu/imu_i2c.c +++ b/src/drivers/imu/imu_i2c.c @@ -19,6 +19,9 @@ #include #include #include +#include +#include +#include #include "imu_i2c.h" #include "debug_print.h" @@ -35,14 +38,56 @@ #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_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 +/* FIFO 설정값: 50Hz, ±4g, ±500dps, Low Noise, 16Hz 필터 */ +#define IMU_CFG_50HZ_4G 0x4A +#define IMU_CFG_50HZ_500DPS 0x4A +#define IMU_CFG_LN_BW_16HZ 0x07 +#define IMU_PWR_ACCEL_GYRO_LN 0x0F + +#define IMU_PWR_IDLE_MASK BIT(4) +#define IMU_MCLK_RDY_MASK BIT(3) +#define IMU_FIFO_FLUSH_MASK BIT(2) + +#define IMU_FIFO_CONFIG1_STREAM 0x00 +#define IMU_FIFO_CONFIG1_BYPASS 0x01 +#define IMU_FIFO_INTF_RECORD_LITTLE_ENDIAN 0x40 +#define IMU_FIFO_CONFIG5_MREG1 0x01 +#define IMU_TMST_CONFIG1_MREG1 0x00 +#define IMU_FIFO_CONFIG5_ACCEL_GYRO_TMST 0x27 +#define IMU_FIFO_PACKET_BYTES 16U +#define IMU_FIFO_MAX_RECORDS 258U +#define IMU_FIFO_READ_BURST_RECORDS 14U +#define IMU_FIFO_ACCEL_OFFSET 1U +#define IMU_FIFO_GYRO_OFFSET 7U +#define IMU_FIFO_INVALID_HEADER 0x80 +#define IMU_FIFO_INVALID_AXIS 0x8000 + static const struct device *i2c_bus; +static bool fifo_active; +static uint8_t fifo_raw[IMU_FIFO_PACKET_BYTES * IMU_FIFO_MAX_RECORDS]; /*============================================================================== * 내부 I2C 래퍼 @@ -61,6 +106,215 @@ static int imu_read_regs(uint8_t reg, uint8_t *data, uint8_t len) return i2c_write_read(i2c_bus, IMU_I2C_ADDR, ®, 1, data, len); } +static int imu_update_reg(uint8_t reg, uint8_t clear_mask, uint8_t set_mask) +{ + uint8_t val; + int ret = imu_read_regs(reg, &val, 1); + + if (ret) + { + return ret; + } + + val = (uint8_t)((val & ~clear_mask) | set_mask); + return imu_write_reg(reg, val); +} + +static int imu_mclk_on(void) +{ + uint8_t val = 0; + int ret = imu_update_reg(REG_PWR_MGMT0, 0U, IMU_PWR_IDLE_MASK); + + if (ret) + { + return ret; + } + + for (uint16_t i = 0; i < 1000U; i++) + { + ret = imu_read_regs(REG_MCLK_RDY, &val, 1); + if ((ret == 0) && ((val & IMU_MCLK_RDY_MASK) != 0U)) + { + return 0; + } + k_busy_wait(10); + } + + return -ETIMEDOUT; +} + +static int imu_mclk_off(void) +{ + return imu_update_reg(REG_PWR_MGMT0, IMU_PWR_IDLE_MASK, 0U); +} + +static int imu_mreg_write(uint8_t addr, uint8_t val) +{ + int ret = imu_mclk_on(); + + if (ret) + { + return ret; + } + + ret = imu_write_reg(REG_BLK_SEL_W, 0U); + ret |= imu_write_reg(REG_MADDR_W, addr); + ret |= imu_write_reg(REG_M_W, val); + k_busy_wait(10); + ret |= imu_write_reg(REG_BLK_SEL_W, 0U); + ret |= imu_mclk_off(); + return ret; +} + +static int imu_fifo_reset(void) +{ + uint8_t val = IMU_FIFO_FLUSH_MASK; + int ret = imu_mclk_on(); + + if (ret) + { + return ret; + } + + ret = imu_write_reg(REG_SIGNAL_PATH_RESET, val); + for (uint16_t i = 0; (ret == 0) && (i < 1000U); i++) + { + ret = imu_read_regs(REG_SIGNAL_PATH_RESET, &val, 1); + if ((val & IMU_FIFO_FLUSH_MASK) == 0U) + { + break; + } + k_busy_wait(10); + } + + ret |= imu_mclk_off(); + return ret; +} + +static int imu_fifo_read_count(uint16_t *count) +{ + uint8_t raw[2] = {0}; + int ret; + + if (count == NULL) + { + return -EINVAL; + } + + ret = imu_mclk_on(); + if (ret) + { + return ret; + } + + ret = imu_read_regs(REG_FIFO_COUNTH, raw, sizeof(raw)); + ret |= imu_mclk_off(); + + if (ret) + { + return ret; + } + + *count = (uint16_t)raw[0] | ((uint16_t)raw[1] << 8); + if (*count > IMU_FIFO_MAX_RECORDS) + { + *count = IMU_FIFO_MAX_RECORDS; + } + + return 0; +} + +static int imu_fifo_read_records(uint16_t record_count) +{ + uint16_t record_idx = 0; + int ret = imu_mclk_on(); + + if (ret) + { + return ret; + } + + while ((record_idx < record_count) && (ret == 0)) + { + uint16_t burst_records = record_count - record_idx; + uint16_t burst_bytes; + + if (burst_records > IMU_FIFO_READ_BURST_RECORDS) + { + burst_records = IMU_FIFO_READ_BURST_RECORDS; + } + + burst_bytes = burst_records * IMU_FIFO_PACKET_BYTES; + ret = imu_read_regs(REG_FIFO_DATA, + &fifo_raw[record_idx * IMU_FIFO_PACKET_BYTES], + (uint8_t)burst_bytes); + record_idx = (uint16_t)(record_idx + burst_records); + } + + ret |= imu_mclk_off(); + return ret; +} + +static bool imu_fifo_record_is_placeholder(const uint8_t *rec) +{ + if (rec[0] != IMU_FIFO_INVALID_HEADER) + { + return false; + } + + for (uint8_t i = 1U; i < IMU_FIFO_PACKET_BYTES; i++) + { + if (rec[i] != 0U) + { + return false; + } + } + + return true; +} + +static int16_t imu_fifo_axis_be(const uint8_t *rec, uint8_t offset) +{ + return (int16_t)(((uint16_t)rec[offset] << 8) | rec[offset + 1U]); +} + +static bool imu_fifo_record_has_valid_gyro(const uint8_t *rec) +{ + for (uint8_t axis = 0U; axis < 3U; axis++) + { + if (imu_fifo_axis_be(rec, (uint8_t)(IMU_FIFO_GYRO_OFFSET + axis * 2U)) != + (int16_t)IMU_FIFO_INVALID_AXIS) + { + return true; + } + } + + return false; +} + +static uint16_t imu_fifo_compact_records(uint16_t record_count) +{ + uint16_t write_idx = 0; + + for (uint16_t read_idx = 0; read_idx < record_count; read_idx++) + { + uint8_t *rec = &fifo_raw[read_idx * IMU_FIFO_PACKET_BYTES]; + + if (imu_fifo_record_is_placeholder(rec) || !imu_fifo_record_has_valid_gyro(rec)) + { + continue; + } + + if (write_idx != read_idx) + { + memcpy(&fifo_raw[write_idx * IMU_FIFO_PACKET_BYTES], rec, IMU_FIFO_PACKET_BYTES); + } + write_idx++; + } + + return write_idx; +} + /*============================================================================== * 공개 API *============================================================================*/ @@ -133,3 +387,128 @@ int imu_read(int16_t accel[3], int16_t gyro[3]) return 0; } + +int imu_fifo_start(void) +{ + int ret = imu_init(); + + if (ret) + { + return ret; + } + + if (fifo_active) + { + DBG_PRINTF("[IMU FIFO] already running\r\n"); + return 0; + } + + ret = imu_write_reg(REG_PWR_MGMT0, 0x00); + ret |= imu_write_reg(REG_GYRO_CONFIG0, IMU_CFG_50HZ_500DPS); + ret |= imu_write_reg(REG_ACCEL_CONFIG0, IMU_CFG_50HZ_4G); + ret |= imu_write_reg(REG_GYRO_CONFIG1, IMU_CFG_LN_BW_16HZ); + ret |= imu_write_reg(REG_ACCEL_CONFIG1, IMU_CFG_LN_BW_16HZ); + ret |= imu_write_reg(REG_INTF_CONFIG0, IMU_FIFO_INTF_RECORD_LITTLE_ENDIAN); + ret |= imu_write_reg(REG_FIFO_CONFIG1, IMU_FIFO_CONFIG1_STREAM); + ret |= imu_write_reg(REG_FIFO_CONFIG2, 0x01); + ret |= imu_write_reg(REG_FIFO_CONFIG3, 0x00); + ret |= imu_mreg_write(IMU_TMST_CONFIG1_MREG1, 0x01); + ret |= imu_mreg_write(IMU_FIFO_CONFIG5_MREG1, IMU_FIFO_CONFIG5_ACCEL_GYRO_TMST); + ret |= imu_fifo_reset(); + ret |= imu_write_reg(REG_PWR_MGMT0, IMU_PWR_ACCEL_GYRO_LN); + + k_msleep(IMU_GYRO_STARTUP_MS); + ret |= imu_fifo_reset(); + + if (ret) + { + DBG_PRINTF("[IMU FIFO] start fail ret=%d\r\n", ret); + (void)imu_write_reg(REG_FIFO_CONFIG1, IMU_FIFO_CONFIG1_BYPASS); + (void)imu_write_reg(REG_PWR_MGMT0, 0x00); + return ret; + } + + fifo_active = true; + DBG_PRINTF("[IMU FIFO] start 50Hz LN\r\n"); + return 0; +} + +int imu_fifo_read_latest(uint8_t *sample_bytes, + uint16_t max_samples, + uint16_t *out_count) +{ + uint16_t record_count = 0; + uint16_t start_idx = 0; + int ret; + + if ((sample_bytes == NULL) || (out_count == NULL)) + { + return -EINVAL; + } + + *out_count = 0U; + + if (!fifo_active) + { + return -EALREADY; + } + + ret = imu_fifo_read_count(&record_count); + DBG_PRINTF("[IMU FIFO] raw count=%u ret=%d target=%u\r\n", + record_count, + ret, + max_samples); + if ((ret == 0) && (record_count > 0U)) + { + ret = imu_fifo_read_records(record_count); + DBG_PRINTF("[IMU FIFO] read records ret=%d\r\n", ret); + } + + if (ret) + { + DBG_PRINTF("[IMU FIFO] drain fail ret=%d count=%u\r\n", ret, record_count); + return ret; + } + + { + uint16_t raw_count = record_count; + record_count = imu_fifo_compact_records(record_count); + DBG_PRINTF("[IMU FIFO] compact %u -> %u\r\n", raw_count, record_count); + } + if (record_count > max_samples) + { + start_idx = (uint16_t)(record_count - max_samples); + record_count = max_samples; + } + + for (uint16_t i = 0; i < record_count; i++) + { + const uint8_t *rec = &fifo_raw[(start_idx + i) * IMU_FIFO_PACKET_BYTES]; + uint8_t *dst = &sample_bytes[i * IMU_FIFO_SAMPLE_BYTES]; + + memcpy(dst, &rec[IMU_FIFO_ACCEL_OFFSET], 6U); + memcpy(&dst[6], &rec[IMU_FIFO_GYRO_OFFSET], 6U); + } + + *out_count = record_count; + DBG_PRINTF("[IMU FIFO] drained samples=%u\r\n", record_count); + return 0; +} + +int imu_fifo_stop(void) +{ + int ret = 0; + + if (!fifo_active) + { + return 0; + } + + ret |= imu_write_reg(REG_FIFO_CONFIG1, IMU_FIFO_CONFIG1_BYPASS); + ret |= imu_mreg_write(IMU_FIFO_CONFIG5_MREG1, 0x00); + ret |= imu_write_reg(REG_PWR_MGMT0, 0x00); + fifo_active = false; + + DBG_PRINTF("[IMU FIFO] stop ret=%d\r\n", ret); + return ret; +} diff --git a/src/drivers/imu/imu_i2c.h b/src/drivers/imu/imu_i2c.h index 2dd6a7c..a1a94d2 100644 --- a/src/drivers/imu/imu_i2c.h +++ b/src/drivers/imu/imu_i2c.h @@ -16,6 +16,9 @@ #include +#define IMU_FIFO_SAMPLE_BYTES 12U +#define IMU_FIFO_RIM_TARGET_SAMPLES 15U + /** * @brief IMU 초기화 — I2C 버스 준비 및 WHOAMI 확인 * @return 0 성공, 음수 에러 @@ -33,4 +36,37 @@ int imu_init(void); */ int imu_read(int16_t accel[3], int16_t gyro[3]); +/** + * @brief IMU FIFO 캡처 시작 + * + * Accel/Gyro 50Hz, Low Noise, FIFO stream 모드로 설정하고 FIFO를 비운다. + * + * @return 0 성공, 음수 에러 + */ +int imu_fifo_start(void); + +/** + * @brief 실행 중인 FIFO에서 최신 샘플 읽기 + * + * 각 샘플은 accel XYZ 6B + gyro XYZ 6B, 총 12B이다. + * 함수 종료 후에도 FIFO와 accel/gyro는 계속 동작한다. + * + * @param sample_bytes 12B 샘플이 연속 저장될 버퍼 + * @param max_samples 버퍼에 담을 최대 샘플 수 + * @param out_count 실제 저장한 샘플 수 + * @return 0 성공, 음수 에러 + */ +int imu_fifo_read_latest(uint8_t *sample_bytes, + uint16_t max_samples, + uint16_t *out_count); + +/** + * @brief FIFO와 accel/gyro 전원 끄기 + * + * 켜져 있지 않으면 아무 작업 없이 성공 처리한다. + * + * @return 0 성공, 음수 에러 + */ +int imu_fifo_stop(void); + #endif /* IMU_I2C_H__ */