From ba964a9301e828d6450c99b7bfb881ec0a4825bb Mon Sep 17 00:00:00 2001 From: jhchun Date: Mon, 1 Jun 2026 15:06:19 +0900 Subject: [PATCH] =?UTF-8?q?TMP235=20=EC=98=A8=EB=8F=84=20=EC=84=BC?= =?UTF-8?q?=EC=84=9C=20=E2=86=92=20IMU=20register=20direact=20read=20?= =?UTF-8?q?=EB=8C=80=EC=B2=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - rbb: 응답 패킷, 60초 주기 safety check 온도 소스를 TMP235에서 IMU TEMP_DATA 레지스터로 변경 - IMU 온도 레지스터를 4회 연속 read 후 평균값 사용 - rbb: 응답 패킷 포맷은 섭씨 x100 형식 유지 --- .../command/cmd_common.h | 1 + .../command/handlers/cmd_sensor.c | 42 +++--- .../measurement/battery/battery_saadc.c | 26 ++-- .../measurement/battery/battery_saadc.h | 6 +- .../measurement/imu/app_raw/app_raw.c | 129 ++++++++++++++---- .../measurement/imu/app_raw/app_raw.h | 8 ++ .../ble_app_bladder_patch_s140.uvoptx | 2 +- 7 files changed, 156 insertions(+), 58 deletions(-) diff --git a/project/ble_peripheral/ble_app_bladder_patch/command/cmd_common.h b/project/ble_peripheral/ble_app_bladder_patch/command/cmd_common.h index e8c2cad..912bab7 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/command/cmd_common.h +++ b/project/ble_peripheral/ble_app_bladder_patch/command/cmd_common.h @@ -48,6 +48,7 @@ extern void battery_level_meas(void); extern void pressure_all_level_meas(void); extern void tmp235_voltage_level_meas(void); extern int imu_read_direct(void); +extern int imu_read_temperature_x100(uint16_t *temp_x100, float *temp_c); extern int imu_fifo_capture_start(void); extern int imu_fifo_capture_stop_and_send_rim(void); extern void battery_timer_stop(void); diff --git a/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_sensor.c b/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_sensor.c index ed213ad..6c5a5a0 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_sensor.c +++ b/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_sensor.c @@ -80,7 +80,7 @@ int Cmd_msp(const ParsedCmd *cmd) * single rbb: packet. SAADC measurements run asynchronously (callback), so * dr_sd_delay_ms() is used to wait for completion. * - * Order: battery -> IMU -> (Piezo TX/RX ON) -> temperature + * Order: battery -> IMU 6-axis + IMU temperature * Response: rbb: [batt 2B] [IMU 6x2B] [temp 2B] = 20 bytes = 10 words *============================================================================*/ void all_sensors(void) @@ -98,23 +98,11 @@ void all_sensors(void) dr_sd_delay_ms(1); } - /* 2. IMU 6-axis single read -> info_imu[6] */ + /* 2. IMU 6-axis single read + IMU die temperature -> info_imu[6], info_temp */ + info_temp = 0xFFFF; hw_i2c_init_once(); imu_read_direct(); - /* 3. Temperature -> info_temp (TMP235 needs Piezo TX/RX power) */ - if (!dr_piezo_is_power_on()) - { - dr_piezo_power_on(); - } - - tmp235_saadc_done = false; - tmp235_voltage_level_meas(); - for (timeout_cnt = 0; !tmp235_saadc_done && timeout_cnt < 100; timeout_cnt++) - { - dr_sd_delay_ms(1); - } - info4 = false; /* Assemble and transmit the rbb: packet (dedicated buffer to avoid @@ -134,6 +122,11 @@ void all_sensors(void) buf[18] = (uint8_t)(info_temp >> 8); buf[19] = (uint8_t)(info_temp & 0xFF); + DBG_PRINTF("[RBB] IMU temp_x100=%u (%u.%02u C)\r\n", + info_temp, + info_temp / 100, + info_temp % 100); + dr_binary_tx_safe(buf, 10); /* 20 bytes = 10 words */ } @@ -143,7 +136,7 @@ void all_sensors(void) * Emits rbb: [batt 2B] [temp 2B] = 8 bytes = 4 words (no IMU). * Not used by mtb? anymore; kept for optional host/tests. * - * Order: battery -> (Piezo TX/RX ON) -> temperature + * Order: battery -> IMU temperature * Response: rbb: [batt 2B] [temp 2B] = 8 bytes = 4 words * TX layer appends CRC 2B, so the BLE packet is 10B total. *============================================================================*/ @@ -151,6 +144,7 @@ void all_sensors_batt_temp(void) { uint8_t *buf; uint32_t timeout_cnt; + uint16_t imu_temp_x100; info4 = true; @@ -161,16 +155,13 @@ void all_sensors_batt_temp(void) dr_sd_delay_ms(1); } - if (!dr_piezo_is_power_on()) + if (imu_read_temperature_x100(&imu_temp_x100, NULL) == 0) { - dr_piezo_power_on(); + info_temp = imu_temp_x100; } - - tmp235_saadc_done = false; - tmp235_voltage_level_meas(); - for (timeout_cnt = 0; !tmp235_saadc_done && timeout_cnt < 100; timeout_cnt++) + else { - dr_sd_delay_ms(1); + info_temp = 0xFFFF; } info4 = false; @@ -183,6 +174,11 @@ void all_sensors_batt_temp(void) buf[6] = (uint8_t)(info_temp >> 8); buf[7] = (uint8_t)(info_temp & 0xFF); + DBG_PRINTF("[RBB] IMU temp_x100=%u (%u.%02u C)\r\n", + info_temp, + info_temp / 100, + info_temp % 100); + dr_binary_tx_safe(buf, 4); /* 8 bytes = 4 words, CRC appended by TX layer */ } diff --git a/project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.c b/project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.c index 2d45aa5..96cd834 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.c +++ b/project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.c @@ -4,7 +4,7 @@ * Measures battery voltage via nRF52840 SAADC on AIN2: * - 12-bit resolution, 4x oversampling * - Periodic safety check via battery_loop timer (60 s interval) - * - Sequential: battery -> temperature measurement + * - Sequential: battery -> IMU temperature measurement * - Auto power-off after 5 consecutive readings below 3500 mV or above 40 C * - In info4 mode (bulk sensor collection): stores to info_batt * @@ -28,6 +28,7 @@ #include "battery_saadc.h" #include "main_timer.h" #include "tmp235_q1.h" +#include "app_raw.h" #include "dr_piezo.h" #include "debug_print.h" @@ -59,7 +60,7 @@ APP_TIMER_DEF(m_battery_loop_timer_id); /* Low-battery check flag — set by battery_loop, consumed by handler */ bool low_battery_check = false; -/* Safety check mode flag — set by battery handler, consumed by tmp235 handler */ +/* Safety check mode flag — legacy TMP235 path */ bool safety_check_mode = false; /* SAADC callback completion flag — used by all_sensors() to wait */ @@ -179,19 +180,28 @@ void battery_event_handler(nrf_drv_saadc_evt_t const * p_event) /* Resistor divider correction factor 1.42 */ batt_lvl_in_milli_volt_1 = batt_lvl_in_milli_volt_0 * 1.42f; - /* --- Safety check mode: store voltage, chain temperature measurement --- */ + /* --- Safety check mode: store voltage, then check IMU die temperature --- */ if (low_battery_check == true) { + float imu_temp_c = -273.15f; + low_battery_check = false; safety_batt_mv = batt_lvl_in_milli_volt_1; - safety_check_mode = true; - /* TMP235 shares piezo TX/RX power rail */ - if (!dr_piezo_is_power_on()) + if (imu_read_temperature_x100(NULL, &imu_temp_c) != 0) { - dr_piezo_power_on(); + DBG_PRINTF("[SAFETY] IMU temp read failed\r\n"); } - tmp235_voltage_level_meas(); + else + { + int temp_x100 = (int)(imu_temp_c * 100.0f); + DBG_PRINTF("[SAFETY] 60s IMU temp_x100=%d (%d.%02d C)\r\n", + temp_x100, + temp_x100 / 100, + temp_x100 % 100); + } + + safety_check_complete(imu_temp_c); } /* --- info4 mode: store value for mbb? bulk response --- */ diff --git a/project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.h b/project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.h index 09002a0..c8357cb 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.h +++ b/project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.h @@ -8,7 +8,7 @@ * battery_timer_init/start/stop() : 60-second periodic monitoring timer * * Periodic safety check (every 60 s): - * Battery -> Temperature sequential measurement via SAADC. + * Battery SAADC -> IMU die temperature sequential measurement. * Auto power-off after 5 consecutive readings below LOW_BATTERY_VOLTAGE (3500 mV) * or above OVER_TEMPERATURE_THRESHOLD (40 C). *============================================================================*/ @@ -25,10 +25,10 @@ /* SAADC callback completion flag (used by all_sensors() to wait) */ extern volatile bool battery_saadc_done; -/* Safety check mode flag — set by battery_loop, consumed by tmp235 handler */ +/* Safety check mode flag — legacy TMP235 path */ extern bool safety_check_mode; -/* Called by tmp235 handler when safety check temperature measurement completes */ +/* Called when safety check temperature measurement completes */ void safety_check_complete(float temp_c); /* Start a single async battery measurement. Result handled in callback. */ 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 2728dc9..9dd6b9d 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 @@ -25,7 +25,7 @@ * - 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 + * - 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 @@ -93,6 +93,7 @@ extern which_cmd_t cmd_type_t; /* Current command source ( 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 */ +extern volatile uint16_t info_temp; /* Global temperature cache (deg C x 100) */ /* -------------------------------------------------------------------------------------- * static function declaration @@ -447,7 +448,7 @@ static void apply_mounting_matrix(const int32_t matrix[9], int32_t raw[3]) * 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) + * 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 * 9) Send via BLE with "rsp:" tag @@ -462,7 +463,26 @@ 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 — accel X-axis upper byte register */ +#define REG_TEMP_DATA1 0x09 /* TEMP_DATA1 — temperature upper byte register */ +#define IMU_TEMP_AVG_SAMPLES 4U + +static bool s_direct_twi_ready = false; + +static void imu_direct_twi_init_once(void) +{ + /* TWI (I2C) init — performed only once (re-init ensures clean state) */ + if (!s_direct_twi_ready) { + inv_i2c_master_uninitialize(); + inv_i2c_master_initialize(); + s_direct_twi_ready = true; + } +} + +static uint16_t imu_temp_raw_to_x100(int16_t temp_raw) +{ + float temp_c = 25.0f + ((float)temp_raw / 128.0f); + return (uint16_t)(temp_c * 100.0f); +} /* -------------------------------------------------------------------------------------- * Direct IMU register read — raw I2C, no DRDY, sends rsp: via BLE @@ -470,19 +490,13 @@ extern const nrfx_twi_t m_twi_icm42670; * -------------------------------------------------------------------------------------- */ int imu_read_direct(void) { - uint8_t raw[12]; /* accel 6 bytes + gyro 6 bytes */ + uint8_t raw[14]; /* temp 2 bytes + accel 6 bytes + gyro 6 bytes */ int32_t accel[3], gyro[3]; + int16_t temp_raw; uint8_t reg; uint32_t ret; - static bool twi_ready = false; - - /* 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; - } + imu_direct_twi_init_once(); /* Gyro config: GYRO_CONFIG0(0x20) = 0x09 -> +/-2000dps FSR, 100Hz ODR */ { @@ -504,8 +518,8 @@ int imu_read_direct(void) dr_sd_delay_ms(80); } - /* Read 12 consecutive bytes from ACCEL_DATA_X1 (0x0B~0x16) */ - reg = REG_ACCEL_X1; + /* Read 14 consecutive bytes from TEMP_DATA1 (0x09~0x16): temp + accel + gyro */ + reg = REG_TEMP_DATA1; ret = icm42670_twi_tx(IMU_I2C_ADDR, ®, 1, true); /* Send register address (no STOP) */ if (ret) @@ -514,7 +528,7 @@ int imu_read_direct(void) return -1; } - ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, 12); /* Receive 12 bytes of data */ + ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, sizeof(raw)); if (ret) { @@ -524,15 +538,17 @@ int imu_read_direct(void) /* * 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) + * raw[0..1] = temperature + * raw[2..7] = accel X,Y,Z (2 bytes each, MSB first) + * raw[8..13] = 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]); - 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]); + temp_raw = (int16_t)((raw[0] << 8) | raw[1]); + accel[0] = (int16_t)((raw[2] << 8) | raw[3]); + accel[1] = (int16_t)((raw[4] << 8) | raw[5]); + accel[2] = (int16_t)((raw[6] << 8) | raw[7]); + gyro[0] = (int16_t)((raw[8] << 8) | raw[9]); + 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); @@ -548,6 +564,8 @@ int imu_read_direct(void) if (info4 == true) { + uint16_t temp_x100 = imu_temp_raw_to_x100(temp_raw); + /* info4 mode: store in global array (sent as rbb: packet by mbb?) */ info_imu[0] = ssp_data[0]; info_imu[1] = ssp_data[1]; @@ -555,6 +573,7 @@ int imu_read_direct(void) info_imu[3] = ssp_data[3]; info_imu[4] = ssp_data[4]; info_imu[5] = ssp_data[5]; + info_temp = temp_x100; } else { @@ -572,6 +591,70 @@ int imu_read_direct(void) return 0; } +int imu_read_temperature_x100(uint16_t *temp_x100, float *temp_c) +{ + uint8_t raw[2]; + uint8_t reg = REG_TEMP_DATA1; + int16_t temp_raw; + int32_t temp_sum = 0; + uint32_t ret; + uint8_t i; + + if (temp_x100 == NULL && temp_c == NULL) + { + return -1; + } + + imu_direct_twi_init_once(); + + /* Power ON briefly so the temperature register is refreshed before reading. */ + { + uint8_t pwr_cmd[2] = { 0x1F, 0x0F }; + icm42670_twi_tx(IMU_I2C_ADDR, pwr_cmd, 2, false); + dr_sd_delay_ms(80); + } + + for (i = 0; i < IMU_TEMP_AVG_SAMPLES; i++) + { + ret = icm42670_twi_tx(IMU_I2C_ADDR, ®, 1, true); + if (ret) + { + uint8_t pwr_off[2] = { 0x1F, 0x00 }; + icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false); + DBG_PRINTF("[IMU] temp tx FAIL %u\r\n", ret); + return -2; + } + + ret = icm42670_twi_rx(IMU_I2C_ADDR, raw, sizeof(raw)); + if (ret) + { + uint8_t pwr_off[2] = { 0x1F, 0x00 }; + icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false); + DBG_PRINTF("[IMU] temp rx FAIL %u\r\n", ret); + return -3; + } + + temp_sum += (int16_t)((raw[0] << 8) | raw[1]); + } + + temp_raw = (int16_t)(temp_sum / (int32_t)IMU_TEMP_AVG_SAMPLES); + if (temp_x100 != NULL) + { + *temp_x100 = imu_temp_raw_to_x100(temp_raw); + } + if (temp_c != NULL) + { + *temp_c = 25.0f + ((float)temp_raw / 128.0f); + } + + { + uint8_t pwr_off[2] = { 0x1F, 0x00 }; + icm42670_twi_tx(IMU_I2C_ADDR, pwr_off, 2, false); + } + + return 0; +} + /* -------------------------------------------------------------------------------------- * mtb? FIFO capture support * diff --git a/project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.h b/project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.h index d0b88d0..eacc80c 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.h +++ b/project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.h @@ -97,6 +97,14 @@ void imu_callback(inv_imu_sensor_event_t *event); */ int imu_read_direct(void); +/** + * \brief Read IMU die temperature directly and return deg C x 100. + * \param[out] temp_x100 Optional temperature output in Celsius x 100. + * \param[out] temp_c Optional temperature output in Celsius. + * \return 0=success, negative=error + */ +int imu_read_temperature_x100(uint16_t *temp_x100, float *temp_c); + /** * \brief Start IMU internal FIFO capture for mtb? test flow. * Configures accel/gyro 100 Hz and flushes FIFO before capture. diff --git a/project/ble_peripheral/ble_app_bladder_patch/pca10056/s140/arm5_no_packs/ble_app_bladder_patch_s140.uvoptx b/project/ble_peripheral/ble_app_bladder_patch/pca10056/s140/arm5_no_packs/ble_app_bladder_patch_s140.uvoptx index dbea7d1..749690e 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/pca10056/s140/arm5_no_packs/ble_app_bladder_patch_s140.uvoptx +++ b/project/ble_peripheral/ble_app_bladder_patch/pca10056/s140/arm5_no_packs/ble_app_bladder_patch_s140.uvoptx @@ -1156,7 +1156,7 @@ 1 1 1 - 1 + 0 0 0 ..\..\..\main.c