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

mbb? 응답 시 TMP235 → IMU Temp 변경

Browse Source
This commit is contained in:
jhchun
2026-06-12 17:33:43 +09:00
parent 1b923c0d37
commit e04a7af6c9
3 changed files with 225 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/imu_i2c.c
+201 -2
View File
@@ -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);