Compare commits

...

2 Commits

Author SHA1 Message Date
jh.chun ba964a9301 TMP235 온도 센서 → IMU register direact read 대체
- rbb: 응답 패킷, 60초 주기 safety check 온도 소스를 TMP235에서 IMU TEMP_DATA 레지스터로 변경
- IMU 온도 레지스터를 4회 연속 read 후 평균값 사용
- rbb: 응답 패킷 포맷은 섭씨 x100 형식 유지
2026-06-01 15:06:19 +09:00
jh.chun aa8d8f698f ADC raw data 응답 패킷(reb:) 채널 정보 추가
- [ch info 2B] = [ch session 1B] + [ch num 1B]
- 동일한 ch session을 가진 reb: 패킷들을 하나의 측정 세트로 판단
- ch num 값으로 채널별 ADC raw data 구분- session 불일치 또는 중복 채널 수신 시 해당 세트는 무효 처리 가능
2026-06-01 12:45:13 +09:00
9 changed files with 176 additions and 65 deletions
@@ -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);
@@ -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 */
}
@@ -152,6 +152,8 @@ static uint16_t clamp_measure_delay_us(uint16_t delay_us)
#define BLE_RDB_HEADER_LEN 8 /* "rdb:" tag(4) + num_samples(2) + compressed_size(2) */
#define BLE_REB_DATA_LEN (BLE_MTU_SIZE - BLE_REB_HEADER_LEN) /* 238 bytes = 119 samples */
#define BLE_RED_DATA_LEN (BLE_MTU_SIZE - BLE_RED_HEADER_LEN) /* 238 bytes = 119 samples */
#define MAA_REB_HEADER_LEN 8 /* "reb:" tag(4) + ch_info(2) + num_samples(2) */
#define MAA_REB_DATA_LEN (BLE_MTU_SIZE - MAA_REB_HEADER_LEN) /* 236 bytes = 118 samples */
#define BLE_PACKET_DELAY_MS 100 /* Inter-packet delay - allow BLE TX buffer to drain */
/* maa_async: inter-packet busy-wait (0 = rely on BLE_NUS_EVT_TX_RDY only). */
#define MAA_ASYNC_POST_REB_MS 0U
@@ -1162,6 +1164,7 @@ dr_adc_err_t dr_adc_transmit_channel_delta(const dr_maa_channel_t *ch_data, uint
/* Global async context */
static maa_async_ctx_t g_maa_ctx = { .state = MAA_ASYNC_IDLE };
static uint8_t g_maa_ch_session = 0;
/* (reb+red merged protocol) */
@@ -1202,7 +1205,8 @@ static dr_adc_err_t maa_async_capture_channel(uint8_t ch)
/**
* @brief Send reb: header + data merged for current channel
*
* reb: tag(4) + num_samples(2) + data(up to 238 bytes)
* reb: tag(4) + ch_info(2) + num_samples(2) + data(up to 236 bytes)
* ch_info: session(1) + channel(1)
* If <= 100 samples (200B), the channel completes in this single packet
*/
static bool maa_async_send_header(void)
@@ -1212,16 +1216,23 @@ static bool maa_async_send_header(void)
uint32_t tx_err;
uint16_t total_data_bytes = ch->num_samples * 2;
uint16_t ch_info = ((uint16_t)g_maa_ctx.ch_session << 8) | g_maa_ctx.current_ch;
/* reb: header + data merged (Big-Endian) */
/* reb: header + channel info + data merged (Big-Endian) */
buf[0] = 'r'; buf[1] = 'e'; buf[2] = 'b'; buf[3] = ':';
buf[4] = (uint8_t)(ch->num_samples >> 8);
buf[5] = (uint8_t)(ch->num_samples & 0xFF);
buf[4] = (uint8_t)(ch_info >> 8);
buf[5] = (uint8_t)(ch_info & 0xFF);
buf[6] = (uint8_t)(ch->num_samples >> 8);
buf[7] = (uint8_t)(ch->num_samples & 0xFF);
ADC_LOG("reb ch_info: session=%u, ch=%u, value=0x%04X",
g_maa_ctx.ch_session,
g_maa_ctx.current_ch,
ch_info);
uint16_t dst_idx = BLE_REB_HEADER_LEN;
uint16_t first_chunk = (total_data_bytes > BLE_REB_DATA_LEN) ? BLE_REB_DATA_LEN : total_data_bytes;
uint16_t dst_idx = MAA_REB_HEADER_LEN;
uint16_t first_chunk = (total_data_bytes > MAA_REB_DATA_LEN) ? MAA_REB_DATA_LEN : total_data_bytes;
uint16_t src_idx = 0;
while (src_idx < ch->num_samples && (dst_idx - BLE_REB_HEADER_LEN) < first_chunk)
while (src_idx < ch->num_samples && (dst_idx - MAA_REB_HEADER_LEN) < first_chunk)
{
uint16_t sample = ch->samples[src_idx++];
buf[dst_idx++] = (uint8_t)(sample >> 8);
@@ -1376,6 +1387,7 @@ dr_adc_err_t maa_async_start(uint8_t freq_option, uint16_t delay_us, uint16_t nu
g_maa_ctx.ble_buffer = ble_buffer;
g_maa_ctx.current_ch = 0;
g_maa_ctx.current_pkt = 0;
g_maa_ctx.ch_session = g_maa_ch_session++;
g_maa_ctx.data_offset = 0;
g_maa_ctx.on_complete_cb = NULL; /* default: no callback (maa?) */
@@ -434,6 +434,7 @@ typedef struct {
maa_async_state_t state; /**< Current state */
uint8_t current_ch; /**< Current channel (0~7) */
uint8_t current_pkt; /**< Current packet index */
uint8_t ch_session; /**< reb: channel session id for one measurement set */
uint16_t data_offset; /**< Bytes sent so far for current channel */
uint8_t freq_option; /**< Frequency option */
uint16_t delay_us; /**< Capture delay */
@@ -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 --- */
@@ -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. */
@@ -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
*
@@ -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.
@@ -1156,7 +1156,7 @@
<GroupNumber>1</GroupNumber>
<FileNumber>1</FileNumber>
<FileType>1</FileType>
<tvExp>1</tvExp>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\..\..\main.c</PathWithFileName>