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

IMU FIFO, direct 모두 데이터시트 축 기준 raw data로 통일

Browse Source 
- 기존에는 direct read에 mounting matrix(좌표 보정) 적용, FIFO는 raw data
- 데이터시트 축 기준 raw data로 통일
This commit is contained in:
jhchun
2026-06-15 14:14:07 +09:00
parent 736a3f2c9a
commit ad9546b934
1 changed files with 5 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.c
+5 -69
View File
@@ -20,16 +20,15 @@
* - 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)
* - Keeps raw sensor axes (ICM42670P datasheet coordinate system)
* - 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 temp + 6-axis -> sleep
*
* Mounting matrix:
* 3x3 rotation matrix in Q30 fixed-point format, correcting the sensor's
* physical mounting orientation to match the software coordinate system.
* Axis convention:
* Raw ICM42670P datasheet X/Y/Z axes. No mounting matrix is applied.
******************************************************************************/
#include "sdk_config.h"
@@ -67,25 +66,6 @@ static struct inv_imu_device icm_driver;
/* Binary buffer for BLE transmission */
uint8_t imu_bin_buffer[BLE_NUS_MAX_DATA_LEN] = {0};
/*
* ICM42670P mounting matrix (Q30 fixed-point)
*
* Coordinate transform based on the sensor's physical mounting orientation.
* Q30 format: 1.0 = (1 << 30) = 0x40000000
*
* 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,
(1<<30), 0, 0,
0, 0, (1<<30) };
#else /* For SmartMotion */
static int32_t icm_mounting_matrix[9] = {(1<<30), 0, 0,
0, (1<<30), 0,
0, 0, (1<<30)};
#endif
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 */
@@ -95,11 +75,6 @@ extern bool info4; /* info4 mode flag (set by
volatile uint16_t info_imu[6]; /* Global array storing IMU data in info4 mode */
volatile uint16_t info_temp; /* Global temperature cache (deg C x 100) */
/* --------------------------------------------------------------------------------------
* static function declaration
* -------------------------------------------------------------------------------------- */
static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3]);
/* --------------------------------------------------------------------------------------
* Functions definition
* -------------------------------------------------------------------------------------- */
@@ -263,7 +238,7 @@ static void get_accel_and_gyr_fsr(int16_t * accel_fsr_g, int16_t * gyro_fsr_dps)
* 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)
* 2) Keep raw ICM42670P datasheet axes
* 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
@@ -331,10 +306,6 @@ void imu_callback(inv_imu_sensor_event_t *event)
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
/*
* Convert raw data to physical units (g, dps)
@@ -406,37 +377,6 @@ void imu_callback(inv_imu_sensor_event_t *event)
#endif
}
/* --------------------------------------------------------------------------------------
* Static functions definition
* -------------------------------------------------------------------------------------- */
/*
* apply_mounting_matrix()
* 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]
* 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])
{
unsigned i;
int64_t data_q30[3];
for(i = 0; i < 3; i++) {
data_q30[i] = ((int64_t)matrix[3*i+0] * raw[0]);
data_q30[i] += ((int64_t)matrix[3*i+1] * raw[1]);
data_q30[i] += ((int64_t)matrix[3*i+2] * raw[2]);
}
/* 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);
}
/*
* imu_read_direct()
* Reads IMU registers directly via I2C, bypassing the driver API.
@@ -450,7 +390,7 @@ static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3])
* 5) Wait 80ms (gyro startup: min 45ms + margin)
* 6) Read 14 consecutive bytes from TEMP_DATA1 (0x09) (temp 2 + accel 6 + gyro 6)
* 7) Big-endian -> int16_t conversion
* 8) Apply mounting matrix
* 8) Keep raw ICM42670P datasheet axes
* 9) Send via BLE with "rsp:" tag
* 10) Switch IMU to sleep mode (power saving)
*
@@ -554,10 +494,6 @@ int imu_read_direct(void)
gyro[1] = (int16_t)((raw[10] << 8) | raw[11]);
gyro[2] = (int16_t)((raw[12] << 8) | raw[13]);
/* 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];