mbb? 응답 시 TMP235 → IMU Temp 변경

src/drivers/imu/imu_i2c.c

@@ -21,6 +21,7 @@
 #include <zephyr/devicetree.h>
 #include <zephyr/sys/util.h>
 #include <errno.h>
+#include <limits.h>
 #include <string.h>
 
 #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, &reg, 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);

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 캡처 시작
  *

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");
+        DBG_PRINTF("[MBB] imu/temp read\r\n");
-        if (imu_read(accel, gyro) != 0)
+        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+온도) 정보 패킷 */