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

TMP235 온도 센서 → IMU register direact read 대체

Browse Source 
- rbb: 응답 패킷, 60초 주기 safety check 온도 소스를 TMP235에서 IMU TEMP_DATA 레지스터로 변경
- IMU 온도 레지스터를 4회 연속 read 후 평균값 사용
- rbb: 응답 패킷 포맷은 섭씨 x100 형식 유지
This commit is contained in:
jhchun
2026-06-01 15:06:19 +09:00
parent aa8d8f698f
commit ba964a9301
7 changed files with 156 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.c
+18 -8
View File
@@ -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 --- */
project/ble_peripheral/ble_app_bladder_patch/measurement/battery/battery_saadc.h
+3 -3
View File
@@ -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. */
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.c
+106 -23
View File
@@ -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, &reg, 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, &reg, 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
*
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.h
+8
View File
@@ -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.