From a77919b4d3213fa247047f3ba6782a480f8842dc Mon Sep 17 00:00:00 2001 From: "jh.Chun" Date: Fri, 13 Mar 2026 17:27:17 +0900 Subject: [PATCH] IMU gyro/accel config fix and README piezo frequency table correction - Add GYRO_CONFIG0/ACCEL_CONFIG0 register setup in imu_read_direct() to fix gyro always returning -32768 - Increase sensor startup delay from 2ms to 80ms for reliable first read - Put IMU back to sleep after msp? read to save power - Fix piezo frequency table: swap freq 0/1 values, remove unused freq 5 Co-Authored-By: Claude Opus 4.6 --- README.md | 5 ++-- .../icm42670p/app_raw/app_raw.c | 28 +++++++++++++++---- 2 files changed, 24 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index a8f6364..32df57d 100644 --- a/README.md +++ b/README.md @@ -37,12 +37,11 @@ VesiScan BASIC은 피에조 초음파 트랜스듀서와 IMU 센서를 이용하 | freq | 주파수 | |------|--------| -| 0 (기본) | 2.1 MHz | -| 1 | 1.8 MHz | +| 0 | 1.8 MHz | +| 1 (기본) | 2.1 MHz | | 2 | 2.0 MHz | | 3 | 1.7 MHz | | 4 | 2.2 MHz | -| 5 | 1.9 MHz | ### 8채널 MUX 매핑 diff --git a/project/ble_peripheral/ble_app_bladder_patch/icm42670p/app_raw/app_raw.c b/project/ble_peripheral/ble_app_bladder_patch/icm42670p/app_raw/app_raw.c index 1cc72e7..0c8d112 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/icm42670p/app_raw/app_raw.c +++ b/project/ble_peripheral/ble_app_bladder_patch/icm42670p/app_raw/app_raw.c @@ -17,7 +17,6 @@ #include "app_util_platform.h" #include "main.h" -#include "meas_pd_48.h" #include #include "debug_print.h" #include "nrf_delay.h" @@ -51,14 +50,13 @@ static int32_t icm_mounting_matrix[9] = {(1<<30), 0, 0, 0, (1<<30), 0, 0, 0, (1<<30)}; #endif -extern bool custom_add_data; +bool custom_add_data; extern bool motion_raw_data_enabled; extern char ble_tx_buffer[BLE_NUS_MAX_DATA_LEN]; -extern bool go_pdread; extern which_cmd_t cmd_type_t; uint16_t ssp_data[6]={0,}; extern bool info4; //cmd_parse - extern volatile uint16_t info_imu[6]; + volatile uint16_t info_imu[6]; /* -------------------------------------------------------------------------------------- * static function declaration @@ -278,7 +276,6 @@ void imu_callback(inv_imu_sensor_event_t *event) info_imu[3] = (uint16_t)gyro[0]; info_imu[4] = (uint16_t) gyro[1]; info_imu[5] = (uint16_t)gyro[2]; - go_pdread = true; } else if(cmd_type_t == CMD_UART) { @@ -354,11 +351,23 @@ int imu_read_direct(void) twi_ready = true; } + /* Configure gyro: ±2000dps, 100Hz ODR — GYRO_CONFIG0(0x20) = 0x09 */ + { + uint8_t gyro_cfg[2] = { 0x20, 0x09 }; + icm42670_twi_tx(IMU_I2C_ADDR, gyro_cfg, 2, false); + } + + /* Configure accel: ±4g, 100Hz ODR — ACCEL_CONFIG0(0x21) = 0x29 */ + { + uint8_t accel_cfg[2] = { 0x21, 0x29 }; + icm42670_twi_tx(IMU_I2C_ADDR, accel_cfg, 2, false); + } + /* Enable accel (low-noise) + gyro (low-noise): PWR_MGMT0 = 0x0F */ { uint8_t pwr_cmd[2] = { 0x1F, 0x0F }; /* reg=0x1F, val=0x0F */ icm42670_twi_tx(IMU_I2C_ADDR, pwr_cmd, 2, false); - nrf_delay_ms(2); /* wait for sensor startup */ + nrf_delay_ms(80); /* gyro needs min 45ms + margin for first valid sample */ } /* Read 12 bytes starting from ACCEL_DATA_X1 (0x0B..0x16) */ @@ -398,6 +407,13 @@ int imu_read_direct(void) format_data(imu_bin_buffer, "rsp:", ssp_data, 12); binary_tx_handler(imu_bin_buffer, 8); + /* Put IMU back to sleep: accel OFF + gyro OFF */ + { + uint8_t pwr_off[2] = { 0x1F, 0x00 }; /* reg=PWR_MGMT0, val=0x00 */ + icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false); + DBG_PRINTF("[IMU] sleep\r\n"); + } + DBG_PRINTF("[IMU] sent OK\r\n"); return 0; }