diff --git a/CMakeLists.txt b/CMakeLists.txt index 30aec08..1d1ff73 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ target_include_directories(app PRIVATE src/ble src/drivers/battery src/drivers/led + src/drivers/imu ) target_sources(app PRIVATE @@ -18,4 +19,5 @@ target_sources(app PRIVATE src/ble/ble_service.c src/drivers/battery/battery_adc.c src/drivers/led/led_control.c + src/drivers/imu/imu_i2c.c ) diff --git a/boards/nrf52840dk_nrf52840.overlay b/boards/nrf52840dk_nrf52840.overlay index 1c0fae4..f6b4d80 100644 --- a/boards/nrf52840dk_nrf52840.overlay +++ b/boards/nrf52840dk_nrf52840.overlay @@ -5,13 +5,17 @@ * Pin mapping: * POWER_HOLD: P0.08 (output, active high - latches power) * POWER_BTN: P1.08 (input, active low - pressed = LOW) - * LED_BLE: P0.12 (output, active low) + * LED_BLE: P0.12 (output, active low) * LED_ORANGE: P0.29 (output, active low) * BATT_ADC: P0.04 (AIN2, battery voltage measurement) + * IMU_SCL: P1.14 (I2C0 clock) + * IMU_SDA: P1.15 (I2C0 data) */ #include #include +/* nRF 핀 컨트롤 매크로 (NRF_PSEL - I2C/SPI 등 핀 재배치에 필요) */ +#include /* P0.08이 DK 기본 UART0 RX로 잡혀있어서 GPIO 출력 충돌 → 비활성화 */ &uart0 @@ -19,6 +23,27 @@ status = "disabled"; }; +/* I2C0 핀 재설정: ICM42670P IMU (SCL=P1.14, SDA=P1.15) */ +&i2c0_default { + group1 { + psels = , + ; + }; +}; + +&i2c0_sleep { + group1 { + psels = , + ; + low-power-enable; + }; +}; + +&i2c0 { + status = "okay"; + clock-frequency = ; +}; + /* 배터리 ADC 채널 설정 (AIN2 = P0.04) */ &adc { diff --git a/prj.conf b/prj.conf index d351866..22a5d53 100644 --- a/prj.conf +++ b/prj.conf @@ -47,6 +47,9 @@ CONFIG_BT_SMP=n # ADC (battery, temperature) CONFIG_ADC=y +# I2C (IMU: ICM42670P) +CONFIG_I2C=y + # System CONFIG_HEAP_MEM_POOL_SIZE=2048 CONFIG_SYSTEM_WORKQUEUE_STACK_SIZE=2048 diff --git a/src/drivers/imu/imu_i2c.c b/src/drivers/imu/imu_i2c.c new file mode 100644 index 0000000..f609fb6 --- /dev/null +++ b/src/drivers/imu/imu_i2c.c @@ -0,0 +1,135 @@ +/******************************************************************************* + * @file imu_i2c.c + * @brief ICM42670P IMU 드라이버 (Zephyr I2C API) + * + * 기존 VesiScan-Basic(nRF5 SDK)의 imu_read_direct()를 Zephyr로 포팅. + * + * 동작 방식 (매 측정 시): + * 1) GYRO_CONFIG0 = 0x09 → ±2000dps, 100Hz ODR + * 2) ACCEL_CONFIG0 = 0x29 → ±4g, 100Hz ODR + * 3) PWR_MGMT0 = 0x0F → accel(low-noise) + gyro(low-noise) ON + * 4) 80ms 대기 (자이로 스타트업 최소 45ms + 여유) + * 5) 0x0B부터 12바이트 연속 읽기: accel XYZ(6B) + gyro XYZ(6B) + * 6) 빅엔디안 → int16_t 변환 + * 7) PWR_MGMT0 = 0x00 → 슬립 (전력 절감) + * + * I2C 핀: SCL=P1.14, SDA=P1.15 (overlay에서 설정) + ******************************************************************************/ + +#include +#include +#include + +#include "imu_i2c.h" +#include "debug_print.h" + +/*============================================================================== + * 디바이스트리 / I2C 설정 + *============================================================================*/ +#define IMU_I2C_NODE DT_NODELABEL(i2c0) +#define IMU_I2C_ADDR 0x68 /* ICM42670P 기본 I2C 주소 (AD0=LOW) */ + +/*============================================================================== + * ICM42670P 레지스터 주소 + *============================================================================*/ +#define REG_PWR_MGMT0 0x1F /* 전원 관리: accel/gyro 동작 모드 */ +#define REG_GYRO_CONFIG0 0x20 /* 자이로 FSR + ODR 설정 */ +#define REG_ACCEL_CONFIG0 0x21 /* 가속도 FSR + ODR 설정 */ +#define REG_ACCEL_DATA_X1 0x0B /* 가속도 X축 상위 바이트 (시작 레지스터) */ +#define REG_WHO_AM_I 0x75 /* WHOAMI 레지스터 */ +#define ICM_WHOAMI 0x67 /* ICM42670P 식별값 */ + +/* 자이로 스타트업 대기 시간 (ms) — 스펙 최소 45ms, 80ms로 여유 확보 */ +#define IMU_GYRO_STARTUP_MS 80 + +static const struct device *i2c_bus; + +/*============================================================================== + * 내부 I2C 래퍼 + *============================================================================*/ + +/* 레지스터 1바이트 쓰기: [reg, val] 2바이트 TX */ +static int imu_write_reg(uint8_t reg, uint8_t val) +{ + uint8_t buf[2] = { reg, val }; + return i2c_write(i2c_bus, buf, 2, IMU_I2C_ADDR); +} + +/* 레지스터 연속 읽기: reg 주소 TX → data RX */ +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); +} + +/*============================================================================== + * 공개 API + *============================================================================*/ + +int imu_init(void) +{ + i2c_bus = DEVICE_DT_GET(IMU_I2C_NODE); + if (!device_is_ready(i2c_bus)) { + DBG_PRINTF("[IMU] FAIL — I2C bus not ready\r\n"); + return -1; + } + + uint8_t who_am_i = 0; + if (imu_read_regs(REG_WHO_AM_I, &who_am_i, 1) != 0) { + DBG_PRINTF("[IMU] FAIL — WHOAMI read error (check SCL=P1.14, SDA=P1.15)\r\n"); + return -2; + } + + if (who_am_i != ICM_WHOAMI) { + DBG_PRINTF("[IMU] FAIL — WHOAMI mismatch (got=0x%02X, expected=0x%02X)\r\n", + who_am_i, ICM_WHOAMI); + return -3; + } + + DBG_PRINTF("[IMU] OK — ICM42670P detected (WHOAMI=0x%02X, addr=0x%02X)\r\n", + who_am_i, IMU_I2C_ADDR); + return 0; +} + +int imu_read(int16_t accel[3], int16_t gyro[3]) +{ + uint8_t raw[12]; + int ret; + + /* 자이로 설정: ±2000dps FSR, 100Hz ODR */ + ret = imu_write_reg(REG_GYRO_CONFIG0, 0x09); + if (ret) { DBG_PRINTF("[IMU] FAIL — gyro config write (ret=%d)\r\n", ret); return -1; } + + /* 가속도 설정: ±4g FSR, 100Hz ODR */ + ret = imu_write_reg(REG_ACCEL_CONFIG0, 0x29); + if (ret) { DBG_PRINTF("[IMU] FAIL — accel 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; } + + /* 자이로 스타트업 대기 */ + k_msleep(IMU_GYRO_STARTUP_MS); + + /* ACCEL_DATA_X1(0x0B)부터 12바이트 연속 읽기 + * [0..5] = accel X,Y,Z (MSB first) + * [6..11] = gyro X,Y,Z (MSB first) */ + ret = imu_read_regs(REG_ACCEL_DATA_X1, raw, 12); + if (ret) { DBG_PRINTF("[IMU] FAIL — data read (ret=%d)\r\n", ret); return -2; } + + /* 빅엔디안 → int16_t 변환 */ + accel[0] = (int16_t)((raw[0] << 8) | raw[1]); + accel[1] = (int16_t)((raw[2] << 8) | raw[3]); + accel[2] = (int16_t)((raw[4] << 8) | raw[5]); + gyro[0] = (int16_t)((raw[6] << 8) | raw[7]); + gyro[1] = (int16_t)((raw[8] << 8) | raw[9]); + gyro[2] = (int16_t)((raw[10] << 8) | raw[11]); + + DBG_PRINTF("[IMU] msp: A=(%6d,%6d,%6d) G=(%6d,%6d,%6d)\r\n", + accel[0], accel[1], accel[2], + gyro[0], gyro[1], gyro[2]); + + /* 슬립 모드: 전력 절감 */ + imu_write_reg(REG_PWR_MGMT0, 0x00); + + return 0; +} diff --git a/src/drivers/imu/imu_i2c.h b/src/drivers/imu/imu_i2c.h new file mode 100644 index 0000000..2dd6a7c --- /dev/null +++ b/src/drivers/imu/imu_i2c.h @@ -0,0 +1,36 @@ +/******************************************************************************* + * @file imu_i2c.h + * @brief ICM42670P IMU 드라이버 (Zephyr I2C API) + * + * 기존 VesiScan-Basic(nRF5 SDK)의 imu_read_direct() 방식을 Zephyr로 포팅. + * 드라이버 API 없이 직접 I2C 레지스터 읽기/쓰기 방식 사용. + * + * 핀 배치: + * SCL: P1.14 + * SDA: P1.15 + * I2C 주소: 0x68 + ******************************************************************************/ + +#ifndef IMU_I2C_H__ +#define IMU_I2C_H__ + +#include + +/** + * @brief IMU 초기화 — I2C 버스 준비 및 WHOAMI 확인 + * @return 0 성공, 음수 에러 + */ +int imu_init(void); + +/** + * @brief IMU 1회 측정 + * + * 센서 설정 → 전원 ON → 80ms 대기 → 12바이트 읽기 → 전원 OFF + * + * @param accel 가속도 XYZ (int16, ±4g, 100Hz) + * @param gyro 자이로 XYZ (int16, ±2000dps, 100Hz) + * @return 0 성공, 음수 에러 + */ +int imu_read(int16_t accel[3], int16_t gyro[3]); + +#endif /* IMU_I2C_H__ */ diff --git a/src/main.c b/src/main.c index 0da0a2d..c3f3459 100644 --- a/src/main.c +++ b/src/main.c @@ -23,6 +23,7 @@ #include "ble_service.h" #include "battery_adc.h" #include "parser.h" +#include "imu_i2c.h" LOG_MODULE_REGISTER(vesiscan, LOG_LEVEL_INF); @@ -309,6 +310,7 @@ int main(void) battery_adc_init(); battery_timer_init(); DBG_PRINTF(" gpio/timer/config/led/batt OK\r\n"); + imu_init(); /*── Phase 2: BLE 스택 + NUS ──*/ DBG_PRINTF("[2] BLE Init\r\n"); diff --git a/src/parser.c b/src/parser.c index 7435627..f030202 100644 --- a/src/parser.c +++ b/src/parser.c @@ -16,6 +16,7 @@ #include "ble_service.h" #include "battery_adc.h" #include "led_control.h" +#include "imu_i2c.h" /*============================================================================== * CRC16 (CRC-CCITT, Nordic SDK 호환) @@ -58,6 +59,34 @@ static void send_response_u16(const char *tag, uint16_t value) ble_data_send(buf, 8); } +/*============================================================================== + * 응답 패킷 전송 (IMU) + *============================================================================*/ + +/* TAG(4B) + int16×6 빅엔디안(12B) + CRC16(2B) = 18바이트 전송 + * 기존 format_data() + dr_binary_tx_safe(buf, 8) 방식과 동일한 레이아웃 */ +static void send_response_imu(const int16_t accel[3], const int16_t gyro[3]) +{ + uint8_t buf[18]; + + buf[0] = 'r'; buf[1] = 's'; buf[2] = 'p'; buf[3] = ':'; + + const int16_t vals[6] = { + accel[0], accel[1], accel[2], + gyro[0], gyro[1], gyro[2] + }; + for (int i = 0; i < 6; i++) { + buf[4 + i * 2] = (uint8_t)((uint16_t)vals[i] >> 8); /* MSB */ + buf[4 + i * 2 + 1] = (uint8_t)((uint16_t)vals[i] & 0xFF); /* LSB */ + } + + uint16_t crc = dr_crc16_compute(buf, 16); + buf[16] = (uint8_t)(crc & 0xFF); + buf[17] = (uint8_t)(crc >> 8); + + ble_data_send(buf, 18); +} + /*============================================================================== * 커맨드 핸들러 *============================================================================*/ @@ -77,6 +106,22 @@ static int cmd_msn(const uint8_t *data, uint8_t data_len) return 1; } +/* msp? → IMU 1회 측정 → rsp: + accel XYZ + gyro XYZ (각 int16 빅엔디안) */ +static int cmd_msp(const uint8_t *data, uint8_t data_len) +{ + int16_t accel[3], gyro[3]; + + int ret = imu_read(accel, gyro); + if (ret != 0) { + send_response_u16("rsp:", 0xFFFF); + DBG_PRINTF("[CMD] msp: FAIL (imu_read ret=%d) -> rsp: 0xFFFF\r\n", ret); + return 1; + } + + send_response_imu(accel, gyro); + return 1; +} + /* mls? → LED 상태 변경 → rls: + state echo back * 파라미터: [state(2B LE)] — led_state_t enum 값 * 0=OFF, 4=DETACH_WARNING, 5=ALIGN_SEARCHING, 6=ALIGN_COMPLETE, 7=ERROR @@ -121,6 +166,7 @@ static const cmd_entry_t cmd_table[] = { { "msn?", cmd_msn }, { "mls?", cmd_mls }, + { "msp?", cmd_msp }, }; #define CMD_TABLE_SIZE (sizeof(cmd_table) / sizeof(cmd_table[0]))