코드 정리
- 주석 영문으로 변경 - Allman 스타일로 통일
This commit is contained in:
@@ -7,28 +7,29 @@
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [모듈 개요] ICM42670P IMU 드라이버 상위 레이어
|
||||
* [Module overview] ICM42670P IMU driver application layer
|
||||
*
|
||||
* ICM42670P IMU 센서의 초기화, 설정, 데이터 읽기를 담당하는 애플리케이션 레이어 모듈
|
||||
* InvenSense 드라이버 API를 래핑하여 사용
|
||||
* Application layer module responsible for initialization, configuration,
|
||||
* and data reading of the ICM42670P IMU sensor. Wraps InvenSense driver API.
|
||||
*
|
||||
* 주요 기능:
|
||||
* 1) setup_imu_device() - IMU 초기화 및 WHOAMI 확인 (0x67 = ICM42670P)
|
||||
* 2) configure_imu_device() - 센서 파라미터 설정
|
||||
* - 가속도계: ±4g FSR, 100Hz(저전력) 또는 800Hz(저잡음)
|
||||
* - 자이로: ±2000dps FSR, 100Hz 또는 800Hz
|
||||
* - FIFO 비활성화 (레지스터 직접 읽기 모드)
|
||||
* 3) get_imu_data() - FIFO 또는 레지스터에서 센서 데이터 읽기
|
||||
* 4) imu_callback() - 센서 데이터 수신 콜백
|
||||
* - 마운팅 매트릭스 적용 (보드 방향 보정)
|
||||
* - info4 모드: info_imu[6]에 데이터 저장
|
||||
* - BLE 모드: "rsp:" 태그로 6축 데이터 BLE 전송
|
||||
* - UART 모드: 텍스트 형식으로 시리얼 출력
|
||||
* 5) imu_read_direct() - 드라이버 API를 우회한 직접 I2C 레지스터 읽기
|
||||
* - 센서 설정 → 전원 ON → 80ms 대기 → 12바이트 읽기 → 슬립
|
||||
* Key functions:
|
||||
* 1) setup_imu_device() - IMU init and WHOAMI verification (0x67 = ICM42670P)
|
||||
* 2) configure_imu_device() - Sensor parameter configuration
|
||||
* - Accelerometer: +/-4g FSR, 100Hz (low-power) or 800Hz (low-noise)
|
||||
* - Gyroscope: +/-2000dps FSR, 100Hz or 800Hz
|
||||
* - FIFO disabled (direct register read mode)
|
||||
* 3) get_imu_data() - Read sensor data from FIFO or registers
|
||||
* 4) imu_callback() - Sensor data receive callback
|
||||
* - Applies mounting matrix (board orientation correction)
|
||||
* - info4 mode: stores data in info_imu[6]
|
||||
* - BLE mode: sends 6-axis data via BLE with "rsp:" tag
|
||||
* - UART mode: outputs text format to serial
|
||||
* 5) imu_read_direct() - Direct I2C register read bypassing driver API
|
||||
* - Configure sensor -> power ON -> wait 80ms -> read 12 bytes -> sleep
|
||||
*
|
||||
* 마운팅 매트릭스:
|
||||
* Q30 고정소수점 형식의 3x3 회전 매트릭스로, 보드에 장착된 센서의물리적 방향을 소프트웨어 좌표계에 맞춰 보정
|
||||
* Mounting matrix:
|
||||
* 3x3 rotation matrix in Q30 fixed-point format, correcting the sensor's
|
||||
* physical mounting orientation to match the software coordinate system.
|
||||
******************************************************************************/
|
||||
|
||||
#include "sdk_config.h"
|
||||
@@ -47,9 +48,9 @@
|
||||
|
||||
|
||||
/*
|
||||
* 데이터 출력 형식 선택
|
||||
* 0 : 원시 데이터 (raw accel, gyro, temp) 출력
|
||||
* 1 : 스케일링된 데이터 (g, dps, 섭씨) 출력
|
||||
* Data output format selection
|
||||
* 0 : Raw data output (raw accel, gyro, temp)
|
||||
* 1 : Scaled data output (g, dps, Celsius)
|
||||
*/
|
||||
#define SCALED_DATA_G_DPS 0
|
||||
|
||||
@@ -58,20 +59,20 @@
|
||||
* Static and extern variables
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
|
||||
/* IMU 드라이버 객체 — 드라이버 API 호출 시 항상 이 구조체 전달 */
|
||||
/* IMU driver object — always passed to driver API calls */
|
||||
static struct inv_imu_device icm_driver;
|
||||
|
||||
/* BLE 전송용 바이너리 버퍼 */
|
||||
/* Binary buffer for BLE transmission */
|
||||
uint8_t imu_bin_buffer[BLE_NUS_MAX_DATA_LEN];
|
||||
|
||||
/*
|
||||
* ICM42670P 마운팅 매트릭스 (Q30 고정소수점)
|
||||
* ICM42670P mounting matrix (Q30 fixed-point)
|
||||
*
|
||||
* 센서가 보드에 장착된 물리적 방향에 따라 좌표 변환
|
||||
* Q30 형식: 1.0 = (1 << 30) = 0x40000000
|
||||
* Coordinate transform based on the sensor's physical mounting orientation.
|
||||
* Q30 format: 1.0 = (1 << 30) = 0x40000000
|
||||
*
|
||||
* SM_REVB_DB (개발보드): X→-Y, Y→X 변환 (90도 회전)
|
||||
* 기본 (SmartMotion): 단위 행렬 (변환 없음)
|
||||
* SM_REVB_DB (dev board): X->-Y, Y->X transform (90-degree rotation)
|
||||
* Default (SmartMotion): identity matrix (no transform)
|
||||
*/
|
||||
#if (SM_BOARD_REV == SM_REVB_DB) /* when DB or EVB are used */
|
||||
static int32_t icm_mounting_matrix[9] = { 0, -(1<<30), 0,
|
||||
@@ -83,13 +84,13 @@ static int32_t icm_mounting_matrix[9] = {(1<<30), 0, 0,
|
||||
0, 0, (1<<30)};
|
||||
#endif
|
||||
|
||||
bool custom_add_data; /* 커스텀 데이터 추가 플래그 (BLE 전송 제어) */
|
||||
extern bool motion_raw_data_enabled; /* 외부에서 원시 데이터 읽기를 요청하는 플래그 */
|
||||
extern char ble_tx_buffer[BLE_NUS_MAX_DATA_LEN]; /* BLE 텍스트 전송 버퍼 */
|
||||
extern which_cmd_t cmd_type_t; /* 현재 명령 소스 (BLE 또는 UART) */
|
||||
uint16_t ssp_data[6]={0,}; /* BLE 전송용 6축 데이터 배열 (accel XYZ + gyro XYZ) */
|
||||
extern bool info4; /* info4 모드 플래그 (cmd_parse에서 설정) */
|
||||
volatile uint16_t info_imu[6]; /* info4 모드에서 IMU 데이터를 저장하는 전역 배열 */
|
||||
bool custom_add_data; /* Custom data append flag (BLE transmission control) */
|
||||
extern bool motion_raw_data_enabled; /* Flag requesting raw data read from external module */
|
||||
extern char ble_tx_buffer[BLE_NUS_MAX_DATA_LEN]; /* BLE text transmit buffer */
|
||||
extern which_cmd_t cmd_type_t; /* Current command source (BLE or UART) */
|
||||
uint16_t ssp_data[6]={0,}; /* 6-axis data array for BLE (accel XYZ + gyro XYZ) */
|
||||
extern bool info4; /* info4 mode flag (set by cmd_parse) */
|
||||
volatile uint16_t info_imu[6]; /* Global array storing IMU data in info4 mode */
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* static function declaration
|
||||
@@ -102,35 +103,35 @@ static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3]);
|
||||
|
||||
/*
|
||||
* setup_imu_device()
|
||||
* IMU 디바이스 초기화 및 식별 확인
|
||||
* IMU device initialization and identification verification.
|
||||
*
|
||||
* 처리 흐름:
|
||||
* 1) inv_imu_init()으로 드라이버 초기화 (시리얼 인터페이스 + 콜백 등록)
|
||||
* 2) WHOAMI 레지스터 읽기로 디바이스 확인
|
||||
* 3) WHOAMI 값이 ICM_WHOAMI(0x67)와 일치하는지 검증
|
||||
* Flow:
|
||||
* 1) Initialize driver via inv_imu_init() (serial interface + callback registration)
|
||||
* 2) Read WHOAMI register for device identification
|
||||
* 3) Verify WHOAMI value matches ICM_WHOAMI (0x67)
|
||||
*
|
||||
* 반환값: 0=성공, 음수=에러
|
||||
* Returns: 0=success, negative=error
|
||||
*/
|
||||
int setup_imu_device(struct inv_imu_serif *icm_serif)
|
||||
{
|
||||
int rc = 0;
|
||||
uint8_t who_am_i;
|
||||
|
||||
/* IMU 드라이버 초기화 — 시리얼 인터페이스 연결 및 콜백 함수 등록 */
|
||||
/* Initialize IMU driver — connect serial interface and register callback */
|
||||
rc = inv_imu_init(&icm_driver, icm_serif, imu_callback);
|
||||
if (rc != INV_ERROR_SUCCESS) {
|
||||
DBG_PRINTF("!!! ERROR : Failed to initialize IMU!\r\n");
|
||||
return rc;
|
||||
}
|
||||
|
||||
/* WHOAMI 레지스터 읽기 — 디바이스 존재 및 통신 확인 */
|
||||
/* Read WHOAMI register — verify device presence and communication */
|
||||
rc = inv_imu_get_who_am_i(&icm_driver, &who_am_i);
|
||||
if (rc != INV_ERROR_SUCCESS) {
|
||||
DBG_PRINTF("!!! ERROR : Failed to read whoami!\r\n");
|
||||
return rc;
|
||||
}
|
||||
|
||||
/* WHOAMI 값 검증 — ICM42670P의 경우 0x67이어야 함 */
|
||||
/* Verify WHOAMI value — must be 0x67 for ICM42670P */
|
||||
if (who_am_i != ICM_WHOAMI) {
|
||||
DBG_PRINTF("!!! ERROR : Bad WHOAMI value! Read 0x%02x, expected 0x%02x\r\n", who_am_i, ICM_WHOAMI);
|
||||
return INV_ERROR;
|
||||
@@ -141,53 +142,53 @@ int setup_imu_device(struct inv_imu_serif *icm_serif)
|
||||
|
||||
/*
|
||||
* configure_imu_device()
|
||||
* IMU 센서 동작 파라미터 설정
|
||||
* Configures IMU sensor operating parameters.
|
||||
*
|
||||
* 설정 항목:
|
||||
* - FIFO: 비활성화 (USE_FIFO=0일 때, 레지스터 직접 읽기 모드)
|
||||
* - 가속도계 FSR: ±4g (USE_HIGH_RES_MODE=0일 때)
|
||||
* - 자이로 FSR: ±2000dps
|
||||
* - ODR(출력 데이터율):
|
||||
* - 저잡음 모드(USE_LOW_NOISE_MODE=1): 800Hz
|
||||
* - 저전력 모드(USE_LOW_NOISE_MODE=0): 100Hz
|
||||
* - 자이로는 항상 저잡음 모드로 동작
|
||||
* - FIFO 미사용 시 자이로 스타트업 시간만큼 대기
|
||||
* Settings:
|
||||
* - FIFO: disabled (when USE_FIFO=0, direct register read mode)
|
||||
* - Accel FSR: +/-4g (when USE_HIGH_RES_MODE=0)
|
||||
* - Gyro FSR: +/-2000dps
|
||||
* - ODR (output data rate):
|
||||
* - Low-noise mode (USE_LOW_NOISE_MODE=1): 800Hz
|
||||
* - Low-power mode (USE_LOW_NOISE_MODE=0): 100Hz
|
||||
* - Gyro always operates in low-noise mode
|
||||
* - Waits for gyro startup time when FIFO is not used
|
||||
*
|
||||
* 반환값: 0=성공, 음수=에러
|
||||
* Returns: 0=success, negative=error
|
||||
*/
|
||||
int configure_imu_device(void)
|
||||
{
|
||||
int rc = 0;
|
||||
|
||||
/* FIFO 비활성화 — 레지스터에서 직접 데이터를 읽기 */
|
||||
/* Disable FIFO — read data directly from registers */
|
||||
if (!USE_FIFO)
|
||||
rc |= inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_DISABLED);
|
||||
|
||||
if (USE_HIGH_RES_MODE) {
|
||||
/* 고해상도 FIFO 모드: 20비트 데이터, FSR은 16g/2000dps로 고정됨 */
|
||||
/* High-resolution FIFO mode: 20-bit data, FSR locked to 16g/2000dps */
|
||||
rc |= inv_imu_enable_high_resolution_fifo(&icm_driver);
|
||||
} else {
|
||||
/* 표준 모드: 가속도계 ±4g, 자이로 ±2000dps FSR 설정 */
|
||||
/* Standard mode: accel +/-4g, gyro +/-2000dps FSR */
|
||||
rc |= inv_imu_set_accel_fsr(&icm_driver, ACCEL_CONFIG0_FS_SEL_4g);
|
||||
rc |= inv_imu_set_gyro_fsr(&icm_driver, GYRO_CONFIG0_FS_SEL_2000dps);
|
||||
}
|
||||
|
||||
if (USE_LOW_NOISE_MODE) {
|
||||
/* 저잡음 모드: 800Hz ODR, 가속도계 저잡음 모드 활성화 */
|
||||
/* Low-noise mode: 800Hz ODR, enable accel low-noise mode */
|
||||
rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_800_HZ);
|
||||
rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_800_HZ);
|
||||
rc |= inv_imu_enable_accel_low_noise_mode(&icm_driver);
|
||||
} else {
|
||||
/* 저전력 모드: 100Hz ODR, 가속도계 저전력 모드 활성화 */
|
||||
/* Low-power mode: 100Hz ODR, enable accel low-power mode */
|
||||
rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_100_HZ);
|
||||
rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_100_HZ);
|
||||
rc |= inv_imu_enable_accel_low_power_mode(&icm_driver);
|
||||
}
|
||||
|
||||
/* 자이로는 모드에 관계없이 항상 저잡음 모드로 동작 */
|
||||
/* Gyro always operates in low-noise mode regardless of setting */
|
||||
rc |= inv_imu_enable_gyro_low_noise_mode(&icm_driver);
|
||||
|
||||
/* FIFO 미사용 시 자이로 스타트업 시간만큼 대기 (첫 유효 데이터까지의 지연) */
|
||||
/* When FIFO is not used, wait for gyro startup time (delay until first valid data) */
|
||||
if (!USE_FIFO)
|
||||
inv_imu_sleep_us(GYR_STARTUP_TIME_US);
|
||||
|
||||
@@ -197,9 +198,9 @@ int configure_imu_device(void)
|
||||
|
||||
/*
|
||||
* get_imu_data()
|
||||
* IMU에서 센서 데이터 읽기
|
||||
* USE_FIFO 설정에 따라 FIFO 또는 레지스터에서 데이터를 가져옴
|
||||
* 읽은 데이터는 imu_callback()을 통해 처리
|
||||
* Reads sensor data from the IMU.
|
||||
* Fetches data from FIFO or registers depending on USE_FIFO setting.
|
||||
* Read data is processed via imu_callback().
|
||||
*/
|
||||
int get_imu_data(void)
|
||||
{
|
||||
@@ -214,8 +215,8 @@ int get_imu_data(void)
|
||||
#if SCALED_DATA_G_DPS
|
||||
/*
|
||||
* get_accel_and_gyr_fsr()
|
||||
* 현재 설정된 가속도계와 자이로의 FSR(Full Scale Range) 값을 가져온다.
|
||||
* 스케일링된 데이터(g, dps) 변환에 사용된다.
|
||||
* Retrieves the currently configured FSR (Full Scale Range) for accel and gyro.
|
||||
* Used for converting to scaled data (g, dps).
|
||||
*/
|
||||
static void get_accel_and_gyr_fsr(int16_t * accel_fsr_g, int16_t * gyro_fsr_dps)
|
||||
{
|
||||
@@ -253,17 +254,17 @@ static void get_accel_and_gyr_fsr(int16_t * accel_fsr_g, int16_t * gyro_fsr_dps)
|
||||
|
||||
/*
|
||||
* imu_callback()
|
||||
* IMU 드라이버가 새 센서 데이터를 읽을 때마다 호출되는 콜백 함수
|
||||
* Callback invoked each time the IMU driver reads new sensor data.
|
||||
*
|
||||
* 처리 흐름:
|
||||
* 1) 이벤트에서 가속도/자이로 원시 데이터 추출
|
||||
* - FIFO 모드: 타임스탬프 롤오버 처리, 고해상도(20비트) 지원
|
||||
* - 레지스터 모드: 16비트 데이터 직접 사용
|
||||
* 2) 마운팅 매트릭스 적용 (보드 장착 방향 보정)
|
||||
* 3) 데이터 출력 (모드에 따라 분기):
|
||||
* - info4 모드: info_imu[6] 전역 배열에 저장 (외부에서 폴링)
|
||||
* - UART 모드: "Tp" 접두사로 6축 데이터 텍스트 출력
|
||||
* - BLE 모드: "rsp:" 태그로 바이너리 패킷 전송 + UART 텍스트 동시 출력
|
||||
* Flow:
|
||||
* 1) Extract raw accel/gyro data from event
|
||||
* - FIFO mode: handles timestamp rollover, supports high-res (20-bit)
|
||||
* - Register mode: uses 16-bit data directly
|
||||
* 2) Apply mounting matrix (board orientation correction)
|
||||
* 3) Output data (branches by mode):
|
||||
* - info4 mode: stores in info_imu[6] global array (polled externally)
|
||||
* - UART mode: text output with "Tp" prefix for 6-axis data
|
||||
* - BLE mode: binary packet with "rsp:" tag + simultaneous UART output
|
||||
*/
|
||||
void imu_callback(inv_imu_sensor_event_t *event)
|
||||
{
|
||||
@@ -280,18 +281,18 @@ void imu_callback(inv_imu_sensor_event_t *event)
|
||||
static uint64_t last_fifo_timestamp = 0;
|
||||
static uint32_t rollover_num = 0;
|
||||
|
||||
/* FIFO 타임스탬프 롤오버 처리 (16비트 → 64비트 확장) */
|
||||
/* FIFO timestamp rollover handling (16-bit -> 64-bit extension) */
|
||||
if (last_fifo_timestamp > event->timestamp_fsync)
|
||||
rollover_num++;
|
||||
last_fifo_timestamp = event->timestamp_fsync;
|
||||
|
||||
/* 타임스탬프를 마이크로초 단위로 변환 (Q24 해상도 적용) */
|
||||
/* Convert timestamp to microseconds (apply Q24 resolution) */
|
||||
timestamp = event->timestamp_fsync + rollover_num * UINT16_MAX;
|
||||
timestamp *= inv_imu_get_fifo_timestamp_resolution_us_q24(&icm_driver);
|
||||
timestamp /= (1UL << 24);
|
||||
|
||||
if (icm_driver.fifo_highres_enabled) {
|
||||
/* 고해상도 모드: 16비트 데이터를 4비트 좌측 시프트 + 하위 4비트 추가 → 20비트 */
|
||||
/* High-res mode: left-shift 16-bit data by 4 + add lower 4 bits -> 20-bit */
|
||||
accel[0] = (((int32_t)event->accel[0] << 4)) | event->accel_high_res[0];
|
||||
accel[1] = (((int32_t)event->accel[1] << 4)) | event->accel_high_res[1];
|
||||
accel[2] = (((int32_t)event->accel[2] << 4)) | event->accel_high_res[2];
|
||||
@@ -301,7 +302,7 @@ void imu_callback(inv_imu_sensor_event_t *event)
|
||||
gyro[2] = (((int32_t)event->gyro[2] << 4)) | event->gyro_high_res[2];
|
||||
|
||||
} else {
|
||||
/* 표준 해상도: 16비트 데이터 그대로 사용 */
|
||||
/* Standard resolution: use 16-bit data as-is */
|
||||
accel[0] = event->accel[0];
|
||||
accel[1] = event->accel[1];
|
||||
accel[2] = event->accel[2];
|
||||
@@ -312,7 +313,7 @@ void imu_callback(inv_imu_sensor_event_t *event)
|
||||
}
|
||||
#else
|
||||
|
||||
/* 레지스터 직접 읽기 모드: 16비트 원시 데이터 추출 */
|
||||
/* Direct register read mode: extract 16-bit raw data */
|
||||
accel[0] = event->accel[0];
|
||||
accel[1] = event->accel[1];
|
||||
accel[2] = event->accel[2];
|
||||
@@ -321,20 +322,20 @@ void imu_callback(inv_imu_sensor_event_t *event)
|
||||
gyro[1] = event->gyro[1];
|
||||
gyro[2] = event->gyro[2];
|
||||
|
||||
/* 레지스터 모드에서는 센서 마스크를 강제 설정하여 아래 출력 로직이 동작하도록 함 */
|
||||
/* In register mode, force sensor mask so the output logic below works */
|
||||
event->sensor_mask |= (1 << INV_SENSOR_TEMPERATURE);
|
||||
event->sensor_mask |= (1 << INV_SENSOR_ACCEL);
|
||||
event->sensor_mask |= (1 << INV_SENSOR_GYRO);
|
||||
#endif
|
||||
|
||||
/* 마운팅 매트릭스 적용 — 센서의 물리적 장착 방향을 소프트웨어 좌표계로 보정 */
|
||||
/* Apply mounting matrix — correct sensor physical orientation to software coordinates */
|
||||
apply_mounting_matrix(icm_mounting_matrix, accel);
|
||||
apply_mounting_matrix(icm_mounting_matrix, gyro);
|
||||
|
||||
#if SCALED_DATA_G_DPS
|
||||
/*
|
||||
* 원시 데이터를 물리 단위(g, dps)로 변환
|
||||
* 변환 공식: 물리값 = 원시값 * FSR / INT16_MAX
|
||||
* Convert raw data to physical units (g, dps)
|
||||
* Formula: physical_value = raw_value * FSR / INT16_MAX
|
||||
*/
|
||||
get_accel_and_gyr_fsr(&accel_fsr_g, &gyro_fsr_dps);
|
||||
accel_g[0] = (float)(accel[0] * accel_fsr_g) / INT16_MAX;
|
||||
@@ -344,14 +345,14 @@ void imu_callback(inv_imu_sensor_event_t *event)
|
||||
gyro_dps[1] = (float)(gyro[1] * gyro_fsr_dps) / INT16_MAX;
|
||||
gyro_dps[2] = (float)(gyro[2] * gyro_fsr_dps) / INT16_MAX;
|
||||
|
||||
/* 온도 변환: 고해상도/레지스터 모드는 /128, FIFO 표준 모드는 /2 */
|
||||
/* Temperature conversion: high-res/register mode uses /128, FIFO standard mode uses /2 */
|
||||
if (USE_HIGH_RES_MODE || !USE_FIFO)
|
||||
temp_degc = 25 + ((float)event->temperature / 128);
|
||||
else
|
||||
temp_degc = 25 + ((float)event->temperature / 2);
|
||||
|
||||
/*
|
||||
* 스케일링된 데이터를 UART로 출력
|
||||
* Output scaled data via UART
|
||||
*/
|
||||
if (event->sensor_mask & (1 << INV_SENSOR_ACCEL) && event->sensor_mask & (1 << INV_SENSOR_GYRO))
|
||||
DBG_PRINTF("%u: %.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f\r\n",
|
||||
@@ -362,13 +363,13 @@ void imu_callback(inv_imu_sensor_event_t *event)
|
||||
#else
|
||||
|
||||
/*
|
||||
* 원시 데이터 출력 — 명령 소스(info4/UART/BLE)에 따라 분기
|
||||
* Raw data output — branches by command source (info4/UART/BLE)
|
||||
*/
|
||||
if (event->sensor_mask & (1 << INV_SENSOR_ACCEL) && event->sensor_mask & (1 << INV_SENSOR_GYRO) || motion_raw_data_enabled)
|
||||
{
|
||||
motion_raw_data_enabled = false;
|
||||
|
||||
/* info4 모드: 전역 배열 info_imu[6]에 데이터 저장, 외부 모듈에서 이 배열을 폴링하여 데이터 사용 */
|
||||
/* info4 mode: store data in global array info_imu[6], polled by external modules */
|
||||
if (info4 == true)
|
||||
{
|
||||
info_imu[0] = (uint16_t)accel[0];
|
||||
@@ -379,16 +380,16 @@ void imu_callback(inv_imu_sensor_event_t *event)
|
||||
info_imu[5] = (uint16_t)gyro[2];
|
||||
}
|
||||
|
||||
/* UART 모드: "Tp" 접두사로 6축 데이터를 텍스트 형식으로 출력 */
|
||||
/* UART mode: output 6-axis data in text format with "Tp" prefix */
|
||||
else if(cmd_type_t == CMD_UART) {
|
||||
//DBG_PRINTF("Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
|
||||
}
|
||||
|
||||
/*
|
||||
* BLE 모드: 6축 데이터를 바이너리 패킷으로 BLE 전송
|
||||
* ssp_data[0~2] = 가속도 XYZ, ssp_data[3~5] = 자이로 XYZ
|
||||
* format_data()로 "rsp:" 태그 + 12바이트 데이터를 패킷화
|
||||
* dr_binary_tx_safe()로 8바이트 BLE 전송
|
||||
* BLE mode: send 6-axis data as binary packet via BLE
|
||||
* ssp_data[0..2] = accel XYZ, ssp_data[3..5] = gyro XYZ
|
||||
* format_data() packs "rsp:" tag + 12-byte data
|
||||
* dr_binary_tx_safe() sends 8 bytes via BLE
|
||||
*/
|
||||
else if(cmd_type_t == CMD_BLE) {
|
||||
ssp_data[0] = (uint16_t)accel[0];
|
||||
@@ -419,13 +420,13 @@ void imu_callback(inv_imu_sensor_event_t *event)
|
||||
|
||||
/*
|
||||
* apply_mounting_matrix()
|
||||
* Q30 고정소수점 회전 매트릭스를 3축 벡터에 적용한다.
|
||||
* Applies a Q30 fixed-point rotation matrix to a 3-axis vector.
|
||||
*
|
||||
* 계산 방식:
|
||||
* Calculation:
|
||||
* result[i] = matrix[i*3+0]*raw[0] + matrix[i*3+1]*raw[1] + matrix[i*3+2]*raw[2]
|
||||
* 결과를 30비트 우측 시프트하여 Q30 → 정수 변환
|
||||
* Right-shift result by 30 bits for Q30 -> integer conversion.
|
||||
*
|
||||
* 이를 통해 센서의 물리적 장착 방향에 관계없이 일관된 좌표계를 사용할 수 있다.
|
||||
* Ensures a consistent coordinate system regardless of physical sensor orientation.
|
||||
*/
|
||||
static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
|
||||
{
|
||||
@@ -437,7 +438,7 @@ static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
|
||||
data_q30[i] += ((int64_t)matrix[3*i+1] * raw[1]);
|
||||
data_q30[i] += ((int64_t)matrix[3*i+2] * raw[2]);
|
||||
}
|
||||
/* Q30 → 정수 변환: 30비트 우측 시프트 */
|
||||
/* Q30 -> integer conversion: right-shift by 30 bits */
|
||||
raw[0] = (int32_t)(data_q30[0]>>30);
|
||||
raw[1] = (int32_t)(data_q30[1]>>30);
|
||||
raw[2] = (int32_t)(data_q30[2]>>30);
|
||||
@@ -446,22 +447,22 @@ static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
|
||||
|
||||
/*
|
||||
* imu_read_direct()
|
||||
* 드라이버 API를 우회하여 직접 I2C 레지스터를 읽는 함수.
|
||||
* DRDY(데이터 준비) 인터럽트를 기다리지 않고 즉시 데이터를 읽는다.
|
||||
* Reads IMU registers directly via I2C, bypassing the driver API.
|
||||
* Reads data immediately without waiting for DRDY interrupt.
|
||||
*
|
||||
* 처리 흐름:
|
||||
* 1) TWI 초기화 확인 (최초 1회만)
|
||||
* 2) 자이로 설정: ±2000dps, 100Hz ODR (GYRO_CONFIG0 = 0x09)
|
||||
* 3) 가속도 설정: ±4g, 100Hz ODR (ACCEL_CONFIG0 = 0x29)
|
||||
* 4) 전원 ON: 가속도+자이로 저잡음 모드 (PWR_MGMT0 = 0x0F)
|
||||
* 5) 80ms 대기 (자이로 스타트업: 최소 45ms + 여유)
|
||||
* 6) ACCEL_DATA_X1(0x0B)부터 12바이트 연속 읽기 (accel 6 + gyro 6)
|
||||
* 7) 빅엔디안 → int16_t 변환
|
||||
* 8) 마운팅 매트릭스 적용
|
||||
* 9) "rsp:" 태그로 BLE 전송
|
||||
* 10) IMU 슬립 모드로 전환 (전력 절감)
|
||||
* Flow:
|
||||
* 1) Check TWI initialization (first call only)
|
||||
* 2) Gyro config: +/-2000dps, 100Hz ODR (GYRO_CONFIG0 = 0x09)
|
||||
* 3) Accel config: +/-4g, 100Hz ODR (ACCEL_CONFIG0 = 0x29)
|
||||
* 4) Power ON: accel+gyro low-noise mode (PWR_MGMT0 = 0x0F)
|
||||
* 5) Wait 80ms (gyro startup: min 45ms + margin)
|
||||
* 6) Read 12 consecutive bytes from ACCEL_DATA_X1 (0x0B) (accel 6 + gyro 6)
|
||||
* 7) Big-endian -> int16_t conversion
|
||||
* 8) Apply mounting matrix
|
||||
* 9) Send via BLE with "rsp:" tag
|
||||
* 10) Switch IMU to sleep mode (power saving)
|
||||
*
|
||||
* 반환값: 0=성공, -1=TX 실패, -2=RX 실패
|
||||
* Returns: 0=success, -1=TX failure, -2=RX failure
|
||||
*/
|
||||
/* Raw I2C read from ICM42670P — bypasses driver API entirely */
|
||||
#include "system_interface.h"
|
||||
@@ -470,51 +471,51 @@ static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
|
||||
extern const nrfx_twi_t m_twi_icm42670;
|
||||
|
||||
#define IMU_I2C_ADDR 0x68
|
||||
#define REG_ACCEL_X1 0x0B /* ACCEL_DATA_X1 — 가속도 X축 상위 바이트 레지스터 */
|
||||
#define REG_ACCEL_X1 0x0B /* ACCEL_DATA_X1 — accel X-axis upper byte register */
|
||||
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* Direct IMU register read — raw I2C, no DRDY, sends rsp: via BLE
|
||||
* 직접 I2C로 레지스터 읽는 방식 (인터럽트 X, IMU 드라이버 API X)
|
||||
* Direct I2C register read (no interrupt, no IMU driver API)
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
int imu_read_direct(void)
|
||||
{
|
||||
uint8_t raw[12]; /* 가속도 6바이트 + 자이로 6바이트 */
|
||||
uint8_t raw[12]; /* accel 6 bytes + gyro 6 bytes */
|
||||
int32_t accel[3], gyro[3];
|
||||
uint8_t reg;
|
||||
uint32_t ret;
|
||||
|
||||
static bool twi_ready = false;
|
||||
|
||||
/* TWI(I2C) 초기화 — 최초 1회만 수행 (재초기화로 클린 상태 보장) */
|
||||
/* TWI (I2C) init — performed only once (re-init ensures clean state) */
|
||||
if (!twi_ready) {
|
||||
inv_i2c_master_uninitialize();
|
||||
inv_i2c_master_initialize();
|
||||
twi_ready = true;
|
||||
}
|
||||
|
||||
/* 자이로 설정: GYRO_CONFIG0(0x20) = 0x09 → ±2000dps FSR, 100Hz ODR */
|
||||
/* Gyro config: GYRO_CONFIG0(0x20) = 0x09 -> +/-2000dps FSR, 100Hz ODR */
|
||||
{
|
||||
uint8_t gyro_cfg[2] = { 0x20, 0x09 };
|
||||
icm42670_twi_tx(IMU_I2C_ADDR, gyro_cfg, 2, false);
|
||||
}
|
||||
|
||||
/* 가속도 설정: ACCEL_CONFIG0(0x21) = 0x29 → ±4g FSR, 100Hz ODR */
|
||||
/* Accel config: ACCEL_CONFIG0(0x21) = 0x29 -> +/-4g FSR, 100Hz ODR */
|
||||
{
|
||||
uint8_t accel_cfg[2] = { 0x21, 0x29 };
|
||||
icm42670_twi_tx(IMU_I2C_ADDR, accel_cfg, 2, false);
|
||||
}
|
||||
|
||||
/* 전원 ON: PWR_MGMT0(0x1F) = 0x0F → 가속도(저잡음) + 자이로(저잡음) 활성화 */
|
||||
/* Power ON: PWR_MGMT0(0x1F) = 0x0F -> accel (low-noise) + gyro (low-noise) enabled */
|
||||
{
|
||||
uint8_t pwr_cmd[2] = { 0x1F, 0x0F }; /* reg=0x1F, val=0x0F */
|
||||
icm42670_twi_tx(IMU_I2C_ADDR, pwr_cmd, 2, false);
|
||||
//nrf_delay_ms(80); /* 자이로 스타트업: 최소 45ms + 안전 마진 */
|
||||
//nrf_delay_ms(80); /* Gyro startup: min 45ms + safety margin */
|
||||
dr_sd_delay_ms(80);
|
||||
}
|
||||
|
||||
/* ACCEL_DATA_X1(0x0B)부터 12바이트 연속 읽기 (0x0B~0x16) */
|
||||
/* Read 12 consecutive bytes from ACCEL_DATA_X1 (0x0B~0x16) */
|
||||
reg = REG_ACCEL_X1;
|
||||
ret = icm42670_twi_tx(IMU_I2C_ADDR, ®, 1, true); /* 레지스터 주소 전송 (STOP 없음) */
|
||||
ret = icm42670_twi_tx(IMU_I2C_ADDR, ®, 1, true); /* Send register address (no STOP) */
|
||||
|
||||
if (ret)
|
||||
{
|
||||
@@ -522,7 +523,7 @@ int imu_read_direct(void)
|
||||
return -1;
|
||||
}
|
||||
|
||||
ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, 12); /* 12바이트 데이터 수신 */
|
||||
ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, 12); /* Receive 12 bytes of data */
|
||||
|
||||
if (ret)
|
||||
{
|
||||
@@ -531,9 +532,9 @@ int imu_read_direct(void)
|
||||
}
|
||||
|
||||
/*
|
||||
* 빅엔디안 레지스터 레이아웃을 int16_t로 변환
|
||||
* raw[0..5] = 가속도 X,Y,Z (각 2바이트, MSB first)
|
||||
* raw[6..11] = 자이로 X,Y,Z (각 2바이트, MSB first)
|
||||
* Convert big-endian register layout to int16_t
|
||||
* raw[0..5] = accel X,Y,Z (2 bytes each, MSB first)
|
||||
* raw[6..11] = gyro X,Y,Z (2 bytes each, MSB first)
|
||||
*/
|
||||
accel[0] = (int16_t)((raw[0] << 8) | raw[1]);
|
||||
accel[1] = (int16_t)((raw[2] << 8) | raw[3]);
|
||||
@@ -542,11 +543,11 @@ int imu_read_direct(void)
|
||||
gyro[1] = (int16_t)((raw[8] << 8) | raw[9]);
|
||||
gyro[2] = (int16_t)((raw[10] << 8) | raw[11]);
|
||||
|
||||
/* 마운팅 매트릭스 적용 — 보드 장착 방향 보정 */
|
||||
/* Apply mounting matrix — board orientation correction */
|
||||
apply_mounting_matrix(icm_mounting_matrix, accel);
|
||||
apply_mounting_matrix(icm_mounting_matrix, gyro);
|
||||
|
||||
/* 데이터 패킹 */
|
||||
/* Pack data */
|
||||
ssp_data[0] = (uint16_t)accel[0];
|
||||
ssp_data[1] = (uint16_t)accel[1];
|
||||
ssp_data[2] = (uint16_t)accel[2];
|
||||
@@ -556,7 +557,7 @@ int imu_read_direct(void)
|
||||
|
||||
if (info4 == true)
|
||||
{
|
||||
/* info4 모드: 전역 배열에 저장 (mbb?에서 rbb: 패킷으로 일괄 전송) */
|
||||
/* info4 mode: store in global array (sent as rbb: packet by mbb?) */
|
||||
info_imu[0] = ssp_data[0];
|
||||
info_imu[1] = ssp_data[1];
|
||||
info_imu[2] = ssp_data[2];
|
||||
@@ -566,12 +567,12 @@ int imu_read_direct(void)
|
||||
}
|
||||
else
|
||||
{
|
||||
/* 일반 모드: "rsp:" 태그로 BLE 즉시 전송 */
|
||||
/* Normal mode: send immediately via BLE with "rsp:" tag */
|
||||
format_data(imu_bin_buffer, "rsp:", ssp_data, 12);
|
||||
dr_binary_tx_safe(imu_bin_buffer, 8);
|
||||
}
|
||||
|
||||
/* IMU 슬립 모드: PWR_MGMT0 = 0x00 → 가속도/자이로 모두 OFF (전력 절감) */
|
||||
/* IMU sleep mode: PWR_MGMT0 = 0x00 -> accel/gyro both OFF (power saving) */
|
||||
{
|
||||
uint8_t pwr_off[2] = { 0x1F, 0x00 }; /* reg=PWR_MGMT0, val=0x00 */
|
||||
icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false);
|
||||
|
||||
@@ -7,16 +7,16 @@
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [헤더 개요] ICM42670P IMU 드라이버 상위 레이어 선언
|
||||
* [Header overview] ICM42670P IMU driver application layer declarations
|
||||
*
|
||||
* IMU 센서의 초기화, 설정, 데이터 읽기를 위한 함수 프로토타입과
|
||||
* 동작 모드 설정 매크로를 정의한다.
|
||||
* Function prototypes and operating mode configuration macros for
|
||||
* IMU sensor initialization, configuration, and data reading.
|
||||
*
|
||||
* 주요 설정 매크로:
|
||||
* SERIF_TYPE - 통신 인터페이스 (UI_I2C)
|
||||
* USE_LOW_NOISE_MODE - 1:저잡음(800Hz), 0:저전력(100Hz)
|
||||
* USE_HIGH_RES_MODE - 1:20비트 고해상도, 0:16비트 표준
|
||||
* USE_FIFO - 1:FIFO 사용, 0:레지스터 직접 읽기
|
||||
* Key configuration macros:
|
||||
* SERIF_TYPE - Communication interface (UI_I2C)
|
||||
* USE_LOW_NOISE_MODE - 1: low-noise (800Hz), 0: low-power (100Hz)
|
||||
* USE_HIGH_RES_MODE - 1: 20-bit high-res, 0: 16-bit standard
|
||||
* USE_FIFO - 1: use FIFO, 0: direct register read
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef _APP_RAW_H_
|
||||
@@ -29,71 +29,71 @@
|
||||
#include "inv_imu_driver.h"
|
||||
|
||||
|
||||
/*** 설정 매크로 ***/
|
||||
/*** Configuration macros ***/
|
||||
|
||||
/*
|
||||
* MCU와 IMU 간 통신 인터페이스 선택
|
||||
* UI_I2C: I2C 통신 사용 (기본)
|
||||
* MCU-IMU communication interface selection
|
||||
* UI_I2C: use I2C communication (default)
|
||||
*/
|
||||
#define SERIF_TYPE UI_I2C
|
||||
|
||||
/*
|
||||
* 전원 모드 설정
|
||||
* 1: 저잡음 모드 — 800Hz ODR, 높은 정밀도, 높은 전력 소모
|
||||
* 0: 저전력 모드 — 100Hz ODR, 낮은 전력 소모
|
||||
* 주의: 12.5Hz 미만 ODR에서는 저잡음 모드 사용 불가
|
||||
* Power mode selection
|
||||
* 1: Low-noise mode — 800Hz ODR, high precision, higher power consumption
|
||||
* 0: Low-power mode — 100Hz ODR, lower power consumption
|
||||
* Note: Low-noise mode cannot be used with ODR below 12.5Hz
|
||||
*/
|
||||
#define USE_LOW_NOISE_MODE 1
|
||||
|
||||
/*
|
||||
* FIFO 해상도 모드 선택
|
||||
* 0: 저해상도 — 16비트 데이터 (기본)
|
||||
* 1: 고해상도 — 20비트 데이터 (FSR이 16g/2000dps로 강제 고정됨)
|
||||
* FIFO resolution mode selection
|
||||
* 0: Low resolution — 16-bit data (default)
|
||||
* 1: High resolution — 20-bit data (FSR locked to 16g/2000dps)
|
||||
*/
|
||||
#define USE_HIGH_RES_MODE 0
|
||||
|
||||
/*
|
||||
* 데이터 읽기 방식 선택
|
||||
* 0: 레지스터 직접 읽기 (현재 사용 중)
|
||||
* 1: FIFO에서 읽기
|
||||
* Data read method selection
|
||||
* 0: Direct register read (currently in use)
|
||||
* 1: Read from FIFO
|
||||
*/
|
||||
#define USE_FIFO 0
|
||||
|
||||
|
||||
/**
|
||||
* \brief IMU 디바이스를 리셋하고 초기화한다. WHOAMI 확인 포함.
|
||||
* 다른 IMU 접근 함수 호출 전에 반드시 성공적으로 실행되어야 한다.
|
||||
* \brief Resets and initializes the IMU device. Includes WHOAMI verification.
|
||||
* Must complete successfully before calling any other IMU access functions.
|
||||
*
|
||||
* \return 0=성공, 음수=에러
|
||||
* \return 0=success, negative=error
|
||||
*/
|
||||
int setup_imu_device(struct inv_imu_serif *icm_serif);
|
||||
|
||||
/**
|
||||
* \brief 자이로 및 가속도계 출력을 위한 디바이스 설정을 수행한다.
|
||||
* FSR, ODR, 전원 모드, FIFO 설정 등을 적용한다.
|
||||
* \return 0=성공, 음수=에러
|
||||
* \brief Configures the device for gyro and accel output.
|
||||
* Applies FSR, ODR, power mode, and FIFO settings.
|
||||
* \return 0=success, negative=error
|
||||
*/
|
||||
int configure_imu_device(void);
|
||||
|
||||
/**
|
||||
* \brief FIFO 또는 레지스터에서 IMU 데이터를 추출한다.
|
||||
* 내부적으로 imu_callback()이 호출되어 데이터를 처리한다.
|
||||
* \return 0=성공, 음수=에러
|
||||
* \brief Retrieves IMU data from FIFO or registers.
|
||||
* Internally triggers imu_callback() for data processing.
|
||||
* \return 0=success, negative=error
|
||||
*/
|
||||
int get_imu_data(void);
|
||||
|
||||
/**
|
||||
* \brief 센서 데이터 수신 콜백. 마운팅 매트릭스 적용 후
|
||||
* info4/BLE/UART 모드에 따라 데이터를 출력한다.
|
||||
* \param[in] event 하나의 센서 데이터 패킷을 담은 구조체
|
||||
* \brief Sensor data receive callback. Applies mounting matrix then
|
||||
* outputs data according to info4/BLE/UART mode.
|
||||
* \param[in] event Structure containing one sensor data packet
|
||||
*/
|
||||
void imu_callback(inv_imu_sensor_event_t *event);
|
||||
|
||||
/**
|
||||
* \brief 드라이버 API를 우회한 직접 I2C 레지스터 읽기.
|
||||
* DRDY 인터럽트 없이 즉시 센서 데이터를 읽어 BLE로 전송한다.
|
||||
* 읽기 후 IMU를 슬립 모드로 전환하여 전력을 절감한다.
|
||||
* \return 0=성공, 음수=에러
|
||||
* \brief Direct I2C register read bypassing the driver API.
|
||||
* Reads sensor data immediately without DRDY interrupt and sends via BLE.
|
||||
* Switches IMU to sleep mode after reading to save power.
|
||||
* \return 0=success, negative=error
|
||||
*/
|
||||
int imu_read_direct(void);
|
||||
|
||||
|
||||
@@ -8,30 +8,31 @@
|
||||
|
||||
/*******************************************************************************
|
||||
* 2026.03.26 jhChun
|
||||
* 현재 이 파일은 실제 런타임에 실행되지 않고 있음
|
||||
* 인터럽트 방식 대신 app_raw.c imu_read_direct()에서 직접 레지스터 읽는 방식 사용 중
|
||||
* 추후 필요 여부에 따라 정리 예정
|
||||
* This file is currently not executed at runtime.
|
||||
* Instead of interrupt-driven reads, imu_read_direct() in app_raw.c
|
||||
* reads registers directly. May be cleaned up later as needed.
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [모듈 개요] ICM42670P 메인 초기화 및 폴링 루프
|
||||
* [Module overview] ICM42670P main initialization and polling loop
|
||||
*
|
||||
* ICM42670P IMU 센서의 전체 초기화 시퀀스와 메인 루프를 담당한다.
|
||||
* Handles the full initialization sequence and main loop for the ICM42670P
|
||||
* IMU sensor.
|
||||
*
|
||||
* 초기화 흐름 (icm42670_init):
|
||||
* 1) setup_mcu() - I2C 시리얼 인터페이스 구조체 설정 및 TWI 초기화
|
||||
* 2) setup_imu_device() - IMU 드라이버 초기화 + WHOAMI 확인
|
||||
* 3) configure_imu_device() - 센서 파라미터 설정 (FSR, ODR, 전원 모드)
|
||||
* 4) inv_gpio_sensor_irq_init() - INT1(P1.13) GPIO 인터럽트 설정
|
||||
* Init flow (icm42670_init):
|
||||
* 1) setup_mcu() - Configure I2C serial interface struct and TWI init
|
||||
* 2) setup_imu_device() - IMU driver init + WHOAMI verification
|
||||
* 3) configure_imu_device() - Sensor parameter config (FSR, ODR, power mode)
|
||||
* 4) inv_gpio_sensor_irq_init() - INT1 (P1.13) GPIO interrupt setup
|
||||
*
|
||||
* 메인 루프 (icm42670_main):
|
||||
* - INT1 인터럽트 발생 시 irq_from_device 플래그가 세팅됨
|
||||
* - 메인 루프에서 플래그를 확인하고, 세팅되어 있으면 센서 데이터를 읽음
|
||||
* - 인터럽트는 하강 에지(HITOLO)에서 발생 (INT1 핀 풀업 설정)
|
||||
* Main loop (icm42670_main):
|
||||
* - irq_from_device flag is set when INT1 interrupt fires
|
||||
* - Main loop checks the flag and reads sensor data when set
|
||||
* - Interrupt triggers on falling edge (HITOLO) with INT1 pin pulled up
|
||||
*
|
||||
* 보조 함수:
|
||||
* - inv_imu_sleep_us() - nrf_delay_us 래퍼 (IMU 드라이버가 사용)
|
||||
* - inv_imu_get_time_us() - RTC1 카운터로 타임스탬프 제공
|
||||
* Helper functions:
|
||||
* - inv_imu_sleep_us() - nrf_delay_us wrapper (used by IMU driver)
|
||||
* - inv_imu_get_time_us() - Provides timestamp via RTC1 counter
|
||||
******************************************************************************/
|
||||
#include "sdk_config.h"
|
||||
#include "app_raw.h"
|
||||
@@ -50,7 +51,7 @@
|
||||
#include "nrf_delay.h"
|
||||
|
||||
#include "app_util_platform.h"
|
||||
#include "main.h" /* 2026-03-17: cmd_parse.h 삭제 → main.h */
|
||||
#include "main.h" /* 2026-03-17: removed cmd_parse.h, using main.h */
|
||||
#include "i2c_manager.h"
|
||||
/* --------------------------------------------------------------------------------------
|
||||
* Global variables
|
||||
@@ -61,10 +62,10 @@
|
||||
* -------------------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* IMU 인터럽트 플래그
|
||||
* INT1 핀의 하강 에지 인터럽트 발생 시 1로 세팅된다.
|
||||
* 메인 루프에서 이 플래그를 확인 후 데이터를 읽고 0으로 클리어한다.
|
||||
* volatile: ISR에서 변경되므로 컴파일러 최적화 방지
|
||||
* IMU interrupt flag
|
||||
* Set to 1 on falling-edge interrupt of INT1 pin.
|
||||
* Main loop checks this flag, reads data, and clears it to 0.
|
||||
* volatile: modified by ISR, prevents compiler optimization.
|
||||
*/
|
||||
static volatile int irq_from_device;
|
||||
|
||||
@@ -90,9 +91,9 @@ static int setup_mcu(struct inv_imu_serif *icm_serif);
|
||||
*/
|
||||
/*
|
||||
* inv_gpio_sensor_interrupt_handler()
|
||||
* INT1 핀 인터럽트 핸들러 (ISR).
|
||||
* 센서가 새 데이터를 준비했을 때 호출되며, 플래그만 세팅하고 즉시 반환한다.
|
||||
* 실제 데이터 처리는 메인 루프(icm42670_main)에서 수행한다.
|
||||
* INT1 pin interrupt handler (ISR).
|
||||
* Called when the sensor has new data ready; sets a flag and returns immediately.
|
||||
* Actual data processing is done in the main loop (icm42670_main).
|
||||
*/
|
||||
static void inv_gpio_sensor_interrupt_handler(nrfx_gpiote_pin_t pin, nrf_gpiote_polarity_t action)
|
||||
{
|
||||
@@ -102,52 +103,52 @@ static void inv_gpio_sensor_interrupt_handler(nrfx_gpiote_pin_t pin, nrf_gpiote_
|
||||
|
||||
/*
|
||||
* inv_gpio_sensor_irq_init()
|
||||
* INT1(P1.13) GPIO 인터럽트를 초기화한다.
|
||||
* Initializes INT1 (P1.13) GPIO interrupt.
|
||||
*
|
||||
* 설정:
|
||||
* - 트리거: 하강 에지 (HITOLO) — 센서가 INT를 Low로 끌어내릴 때
|
||||
* - 풀업 저항: 내부 풀업 활성화
|
||||
* - 핸들러: inv_gpio_sensor_interrupt_handler
|
||||
* - GPIOTE 모듈이 미초기화 상태이면 먼저 초기화
|
||||
* Configuration:
|
||||
* - Trigger: falling edge (HITOLO) — when the sensor pulls INT low
|
||||
* - Pull-up: internal pull-up enabled
|
||||
* - Handler: inv_gpio_sensor_interrupt_handler
|
||||
* - Initializes GPIOTE module first if not already initialized
|
||||
*/
|
||||
void inv_gpio_sensor_irq_init(void)
|
||||
{
|
||||
ret_code_t err_code;
|
||||
|
||||
/* GPIOTE 모듈 초기화 (이미 초기화되어 있으면 건너뜀) */
|
||||
/* Initialize GPIOTE module (skip if already initialized) */
|
||||
if (!nrfx_gpiote_is_init())
|
||||
{
|
||||
err_code = nrfx_gpiote_init();
|
||||
APP_ERROR_CHECK(err_code);
|
||||
}
|
||||
|
||||
/* 하강 에지 인터럽트 설정: High→Low 전환 시 트리거, 내부 풀업 사용 */
|
||||
/* Falling-edge interrupt: trigger on High->Low transition, internal pull-up */
|
||||
nrfx_gpiote_in_config_t in_config = NRFX_GPIOTE_CONFIG_IN_SENSE_HITOLO(true);
|
||||
in_config.pull = NRF_GPIO_PIN_PULLUP;
|
||||
|
||||
/* INT1 핀에 인터럽트 핸들러 등록 */
|
||||
/* Register interrupt handler for INT1 pin */
|
||||
err_code = nrfx_gpiote_in_init(ICM42670_INT1_PIN, &in_config, inv_gpio_sensor_interrupt_handler);
|
||||
APP_ERROR_CHECK(err_code);
|
||||
|
||||
/* 인터럽트 이벤트 활성화 */
|
||||
/* Enable interrupt event */
|
||||
nrfx_gpiote_in_event_enable(ICM42670_INT1_PIN, true);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* inv_gpio_sensor_irq_uninit()
|
||||
* INT1 GPIO 인터럽트를 비활성화하고 해제한다.
|
||||
* 센서 비활성화 시 또는 재초기화 전에 호출된다.
|
||||
* Disables and releases the INT1 GPIO interrupt.
|
||||
* Called when deactivating the sensor or before re-initialization.
|
||||
*/
|
||||
void inv_gpio_sensor_irq_uninit(void)
|
||||
{
|
||||
/* 인터럽트 이벤트 비활성화 */
|
||||
/* Disable interrupt event */
|
||||
nrfx_gpiote_in_event_disable(ICM42670_INT1_PIN);
|
||||
|
||||
/* INT1 핀 인터럽트 설정 해제 */
|
||||
/* Release INT1 pin interrupt configuration */
|
||||
nrfx_gpiote_in_uninit(ICM42670_INT1_PIN);
|
||||
|
||||
/* GPIOTE 모듈 해제 (초기화된 경우에만) */
|
||||
/* Release GPIOTE module (only if initialized) */
|
||||
if (nrfx_gpiote_is_init())
|
||||
{
|
||||
nrfx_gpiote_uninit();
|
||||
@@ -161,15 +162,15 @@ void inv_gpio_sensor_irq_uninit(void)
|
||||
|
||||
/*
|
||||
* icm42670_init()
|
||||
* ICM42670P 전체 초기화 시퀀스를 수행한다.
|
||||
* Performs the full ICM42670P initialization sequence.
|
||||
*
|
||||
* 초기화 순서:
|
||||
* 1) setup_mcu() - I2C 인터페이스 구조체 설정 및 TWI 하드웨어 초기화
|
||||
* 2) setup_imu_device() - IMU 드라이버 초기화, WHOAMI(0x67) 확인
|
||||
* 3) configure_imu_device() - FSR, ODR, 전원 모드 설정
|
||||
* 4) inv_gpio_sensor_irq_init() - INT1 인터럽트 활성화 (데이터 준비 알림)
|
||||
* Init order:
|
||||
* 1) setup_mcu() - Configure I2C interface struct and TWI hardware init
|
||||
* 2) setup_imu_device() - IMU driver init, WHOAMI (0x67) verification
|
||||
* 3) configure_imu_device() - FSR, ODR, power mode configuration
|
||||
* 4) inv_gpio_sensor_irq_init() - Enable INT1 interrupt (data-ready notification)
|
||||
*
|
||||
* 반환값: 0=성공, -1=초기화 실패
|
||||
* Returns: 0=success, -1=initialization failure
|
||||
*/
|
||||
int icm42670_init(void)
|
||||
{
|
||||
@@ -185,7 +186,7 @@ int icm42670_init(void)
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* 초기화 성공 후 INT1 인터럽트 활성화 — 이후 데이터 준비 시 ISR이 호출됨 */
|
||||
/* Enable INT1 interrupt after successful init — ISR fires on data ready */
|
||||
inv_gpio_sensor_irq_init();
|
||||
|
||||
return rc;
|
||||
@@ -194,22 +195,23 @@ int icm42670_init(void)
|
||||
|
||||
/*
|
||||
* icm42670_main()
|
||||
* ICM42670P 메인 폴링 루프.
|
||||
* 메인 애플리케이션 루프에서 주기적으로 호출되어야 한다.
|
||||
* ICM42670P main polling loop.
|
||||
* Must be called periodically from the main application loop.
|
||||
*
|
||||
* 동작:
|
||||
* 1) I2C 하드웨어가 초기화되었는지 확인 (hw_i2c_init_once)
|
||||
* 2) irq_from_device 플래그 확인 (ISR에서 세팅됨)
|
||||
* 3) 플래그가 세팅되어 있으면 센서 데이터 읽기 (get_imu_data)
|
||||
* 4) 데이터 읽기 완료 후 플래그 클리어
|
||||
* Operation:
|
||||
* 1) Check if I2C hardware is initialized (hw_i2c_init_once)
|
||||
* 2) Check irq_from_device flag (set by ISR)
|
||||
* 3) If flag is set, read sensor data (get_imu_data)
|
||||
* 4) Clear flag after data read completes
|
||||
*
|
||||
* 참고: 인터럽트 기반 폴링 방식으로, ISR에서는 플래그만 세팅하고 실제 I2C 통신은 메인 컨텍스트에서 수행한다.
|
||||
* Note: Interrupt-based polling — ISR only sets the flag; actual I2C
|
||||
* communication is done in the main context.
|
||||
*/
|
||||
void icm42670_main(void)
|
||||
{
|
||||
int rc = 0;
|
||||
hw_i2c_init_once();
|
||||
/* 인터럽트 발생 여부 확인 후 데이터 읽기 */
|
||||
/* Check for interrupt and read data */
|
||||
|
||||
if (irq_from_device) {
|
||||
rc = get_imu_data();
|
||||
@@ -218,7 +220,7 @@ void icm42670_main(void)
|
||||
printf("error while getting data\r\n");
|
||||
}
|
||||
|
||||
/* 플래그 클리어 — 다음 인터럽트까지 대기 */
|
||||
/* Clear flag — wait for next interrupt */
|
||||
irq_from_device = 0;
|
||||
}
|
||||
|
||||
@@ -231,28 +233,28 @@ void icm42670_main(void)
|
||||
|
||||
/*
|
||||
* setup_mcu()
|
||||
* MCU 측 시리얼 인터페이스를 설정한다.
|
||||
* Configures the MCU-side serial interface.
|
||||
*
|
||||
* inv_imu_serif 구조체에 다음을 등록:
|
||||
* - read_reg / write_reg : I2C 읽기/쓰기 콜백 함수 (system_interface.c에서 구현)
|
||||
* - max_read / max_write : 최대 전송 크기 (32KB)
|
||||
* - serif_type : 통신 타입 (UI_I2C)
|
||||
* Registers the following in the inv_imu_serif struct:
|
||||
* - read_reg / write_reg : I2C read/write callbacks (implemented in system_interface.c)
|
||||
* - max_read / max_write : Max transfer size (32KB)
|
||||
* - serif_type : Communication type (UI_I2C)
|
||||
*
|
||||
* 설정 후 inv_io_hal_init()을 호출하여 실제 TWI 하드웨어를 초기화한다.
|
||||
* After configuration, calls inv_io_hal_init() to initialize the TWI hardware.
|
||||
*/
|
||||
static int setup_mcu(struct inv_imu_serif *icm_serif)
|
||||
{
|
||||
int rc = 0;
|
||||
|
||||
/* IMU 드라이버용 시리얼 인터페이스 구조체 설정 */
|
||||
icm_serif->context = 0; /* 컨텍스트 미사용 */
|
||||
icm_serif->read_reg = inv_io_hal_read_reg; /* 레지스터 읽기 콜백 */
|
||||
icm_serif->write_reg = inv_io_hal_write_reg; /* 레지스터 쓰기 콜백 */
|
||||
icm_serif->max_read = 1024*32; /* 1회 읽기 최대 바이트 수 */
|
||||
icm_serif->max_write = 1024*32; /* 1회 쓰기 최대 바이트 수 */
|
||||
icm_serif->serif_type = SERIF_TYPE; /* UI_I2C (app_raw.h에서 정의) */
|
||||
/* Configure serial interface struct for IMU driver */
|
||||
icm_serif->context = 0; /* Context unused */
|
||||
icm_serif->read_reg = inv_io_hal_read_reg; /* Register read callback */
|
||||
icm_serif->write_reg = inv_io_hal_write_reg; /* Register write callback */
|
||||
icm_serif->max_read = 1024*32; /* Max bytes per read */
|
||||
icm_serif->max_write = 1024*32; /* Max bytes per write */
|
||||
icm_serif->serif_type = SERIF_TYPE; /* UI_I2C (defined in app_raw.h) */
|
||||
|
||||
/* TWI 하드웨어 초기화 */
|
||||
/* Initialize TWI hardware */
|
||||
rc |= inv_io_hal_init(icm_serif);
|
||||
|
||||
return rc;
|
||||
@@ -265,9 +267,9 @@ static int setup_mcu(struct inv_imu_serif *icm_serif)
|
||||
|
||||
/*
|
||||
* inv_imu_sleep_us()
|
||||
* IMU 드라이버가 사용하는 마이크로초 단위 슬립 함수.
|
||||
* nrf_delay_us()를 래핑하여 플랫폼 독립적 인터페이스를 제공한다.
|
||||
* 예: 자이로 스타트업 대기(GYR_STARTUP_TIME_US) 시 사용
|
||||
* Microsecond sleep function used by the IMU driver.
|
||||
* Wraps nrf_delay_us() to provide a platform-independent interface.
|
||||
* Example: used for gyro startup delay (GYR_STARTUP_TIME_US).
|
||||
*/
|
||||
void inv_imu_sleep_us(uint32_t us)
|
||||
{
|
||||
@@ -277,12 +279,12 @@ void inv_imu_sleep_us(uint32_t us)
|
||||
|
||||
/*
|
||||
* inv_imu_get_time_us()
|
||||
* IMU 드라이버가 사용하는 타임스탬프 함수.
|
||||
* nRF52840의 RTC1 카운터 값을 반환한다.
|
||||
* Timestamp function used by the IMU driver.
|
||||
* Returns the nRF52840 RTC1 counter value.
|
||||
*
|
||||
* 주의: RTC1은 32.768kHz로 동작하므로, 반환값의 단위는 엄밀히
|
||||
* 마이크로초가 아닌 RTC 틱(약 30.5us/tick)이다.
|
||||
* 드라이버 내부에서 상대적 시간 비교 용도로 사용된다.
|
||||
* Note: RTC1 runs at 32.768kHz, so the returned value is technically
|
||||
* in RTC ticks (~30.5us/tick), not microseconds.
|
||||
* Used for relative time comparisons within the driver.
|
||||
*/
|
||||
uint64_t inv_imu_get_time_us(void)
|
||||
{
|
||||
|
||||
@@ -7,25 +7,26 @@
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [헤더 개요] ICM42670P 메인 초기화/폴링 루프 선언
|
||||
* [Header overview] ICM42670P main initialization/polling loop declarations
|
||||
*
|
||||
* ICM42670P IMU 센서의 전체 초기화 및 메인 루프 함수를 선언한다.
|
||||
* - icm42670_init() : 전체 초기화 (MCU설정 → IMU초기화 → 센서설정 → 인터럽트 활성화)
|
||||
* - icm42670_main() : 메인 폴링 루프 (INT1 인터럽트 확인 → 데이터 읽기)
|
||||
* - icm42670_uninit() : 해제 (프로토타입만 선언, 구현은 별도)
|
||||
* Declares the full initialization and main loop functions for the
|
||||
* ICM42670P IMU sensor.
|
||||
* - icm42670_init() : Full init (MCU config -> IMU init -> sensor config -> enable IRQ)
|
||||
* - icm42670_main() : Main polling loop (check INT1 interrupt -> read data)
|
||||
* - icm42670_uninit() : Release (prototype only, implementation elsewhere)
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef _APP_RAW_MAIN_H_
|
||||
#define _APP_RAW_MAIN_H_
|
||||
#include "sdk_config.h"
|
||||
|
||||
/* ICM42670P 전체 초기화 — MCU I2C 설정 → IMU 드라이버 초기화 → 센서 설정 → 인터럽트 활성화 */
|
||||
/* ICM42670P full init — MCU I2C config -> IMU driver init -> sensor config -> enable IRQ */
|
||||
int icm42670_init(void);
|
||||
|
||||
/* ICM42670P 메인 폴링 루프 — INT1 인터럽트 플래그 확인 후 센서 데이터 읽기 */
|
||||
/* ICM42670P main polling loop — check INT1 interrupt flag, then read sensor data */
|
||||
void icm42670_main(void);
|
||||
|
||||
/* ICM42670P 해제 (프로토타입 선언) */
|
||||
/* ICM42670P release (prototype declaration) */
|
||||
int icm42670_uninit(void);
|
||||
|
||||
#endif /* !_APP_RAW_MAIN_H_ */
|
||||
|
||||
@@ -7,25 +7,25 @@
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [모듈 개요] ICM42670P IMU 센서 I2C 통신 인터페이스
|
||||
* [Module overview] ICM42670P IMU sensor I2C communication interface
|
||||
*
|
||||
* nRF52840의 TWI(I2C) 하드웨어를 사용하여 ICM42670P IMU 센서와 통신하는
|
||||
* 저수준 인터페이스 모듈이다.
|
||||
* Low-level interface module for communicating with the ICM42670P IMU sensor
|
||||
* via the nRF52840 TWI (I2C) hardware.
|
||||
*
|
||||
* - I2C 슬레이브 주소: 0x68 (ICM42670P 기본 주소)
|
||||
* - I2C 핀 설정: SCL=P1.14, SDA=P1.15 (system_interface.h에서 정의)
|
||||
* - TWI 인스턴스: NRFX_TWI_INSTANCE(0) 사용
|
||||
* - 통신 속도: 100kHz (NRF_TWI_FREQ_100K)
|
||||
* - I2C slave address: 0x68 (ICM42670P default)
|
||||
* - I2C pin config: SCL=P1.14, SDA=P1.15 (defined in system_interface.h)
|
||||
* - TWI instance: NRFX_TWI_INSTANCE(0)
|
||||
* - Bus speed: 100kHz (NRF_TWI_FREQ_100K)
|
||||
*
|
||||
* 주요 함수 흐름:
|
||||
* inv_io_hal_init() → I2C 또는 SPI 초기화 (현재 I2C만 구현)
|
||||
* inv_io_hal_read_reg() → 레지스터 읽기 (TX로 주소 전송 → RX로 데이터 수신)
|
||||
* inv_io_hal_write_reg() → 레지스터 쓰기 (주소+데이터를 한번에 TX)
|
||||
* Main function flow:
|
||||
* inv_io_hal_init() -> Initialize I2C or SPI (only I2C implemented)
|
||||
* inv_io_hal_read_reg() -> Register read (TX address -> RX data)
|
||||
* inv_io_hal_write_reg() -> Register write (TX address+data at once)
|
||||
*
|
||||
* 에러 처리: 모든 I2C 읽기/쓰기에서 실패 시 1회 재시도 후 에러 출력
|
||||
* Error handling: All I2C read/write operations retry once on failure
|
||||
*
|
||||
* 참고: SPI4 인터페이스 코드 경로도 존재하지만 현재 미구현 상태이며,
|
||||
* 실제 운용에서는 I2C(UI_I2C)만 사용한다.
|
||||
* Note: SPI4 code path exists but is not implemented;
|
||||
* only I2C (UI_I2C) is used in production.
|
||||
******************************************************************************/
|
||||
|
||||
/* board driver */
|
||||
@@ -43,17 +43,17 @@
|
||||
#include "system_interface.h"
|
||||
#include "nrf_delay.h"
|
||||
|
||||
/* ICM42670P I2C 슬레이브 주소 및 직렬 쓰기 최대 바이트 수 */
|
||||
/* ICM42670P I2C slave address and max serial write byte count */
|
||||
#define ICM_I2C_ADDR 0x68
|
||||
#define INV_MAX_SERIAL_WRITE 16
|
||||
|
||||
/* TWI(I2C) 인스턴스 생성 — system_interface.h의 ICM42670_I2C_INSTANCE(0)을 사용 */
|
||||
/* TWI (I2C) instance — uses ICM42670_I2C_INSTANCE(0) from system_interface.h */
|
||||
const nrfx_twi_t m_twi_icm42670 = NRFX_TWI_INSTANCE(ICM42670_I2C_INSTANCE);
|
||||
|
||||
/*
|
||||
* inv_i2c_master_uninitialize()
|
||||
* I2C 버스를 비활성화하고 TWI 인스턴스를 해제한다.
|
||||
* 슬립 모드 진입 전 또는 재초기화 전에 호출된다.
|
||||
* Disables the I2C bus and releases the TWI instance.
|
||||
* Called before entering sleep mode or before re-initialization.
|
||||
*/
|
||||
void inv_i2c_master_uninitialize(void){
|
||||
nrfx_twi_disable(&m_twi_icm42670);
|
||||
@@ -62,11 +62,11 @@ void inv_i2c_master_uninitialize(void){
|
||||
|
||||
/*
|
||||
* inv_i2c_master_initialize()
|
||||
* nRF52840 TWI 하드웨어를 초기화하고 활성화한다.
|
||||
* Initializes and enables the nRF52840 TWI hardware.
|
||||
* - SCL: P1.14, SDA: P1.15
|
||||
* - 속도: 100kHz
|
||||
* - 인터럽트 우선순위: 최고 (APP_IRQ_PRIORITY_HIGH)
|
||||
* - 이벤트 핸들러 없음 (블로킹 모드로 동작)
|
||||
* - Speed: 100kHz
|
||||
* - Interrupt priority: highest (APP_IRQ_PRIORITY_HIGH)
|
||||
* - No event handler (blocking mode)
|
||||
*/
|
||||
void inv_i2c_master_initialize(void){
|
||||
ret_code_t err_code;
|
||||
@@ -78,19 +78,19 @@ void inv_i2c_master_initialize(void){
|
||||
.interrupt_priority = APP_IRQ_PRIORITY_HIGH,
|
||||
};
|
||||
|
||||
/* TWI 드라이버 초기화 (이벤트 핸들러=NULL → 블로킹 모드) */
|
||||
/* Initialize TWI driver (event handler=NULL -> blocking mode) */
|
||||
err_code = nrfx_twi_init(&m_twi_icm42670, &twi_icm42670_config, NULL, NULL);
|
||||
APP_ERROR_CHECK(err_code);
|
||||
|
||||
/* TWI 하드웨어 활성화 — 이후 tx/rx 가능 */
|
||||
/* Enable TWI hardware — tx/rx available after this */
|
||||
nrfx_twi_enable(&m_twi_icm42670);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* icm42670_twi_tx()
|
||||
* I2C 전송 래퍼 함수. nrfx_twi_tx를 호출하여 데이터를 송신한다.
|
||||
* no_stop=true이면 STOP 컨디션을 보내지 않음 (Repeated START를 위해 사용)
|
||||
* I2C transmit wrapper. Calls nrfx_twi_tx to send data.
|
||||
* If no_stop=true, STOP condition is omitted (used for Repeated START).
|
||||
*/
|
||||
uint32_t icm42670_twi_tx( uint8_t device_id,
|
||||
uint8_t const * p_data,
|
||||
@@ -105,7 +105,7 @@ uint32_t icm42670_twi_tx( uint8_t device_id,
|
||||
|
||||
/*
|
||||
* icm42670_twi_rx()
|
||||
* I2C 수신 래퍼 함수. nrfx_twi_rx를 호출하여 데이터를 수신한다.
|
||||
* I2C receive wrapper. Calls nrfx_twi_rx to receive data.
|
||||
*/
|
||||
uint32_t icm42670_twi_rx( uint8_t device_id,
|
||||
uint8_t * p_data,
|
||||
@@ -119,33 +119,33 @@ uint32_t icm42670_twi_rx( uint8_t device_id,
|
||||
|
||||
/*
|
||||
* inv_i2c_master_read_register()
|
||||
* ICM42670P의 특정 레지스터에서 데이터를 읽는다.
|
||||
* Reads data from a specific ICM42670P register.
|
||||
*
|
||||
* 동작 순서:
|
||||
* 1) TX: 레지스터 주소 1바이트 전송 (no_stop=true → Repeated START 준비)
|
||||
* 2) RX: 지정된 길이만큼 데이터 수신
|
||||
* Sequence:
|
||||
* 1) TX: Send 1-byte register address (no_stop=true -> prepare Repeated START)
|
||||
* 2) RX: Receive data of specified length
|
||||
*
|
||||
* 에러 처리: TX, RX 각각 실패 시 1회 재시도한다.
|
||||
* Error handling: Retries once on TX or RX failure.
|
||||
*/
|
||||
static unsigned long inv_i2c_master_read_register(unsigned char Address, unsigned char RegisterAddr, unsigned short RegisterLen, unsigned char *RegisterValue){
|
||||
//ret_code_t ret;
|
||||
uint32_t ret;
|
||||
uint8_t addr8 = (uint8_t)RegisterAddr;
|
||||
|
||||
/* 1단계: 읽을 레지스터 주소를 전송 (STOP 없이 → Repeated START 사용) */
|
||||
/* Step 1: Send register address to read (no STOP -> uses Repeated START) */
|
||||
ret = icm42670_twi_tx(Address, &addr8, 1, true);
|
||||
if(ret != NRF_SUCCESS) {
|
||||
/* 실패 시 1회 재시도 */
|
||||
/* Retry once on failure */
|
||||
ret = icm42670_twi_tx(Address, &addr8, 1, true);
|
||||
if(ret != NRF_SUCCESS) {
|
||||
printf("ERR! i2c read-1\r\n");
|
||||
}
|
||||
}
|
||||
|
||||
/* 2단계: 해당 레지스터에서 데이터 수신 */
|
||||
/* Step 2: Receive data from the register */
|
||||
ret = icm42670_twi_rx(Address, RegisterValue, RegisterLen);
|
||||
if(ret != NRF_SUCCESS) {
|
||||
/* 실패 시 1회 재시도 */
|
||||
/* Retry once on failure */
|
||||
ret = icm42670_twi_rx(Address, RegisterValue, RegisterLen);
|
||||
if(ret != NRF_SUCCESS) {
|
||||
printf("ERR! i2c read-2\r\n");
|
||||
@@ -157,26 +157,26 @@ static unsigned long inv_i2c_master_read_register(unsigned char Address, unsigne
|
||||
|
||||
/*
|
||||
* inv_i2c_master_write_register()
|
||||
* ICM42670P의 특정 레지스터에 데이터를 쓴다.
|
||||
* Writes data to a specific ICM42670P register.
|
||||
*
|
||||
* 동작 순서:
|
||||
* 1) 버퍼[0]에 레지스터 주소, 버퍼[1~N]에 쓸 데이터를 배치
|
||||
* 2) TX: 주소+데이터를 한번에 전송 (no_stop=false → STOP 컨디션 포함)
|
||||
* Sequence:
|
||||
* 1) Place register address in buffer[0], data in buffer[1..N]
|
||||
* 2) TX: Send address+data at once (no_stop=false -> includes STOP condition)
|
||||
*
|
||||
* 에러 처리: 실패 시 1회 재시도한다.
|
||||
* Error handling: Retries once on failure.
|
||||
*/
|
||||
static unsigned long inv_i2c_master_write_register(unsigned char Address, unsigned char RegisterAddr, unsigned short RegisterLen, const unsigned char *RegisterValue){
|
||||
uint32_t ret;
|
||||
uint8_t buffer[1 + INV_MAX_SERIAL_WRITE]; /* 레지스터 주소(1) + 데이터(최대 16바이트) */
|
||||
uint8_t buffer[1 + INV_MAX_SERIAL_WRITE]; /* register address (1) + data (max 16 bytes) */
|
||||
|
||||
/* 버퍼 구성: [레지스터 주소][데이터 바이트들] */
|
||||
/* Buffer layout: [register address][data bytes] */
|
||||
buffer[0] = (uint8_t)RegisterAddr;
|
||||
memcpy(buffer+1, RegisterValue, RegisterLen);
|
||||
|
||||
/* 주소+데이터를 한번에 전송 */
|
||||
/* Send address+data at once */
|
||||
ret = icm42670_twi_tx(Address, buffer, RegisterLen+1, false);
|
||||
if(ret != NRF_SUCCESS) {
|
||||
/* 실패 시 1회 재시도 */
|
||||
/* Retry once on failure */
|
||||
ret = icm42670_twi_tx(Address, buffer, RegisterLen+1, false);
|
||||
if(ret != NRF_SUCCESS) {
|
||||
printf("ERR! i2c write\r\n");
|
||||
@@ -190,9 +190,9 @@ static unsigned long inv_i2c_master_write_register(unsigned char Address, unsign
|
||||
|
||||
/*
|
||||
* inv_io_hal_init()
|
||||
* IMU 드라이버가 사용하는 시리얼 인터페이스(I2C 또는 SPI)를 초기화한다.
|
||||
* serif->serif_type에 따라 분기하며, 현재는 I2C만 구현되어 있다.
|
||||
* 반환값: 0=성공, -1=지원하지 않는 인터페이스 타입
|
||||
* Initializes the serial interface (I2C or SPI) used by the IMU driver.
|
||||
* Branches based on serif->serif_type; only I2C is currently implemented.
|
||||
* Returns: 0=success, -1=unsupported interface type
|
||||
*/
|
||||
|
||||
int inv_io_hal_init(struct inv_imu_serif *serif)
|
||||
@@ -201,7 +201,7 @@ int inv_io_hal_init(struct inv_imu_serif *serif)
|
||||
switch (serif->serif_type) {
|
||||
case UI_SPI4:
|
||||
{
|
||||
/* SPI4 초기화 — 현재 미구현 (I2C만 사용) */
|
||||
/* SPI4 init — not implemented (only I2C is used) */
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -219,8 +219,8 @@ int inv_io_hal_init(struct inv_imu_serif *serif)
|
||||
|
||||
/*
|
||||
* inv_io_hal_read_reg()
|
||||
* IMU 드라이버 콜백: 지정된 레지스터에서 데이터를 읽는다.
|
||||
* 시리얼 타입에 따라 I2C 또는 SPI 읽기를 수행한다.
|
||||
* IMU driver callback: reads data from the specified register.
|
||||
* Performs I2C or SPI read depending on the serial type.
|
||||
*/
|
||||
int inv_io_hal_read_reg(struct inv_imu_serif *serif, uint8_t reg, uint8_t * rbuffer, uint32_t rlen)
|
||||
{
|
||||
@@ -238,8 +238,8 @@ int inv_io_hal_read_reg(struct inv_imu_serif *serif, uint8_t reg, uint8_t * rbuf
|
||||
|
||||
/*
|
||||
* inv_io_hal_write_reg()
|
||||
* IMU 드라이버 콜백: 지정된 레지스터에 데이터를 쓴다.
|
||||
* 시리얼 타입에 따라 I2C 또는 SPI 쓰기를 수행한다.
|
||||
* IMU driver callback: writes data to the specified register.
|
||||
* Performs I2C or SPI write depending on the serial type.
|
||||
*/
|
||||
int inv_io_hal_write_reg(struct inv_imu_serif *serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen)
|
||||
{
|
||||
@@ -257,9 +257,9 @@ int inv_io_hal_write_reg(struct inv_imu_serif *serif, uint8_t reg, const uint8_t
|
||||
|
||||
/*
|
||||
* cat_read()
|
||||
* 범용 I2C 읽기 함수 (디버그/레거시용).
|
||||
* 8바이트를 읽어 첫 번째 바이트를 반환하고, 읽은 데이터를 콘솔에 출력한다.
|
||||
* 참고: 현재 실제 운용에서는 사용되지 않으며, 디버그 목적으로 남아 있다.
|
||||
* Generic I2C read function (debug/legacy).
|
||||
* Reads 8 bytes, returns the first byte, and prints the data to console.
|
||||
* Note: Not used in production; kept for debug purposes.
|
||||
*/
|
||||
uint8_t cat_read(uint8_t device_id, uint8_t address, uint8_t *data)
|
||||
{
|
||||
@@ -270,14 +270,14 @@ uint8_t cat_read(uint8_t device_id, uint8_t address, uint8_t *data)
|
||||
//address = 1|(address<<1);
|
||||
address = (address & 0xFF);
|
||||
|
||||
/* 레지스터 주소 전송 (STOP 없이, Repeated START 준비) */
|
||||
/* Send register address (no STOP, prepare Repeated START) */
|
||||
err_code = nrfx_twi_tx(&m_twi_icm42670, device_id, &address, 1, true);
|
||||
if (err_code != NRF_SUCCESS) {
|
||||
// Handle error
|
||||
// return;
|
||||
}
|
||||
|
||||
/* 8바이트 데이터 수신 */
|
||||
/* Receive 8 bytes of data */
|
||||
err_code = nrfx_twi_rx(&m_twi_icm42670, device_id, data, 8);
|
||||
if (err_code != NRF_SUCCESS) {
|
||||
// Handle error
|
||||
@@ -298,9 +298,9 @@ uint8_t cat_read(uint8_t device_id, uint8_t address, uint8_t *data)
|
||||
|
||||
/*
|
||||
* cat_write()
|
||||
* 범용 I2C 쓰기 함수 (디버그/레거시용).
|
||||
* 주소 1바이트 + 데이터 1바이트를 전송한다.
|
||||
* 참고: buffer에 6바이트를 복사하지만, 실제 전송은 2바이트만 수행한다.
|
||||
* Generic I2C write function (debug/legacy).
|
||||
* Sends 1 byte address + 1 byte data.
|
||||
* Note: Copies 6 bytes into buffer, but only transmits 2 bytes.
|
||||
*/
|
||||
void cat_write(uint8_t device_id, uint8_t address, uint8_t *data){
|
||||
|
||||
@@ -314,7 +314,7 @@ void cat_write(uint8_t device_id, uint8_t address, uint8_t *data){
|
||||
ret_code_t err_code;
|
||||
//err_code = nrf_drv_twi_tx(&m_twi_ir, device_id, 0x00, 1, false);
|
||||
|
||||
/* 주소(1바이트) + 데이터(1바이트) = 2바이트 전송 */
|
||||
/* Address (1 byte) + data (1 byte) = 2 bytes transmitted */
|
||||
err_code = nrfx_twi_tx(&m_twi_icm42670, device_id, buffer, 2, false);
|
||||
// err_code = nrf_drv_twi_tx(&m_twi_ir, device_id, buffer, 2, false);
|
||||
// nrfx_twi_rx(&m_twi_icm42670, device_id, p_data, length);
|
||||
|
||||
@@ -7,17 +7,18 @@
|
||||
******************************************************************************/
|
||||
|
||||
/*******************************************************************************
|
||||
* [헤더 개요] ICM42670P I2C 통신 인터페이스 선언
|
||||
* [Header overview] ICM42670P I2C communication interface declarations
|
||||
*
|
||||
* nRF52840 TWI 하드웨어를 통해 ICM42670P IMU 센서와 통신하기 위한 핀 정의, 함수 프로토타입 선언
|
||||
* Pin definitions and function prototypes for communicating with the
|
||||
* ICM42670P IMU sensor via nRF52840 TWI hardware.
|
||||
*
|
||||
* 핀 배치:
|
||||
* Pin assignment:
|
||||
* - I2C SCL : P1.14
|
||||
* - I2C SDA : P1.15
|
||||
* - INT1 : P1.13 (데이터 준비 인터럽트)
|
||||
* - INT2 : P0.26 (보조 인터럽트, 현재 미사용)
|
||||
* - INT1 : P1.13 (data-ready interrupt)
|
||||
* - INT2 : P0.26 (auxiliary interrupt, currently unused)
|
||||
*
|
||||
* TWI 인스턴스: 0번 사용
|
||||
* TWI instance: 0
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef _SYSTEM_INTERFACE_H_
|
||||
@@ -31,42 +32,42 @@
|
||||
#endif
|
||||
|
||||
|
||||
#define ICM42670_I2C_INSTANCE 0 /**< I2C(TWI) 인스턴스 인덱스 */
|
||||
#define ICM42670_I2C_SDA_PIN NRF_GPIO_PIN_MAP(1,15) /**< SDA 핀: P1.15 */
|
||||
#define ICM42670_I2C_SCL_PIN NRF_GPIO_PIN_MAP(1,14) /**< SCL 핀: P1.14 */
|
||||
#define ICM42670_INT1_PIN NRF_GPIO_PIN_MAP(1,13) /**< INT1 핀: P1.13 (데이터 준비 인터럽트) */
|
||||
#define ICM42670_INT2_PIN NRF_GPIO_PIN_MAP(0,26) /**< INT2 핀: P0.26 (보조, 현재 미사용) */
|
||||
#define ICM42670_I2C_INSTANCE 0 /**< I2C (TWI) instance index */
|
||||
#define ICM42670_I2C_SDA_PIN NRF_GPIO_PIN_MAP(1,15) /**< SDA pin: P1.15 */
|
||||
#define ICM42670_I2C_SCL_PIN NRF_GPIO_PIN_MAP(1,14) /**< SCL pin: P1.14 */
|
||||
#define ICM42670_INT1_PIN NRF_GPIO_PIN_MAP(1,13) /**< INT1 pin: P1.13 (data-ready interrupt) */
|
||||
#define ICM42670_INT2_PIN NRF_GPIO_PIN_MAP(0,26) /**< INT2 pin: P0.26 (auxiliary, currently unused) */
|
||||
|
||||
/* I2C 전송 래퍼 — no_stop=true이면 Repeated START를 위해 STOP 컨디션 생략 */
|
||||
/* I2C transmit wrapper — if no_stop=true, STOP condition is omitted for Repeated START */
|
||||
uint32_t icm42670_twi_tx( uint8_t device_id,
|
||||
uint8_t const * p_data,
|
||||
uint8_t length,
|
||||
bool no_stop);
|
||||
|
||||
/* I2C 수신 래퍼 */
|
||||
/* I2C receive wrapper */
|
||||
uint32_t icm42670_twi_rx( uint8_t device_id,
|
||||
uint8_t * p_data,
|
||||
uint8_t length);
|
||||
|
||||
/* 범용 I2C 읽기 (디버그/레거시용) — 8바이트를 읽어 첫 바이트 반환 */
|
||||
/* Generic I2C read (debug/legacy) — reads 8 bytes and returns the first byte */
|
||||
uint8_t cat_read (uint8_t device_id, uint8_t address, uint8_t *data);
|
||||
|
||||
/* 범용 I2C 쓰기 (디버그/레거시용) — 주소+데이터 2바이트 전송 */
|
||||
/* Generic I2C write (debug/legacy) — sends address+data 2 bytes */
|
||||
void cat_write (uint8_t device_id, uint8_t address, uint8_t *data);
|
||||
|
||||
/* I2C 하드웨어 해제 (슬립 또는 재초기화 전 호출) */
|
||||
/* Release I2C hardware (called before sleep or re-initialization) */
|
||||
void inv_i2c_master_uninitialize(void);
|
||||
|
||||
/* I2C 하드웨어 초기화 (100kHz, 블로킹 모드) */
|
||||
/* Initialize I2C hardware (100kHz, blocking mode) */
|
||||
void inv_i2c_master_initialize(void);
|
||||
|
||||
/* IMU 드라이버용 시리얼 인터페이스 초기화 (I2C/SPI 분기) */
|
||||
/* Initialize serial interface for IMU driver (I2C/SPI branch) */
|
||||
int inv_io_hal_init(struct inv_imu_serif *serif);
|
||||
|
||||
/* IMU 드라이버 콜백: 레지스터 읽기 */
|
||||
/* IMU driver callback: register read */
|
||||
int inv_io_hal_read_reg(struct inv_imu_serif *serif, uint8_t reg, uint8_t * rbuffer, uint32_t rlen);
|
||||
|
||||
/* IMU 드라이버 콜백: 레지스터 쓰기 */
|
||||
/* IMU driver callback: register write */
|
||||
int inv_io_hal_write_reg(struct inv_imu_serif *serif, uint8_t reg, const uint8_t * wbuffer, uint32_t wlen);
|
||||
|
||||
#endif /* !_SYSTEM_INTERFACE_H_ */
|
||||
|
||||
Reference in New Issue
Block a user