IMU 측정, 커맨드 및 응답 추가
This commit is contained in:
geniushyun
@@ -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
|
||||
)
|
||||
|
||||
@@ -8,10 +8,14 @@
|
||||
* 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 <zephyr/dt-bindings/adc/adc.h>
|
||||
#include <zephyr/dt-bindings/adc/nrf-saadc.h>
|
||||
/* nRF 핀 컨트롤 매크로 (NRF_PSEL - I2C/SPI 등 핀 재배치에 필요) */
|
||||
#include <zephyr/dt-bindings/pinctrl/nrf-pinctrl.h>
|
||||
|
||||
/* 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 = <NRF_PSEL(TWIM_SCL, 1, 14)>,
|
||||
<NRF_PSEL(TWIM_SDA, 1, 15)>;
|
||||
};
|
||||
};
|
||||
|
||||
&i2c0_sleep {
|
||||
group1 {
|
||||
psels = <NRF_PSEL(TWIM_SCL, 1, 14)>,
|
||||
<NRF_PSEL(TWIM_SDA, 1, 15)>;
|
||||
low-power-enable;
|
||||
};
|
||||
};
|
||||
|
||||
&i2c0 {
|
||||
status = "okay";
|
||||
clock-frequency = <I2C_BITRATE_STANDARD>;
|
||||
};
|
||||
|
||||
/* 배터리 ADC 채널 설정 (AIN2 = P0.04) */
|
||||
&adc
|
||||
{
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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 <zephyr/kernel.h>
|
||||
#include <zephyr/drivers/i2c.h>
|
||||
#include <zephyr/devicetree.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
@@ -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 <stdint.h>
|
||||
|
||||
/**
|
||||
* @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__ */
|
||||
@@ -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");
|
||||
|
||||
@@ -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]))
|
||||
|
||||
Reference in New Issue
Block a user