From e04a7af6c915dabd4a5631afaa7e4aab699daefb Mon Sep 17 00:00:00 2001 From: jhchun Date: Fri, 12 Jun 2026 17:33:43 +0900 Subject: [PATCH] =?UTF-8?q?mbb=3F=20=EC=9D=91=EB=8B=B5=20=EC=8B=9C=20TMP23?= =?UTF-8?q?5=20=E2=86=92=20IMU=20Temp=20=EB=B3=80=EA=B2=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/drivers/imu/imu_i2c.c | 203 +++++++++++++++++++++++++++++++++++++- src/drivers/imu/imu_i2c.h | 22 +++++ src/parser.c | 14 +-- 3 files changed, 225 insertions(+), 14 deletions(-) diff --git a/src/drivers/imu/imu_i2c.c b/src/drivers/imu/imu_i2c.c index e0f8378..503bf01 100644 --- a/src/drivers/imu/imu_i2c.c +++ b/src/drivers/imu/imu_i2c.c @@ -21,6 +21,7 @@ #include #include #include +#include #include #include "imu_i2c.h" @@ -52,6 +53,7 @@ #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 */ @@ -60,6 +62,7 @@ /* 자이로 스타트업 대기 시간 (ms) — 스펙 최소 45ms, 80ms로 여유 확보 */ #define IMU_GYRO_STARTUP_MS 80 +#define IMU_TEMP_AVG_SAMPLES 4U /* FIFO 설정값: 50Hz, ±4g, ±500dps, Low Noise, 16Hz 필터 */ #define IMU_CFG_50HZ_4G 0x4A @@ -73,7 +76,12 @@ #define IMU_FIFO_CONFIG1_STREAM 0x00 #define IMU_FIFO_CONFIG1_BYPASS 0x01 -#define IMU_FIFO_INTF_RECORD_LITTLE_ENDIAN 0x40 +#define IMU_INTF_FIFO_COUNT_FORMAT_MASK BIT(6) +#define IMU_INTF_FIFO_COUNT_ENDIAN_MASK BIT(5) +#define IMU_INTF_SENSOR_DATA_ENDIAN_MASK BIT(4) +#define IMU_INTF_FIFO_COUNT_RECORD BIT(6) +#define IMU_INTF_FIFO_COUNT_LITTLE_ENDIAN 0x00 +#define IMU_INTF_SENSOR_DATA_BIG_ENDIAN BIT(4) #define IMU_FIFO_CONFIG5_MREG1 0x01 #define IMU_TMST_CONFIG1_MREG1 0x00 #define IMU_FIFO_CONFIG5_ACCEL_GYRO_TMST 0x27 @@ -106,6 +114,42 @@ 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 int16_t imu_temp_raw_to_cdeg(int16_t temp_raw) +{ + int32_t cdeg = 2500 + (((int32_t)temp_raw * 100) / 128); + + if (cdeg > INT16_MAX) + { + cdeg = INT16_MAX; + } + else if (cdeg < INT16_MIN) + { + cdeg = INT16_MIN; + } + + return (int16_t)cdeg; +} + +static int imu_read_temp_raw(int16_t *temp_raw) +{ + uint8_t raw[2]; + int ret; + + if (temp_raw == NULL) + { + return -EINVAL; + } + + ret = imu_read_regs(REG_TEMP_DATA1, raw, sizeof(raw)); + if (ret) + { + return ret; + } + + *temp_raw = (int16_t)(((uint16_t)raw[0] << 8) | raw[1]); + return 0; +} + static int imu_update_reg(uint8_t reg, uint8_t clear_mask, uint8_t set_mask) { uint8_t val; @@ -166,6 +210,24 @@ static int imu_mreg_write(uint8_t addr, uint8_t val) return ret; } +static int imu_set_data_big_endian(void) +{ + return imu_update_reg(REG_INTF_CONFIG0, + IMU_INTF_SENSOR_DATA_ENDIAN_MASK, + IMU_INTF_SENSOR_DATA_BIG_ENDIAN); +} + +static int imu_configure_fifo_count_format(void) +{ + return imu_update_reg(REG_INTF_CONFIG0, + IMU_INTF_FIFO_COUNT_FORMAT_MASK | + IMU_INTF_FIFO_COUNT_ENDIAN_MASK | + IMU_INTF_SENSOR_DATA_ENDIAN_MASK, + IMU_INTF_FIFO_COUNT_RECORD | + IMU_INTF_FIFO_COUNT_LITTLE_ENDIAN | + IMU_INTF_SENSOR_DATA_BIG_ENDIAN); +} + static int imu_fifo_reset(void) { uint8_t val = IMU_FIFO_FLUSH_MASK; @@ -357,6 +419,9 @@ int imu_read(int16_t accel[3], int16_t gyro[3]) 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(저잡음) 모두 활성화 */ ret = imu_write_reg(REG_PWR_MGMT0, 0x0F); if (ret) { DBG_PRINTF("[IMU] FAIL — power on write (ret=%d)\r\n", ret); return -1; } @@ -388,6 +453,140 @@ int imu_read(int16_t accel[3], int16_t gyro[3]) return 0; } +int imu_read_with_temperature(int16_t accel[3], int16_t gyro[3], int16_t *temp_cdeg) +{ + uint8_t raw[14]; + int16_t temp_raw; + int16_t temp_samples[IMU_TEMP_AVG_SAMPLES]; + int32_t temp_sum; + int ret; + + if ((accel == NULL) || (gyro == NULL) || (temp_cdeg == NULL)) + { + return -EINVAL; + } + + ret = imu_write_reg(REG_GYRO_CONFIG0, 0x09); + if (ret) { DBG_PRINTF("[IMU] FAIL — gyro config write (ret=%d)\r\n", ret); return -1; } + + 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; } + + 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); + + ret = imu_read_regs(REG_TEMP_DATA1, raw, sizeof(raw)); + if (ret) { DBG_PRINTF("[IMU] FAIL — temp/data read (ret=%d)\r\n", ret); return -2; } + + temp_raw = (int16_t)(((uint16_t)raw[0] << 8) | raw[1]); + temp_sum = temp_raw; + temp_samples[0] = temp_raw; + for (uint8_t i = 1U; i < IMU_TEMP_AVG_SAMPLES; i++) + { + int16_t next_temp_raw; + + ret = imu_read_temp_raw(&next_temp_raw); + if (ret) + { + DBG_PRINTF("[IMU] temp avg read fail ret=%d idx=%u\r\n", ret, i); + return -3; + } + temp_samples[i] = next_temp_raw; + temp_sum += next_temp_raw; + } + + temp_raw = (int16_t)(temp_sum / (int32_t)IMU_TEMP_AVG_SAMPLES); + *temp_cdeg = imu_temp_raw_to_cdeg(temp_raw); + accel[0] = (int16_t)((raw[2] << 8) | raw[3]); + accel[1] = (int16_t)((raw[4] << 8) | raw[5]); + accel[2] = (int16_t)((raw[6] << 8) | raw[7]); + gyro[0] = (int16_t)((raw[8] << 8) | raw[9]); + gyro[1] = (int16_t)((raw[10] << 8) | raw[11]); + gyro[2] = (int16_t)((raw[12] << 8) | raw[13]); + + DBG_PRINTF("[IMU] mbb: A=(%6d,%6d,%6d) G=(%6d,%6d,%6d) T=%d.%02d C temp_raw=%d/%d/%d/%d avg=%d first=0x%02X%02X\r\n", + accel[0], accel[1], accel[2], + gyro[0], gyro[1], gyro[2], + *temp_cdeg / 100, + (*temp_cdeg < 0 ? -*temp_cdeg : *temp_cdeg) % 100, + temp_samples[0], + temp_samples[1], + temp_samples[2], + temp_samples[3], + temp_raw, + raw[0], raw[1]); + + imu_write_reg(REG_PWR_MGMT0, 0x00); + return 0; +} + +int imu_read_temperature_cdeg(int16_t *temp_cdeg) +{ + int16_t temp_raw; + int32_t temp_sum = 0; + int32_t cdeg; + int ret; + bool was_fifo_active = fifo_active; + + if (temp_cdeg == NULL) + { + return -EINVAL; + } + + ret = imu_init(); + if (ret) + { + return ret; + } + + if (!was_fifo_active) + { + ret = imu_write_reg(REG_PWR_MGMT0, IMU_PWR_ACCEL_GYRO_LN); + if (ret) + { + DBG_PRINTF("[IMU] temp power on fail ret=%d\r\n", ret); + return ret; + } + k_msleep(IMU_GYRO_STARTUP_MS); + } + + for (uint8_t i = 0U; i < IMU_TEMP_AVG_SAMPLES; i++) + { + ret = imu_read_temp_raw(&temp_raw); + if (ret) + { + DBG_PRINTF("[IMU] temp read fail ret=%d idx=%u\r\n", ret, i); + if (!was_fifo_active) + { + (void)imu_write_reg(REG_PWR_MGMT0, 0x00); + } + return ret; + } + + temp_sum += temp_raw; + } + + temp_raw = (int16_t)(temp_sum / (int32_t)IMU_TEMP_AVG_SAMPLES); + cdeg = imu_temp_raw_to_cdeg(temp_raw); + *temp_cdeg = (int16_t)cdeg; + DBG_PRINTF("[IMU] temp=%d.%02d C raw=%d\r\n", + *temp_cdeg / 100, + (*temp_cdeg < 0 ? -*temp_cdeg : *temp_cdeg) % 100, + temp_raw); + + if (!was_fifo_active) + { + (void)imu_write_reg(REG_PWR_MGMT0, 0x00); + } + + return 0; +} + int imu_fifo_start(void) { int ret = imu_init(); @@ -408,7 +607,7 @@ int imu_fifo_start(void) 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_configure_fifo_count_format(); 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); diff --git a/src/drivers/imu/imu_i2c.h b/src/drivers/imu/imu_i2c.h index a1a94d2..c1853d1 100644 --- a/src/drivers/imu/imu_i2c.h +++ b/src/drivers/imu/imu_i2c.h @@ -36,6 +36,28 @@ int imu_init(void); */ int imu_read(int16_t accel[3], int16_t gyro[3]); +/** + * @brief IMU 6축 + 내부 온도 1회 측정 + * + * 센서 설정 → 전원 ON → 80ms 대기 → temp/accel/gyro 14바이트 읽기 → 전원 OFF + * + * @param accel 가속도 XYZ + * @param gyro 자이로 XYZ + * @param temp_cdeg 섭씨 x100 출력 + * @return 0 성공, 음수 에러 + */ +int imu_read_with_temperature(int16_t accel[3], int16_t gyro[3], int16_t *temp_cdeg); + +/** + * @brief IMU 내부 온도 1회 측정 + * + * TEMP_DATA1/2를 읽어 섭씨 x100 정수로 변환한다. + * + * @param temp_cdeg 섭씨 x100 출력 + * @return 0 성공, 음수 에러 + */ +int imu_read_temperature_cdeg(int16_t *temp_cdeg); + /** * @brief IMU FIFO 캡처 시작 * diff --git a/src/parser.c b/src/parser.c index cac656c..0e1bff0 100644 --- a/src/parser.c +++ b/src/parser.c @@ -1046,23 +1046,13 @@ static int cmd_mbb(const uint8_t *data, uint8_t data_len) if (status == ECHO_STATUS_OK) { - DBG_PRINTF("[MBB] imu read\r\n"); - if (imu_read(accel, gyro) != 0) + DBG_PRINTF("[MBB] imu/temp read\r\n"); + if (imu_read_with_temperature(accel, gyro, &temp_cdeg) != 0) { status = ECHO_STATUS_IMU; } } - if (status == ECHO_STATUS_OK) - { - DBG_PRINTF("[MBB] temp read\r\n"); - temp_cdeg = temp_read_cdeg(); - if (temp_cdeg == INT16_MIN) - { - status = ECHO_STATUS_TEMP; - } - } - if (status == ECHO_STATUS_OK) { /* rbb: 센서(배터리+IMU+온도) 정보 패킷 */