diff --git a/project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.c b/project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.c index c159036..50e630a 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.c +++ b/project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.c @@ -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];