jh.chun/VesiScan-Basic_Zephyr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IMU FIFO

Browse Source 
- Zephyr 기반 제어 방식
This commit is contained in:
jhchun
2026-06-12 16:33:22 +09:00
parent c6f05fa802
commit 1bc73fa2b8
2 changed files with 415 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/imu_i2c.c
+379
View File
@@ -19,6 +19,9 @@
#include <zephyr/kernel.h>
#include <zephyr/drivers/i2c.h>
#include <zephyr/devicetree.h>
#include <zephyr/sys/util.h>
#include <errno.h>
#include <string.h>
#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, &reg, 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;
}
src/drivers/imu/imu_i2c.h
+36
View File
@@ -16,6 +16,9 @@
#include <stdint.h>
#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__ */