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

IMU direct read -> FIFO 방식 변경

Browse Source 
- mtb? 커맨드
This commit is contained in:
jhchun
2026-05-18 17:54:15 +09:00
parent 8d9cb6e307
commit d439ae9b68
12 changed files with 617 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
package-lock.json
Generated
+6
View File
@@ -0,0 +1,6 @@
{
"name": "Vesiscan-Basic_imu",
"lockfileVersion": 3,
"requires": true,
"packages": {}
}
project/ble_peripheral/ble_app_bladder_patch/command/cmd_common.h
+2
View File
@@ -48,6 +48,8 @@ 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_fifo_capture_start(void);
extern int imu_fifo_capture_stop_and_send_rim(void);
extern void battery_timer_stop(void);
extern void main_timer_start(void);
extern void hw_i2c_init_once(void);
project/ble_peripheral/ble_app_bladder_patch/command/cmd_table.c
+1
View File
@@ -49,6 +49,7 @@ static const CmdEntry m_cmd_table[] = {
{ "mec?", true, Cmd_mec },
{ "maa?", true, Cmd_maa },
{ "mbb?", true, Cmd_mbb },
{ "mtb?", true, Cmd_mtb },
{ "mcf?", true, Cmd_mcf },
{ "mcs?", true, Cmd_mcs },
project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_piezo.c
+65
View File
@@ -17,6 +17,11 @@
#include "dr_piezo.h"
#include "dr_adc121s051.h"
static void mtb_send_rim_after_piezo(void)
{
send_imu_rim_fifo();
}
/*------------------------------------------------------------------------------
* Internal clamp helpers for persisted piezo configuration
*----------------------------------------------------------------------------*/
@@ -359,6 +364,66 @@ int Cmd_mbb(const ParsedCmd *cmd)
return 1;
}
/*==============================================================================
* mtb? -> reb:+raa:+rim: Piezo ADC + IMU FIFO (no rbb:)
*
* Request: [TAG 4B "mtb?"] [CRC 2B]
* Response: reb: [num_samples 2B] [raw_data...] (per channel; same maa_async as mbb?)
* raa: [status 2B]
* rim: [total_sample_count u16 BE] [samples: 12B each ax,ay,az,gx,gy,gz ...] (may span BLE packets)
* Error: raa: + 0xFFFE (previous capture in progress)
* raa: + (0xFF00|err) (start failed)
*
* reb/raa use the same maa_async_start path as mbb?; no rbb: / all_sensors().
*============================================================================*/
int Cmd_mtb(const ParsedCmd *cmd)
{
dr_adc_err_t err;
(void)cmd;
if (maa_async_is_busy())
{
dr_ble_return_1("raa:", 0xFFFE);
return 1;
}
(void)imu_fifo_capture_start();
if (!dr_piezo_is_power_on())
{
dr_piezo_power_on();
}
maa_async_set_pre_capture_all(true);
err = maa_async_start(
m_config.piezo_freq_option,
m_config.piezo_delay_us,
m_config.piezo_num_samples,
m_config.piezo_cycles,
m_config.piezo_averaging,
ble_bin_buffer
);
if (err != DR_ADC_OK)
{
if (g_plat.log)
{
g_plat.log("[Cmd_mtb] start failed err=%d\r\n", err);
}
single_format_data(ble_bin_buffer, "raa:", (uint16_t)(0xFF00 | err));
dr_binary_tx_safe(ble_bin_buffer, 3);
dr_piezo_power_off();
maa_async_set_on_complete(NULL);
send_imu_rim_fifo();
return 1;
}
maa_async_set_on_complete(mtb_send_rim_after_piezo);
return 1;
}
/*==============================================================================
* mcf? -> rcf: Read piezo parameters from FDS
*
project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_piezo.h
+1
View File
@@ -12,6 +12,7 @@ int Cmd_mpc(const ParsedCmd *cmd); /* mpc? -> rpc: burst generation */
int Cmd_mec(const ParsedCmd *cmd); /* mec? -> reb:+raa: single-channel capture */
int Cmd_maa(const ParsedCmd *cmd); /* maa? -> reb:+raa: 6-channel async capture */
int Cmd_mbb(const ParsedCmd *cmd); /* mbb? -> rbb:+reb:+raa: sensors + capture */
int Cmd_mtb(const ParsedCmd *cmd); /* mtb? -> reb:+raa:+rim: piezo + IMU FIFO (no rbb:) */
int Cmd_mcf(const ParsedCmd *cmd); /* mcf? -> rcf: read piezo parameters */
int Cmd_mcs(const ParsedCmd *cmd); /* mcs? -> rcs: write piezo parameters */
project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_sensor.c
+54
View File
@@ -136,3 +136,57 @@ void all_sensors(void)
dr_binary_tx_safe(buf, 10); /* 20 bytes = 10 words */
}
/*==============================================================================
* all_sensors_batt_temp() - Battery + temperature only, then short rbb:
*
* 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
* Response: rbb: [batt 2B] [temp 2B] = 8 bytes = 4 words
* TX layer appends CRC 2B, so the BLE packet is 10B total.
*============================================================================*/
void all_sensors_batt_temp(void)
{
uint8_t *buf;
uint32_t timeout_cnt;
info4 = true;
battery_saadc_done = false;
battery_level_meas();
for (timeout_cnt = 0; !battery_saadc_done && timeout_cnt < 100; timeout_cnt++)
{
dr_sd_delay_ms(1);
}
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;
static uint8_t rbb_buf[8];
buf = rbb_buf;
buf[0] = 'r'; buf[1] = 'b'; buf[2] = 'b'; buf[3] = ':';
buf[4] = (uint8_t)(info_batt >> 8);
buf[5] = (uint8_t)(info_batt & 0xFF);
buf[6] = (uint8_t)(info_temp >> 8);
buf[7] = (uint8_t)(info_temp & 0xFF);
dr_binary_tx_safe(buf, 4); /* 8 bytes = 4 words, CRC appended by TX layer */
}
void send_imu_rim_fifo(void)
{
(void)imu_fifo_capture_stop_and_send_rim();
}
project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_sensor.h
+7
View File
@@ -13,4 +13,11 @@ int Cmd_msp(const ParsedCmd *cmd); /* msp? -> rsp: IMU 6-axis single read */
* Called from Cmd_mbb() in cmd_piezo.c. */
void all_sensors(void);
/* Optional helper: battery / temperature only, then rbb: [batt 2B] [temp 2B].
* (mtb? no longer calls this; kept for reuse / tooling.) */
void all_sensors_batt_temp(void);
/* Test helper for mtb?: drains IMU FIFO and emits rim: packet(s). */
void send_imu_rim_fifo(void);
#endif /* CMD_SENSOR_H */
project/ble_peripheral/ble_app_bladder_patch/measurement/adc121s051/dr_adc121s051.c
+7 -3
View File
@@ -151,6 +151,10 @@ static uint16_t clamp_measure_delay_us(uint16_t delay_us)
#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 BLE_PACKET_DELAY_MS 100 /* Inter-packet delay - allow BLE TX buffer to drain */
/* maa_async: gaps between BLE notifications (sync MEC path still uses BLE_PACKET_DELAY_MS). */
#define MAA_ASYNC_POST_REB_MS 3U /* after reb: (was 5) */
#define MAA_ASYNC_POST_RED_MS 18U /* after each red: (was 50) */
#define MAA_ASYNC_PRE_RAA_MS 15U /* before raa: completion (was 50) */
#define DR_ADC_AVG_INTER_BURST_GAP_US 500 /* Gap between averaged bursts to reduce residual echo carry-over */
/* Piezo MUX pins (8ch) */
@@ -1211,7 +1215,7 @@ static void maa_async_send_header(void)
}
dr_binary_tx_safe(buf, dst_idx / 2);
dr_sd_delay_ms(5); /* minimal delay; dr_binary_tx_safe retries internally (40 ms) */
dr_sd_delay_ms(MAA_ASYNC_POST_REB_MS);
g_maa_ctx.current_pkt = 0;
g_maa_ctx.data_offset = src_idx * 2; /* bytes already sent */
@@ -1262,7 +1266,7 @@ static bool maa_async_send_data_packet(void)
}
dr_binary_tx_safe(buf, dst_idx / 2);
dr_sd_delay_ms(50); /* Allow BLE stack to process TX */
dr_sd_delay_ms(MAA_ASYNC_POST_RED_MS);
g_maa_ctx.data_offset += chunk_size;
g_maa_ctx.current_pkt++;
@@ -1280,7 +1284,7 @@ static void maa_async_send_completion(uint16_t status)
uint8_t *buf = g_maa_ctx.ble_buffer;
/* Wait for previous TX to complete before sending raa: */
dr_sd_delay_ms(50);
dr_sd_delay_ms(MAA_ASYNC_PRE_RAA_MS);
buf[0] = 'r'; buf[1] = 'a'; buf[2] = 'a'; buf[3] = ':';
buf[4] = (uint8_t)(status >> 8);
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.c
+424 -2
View File
@@ -36,6 +36,7 @@
#include "app_raw.h"
#include "inv_imu_extfunc.h"
#include "inv_imu_driver.h"
#include "inv_imu_transport.h"
#include "ble_nus.h"
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
@@ -45,6 +46,7 @@
#include "main.h"
#include "debug_print.h"
#include "nrf_delay.h"
#include <string.h>
/*
@@ -168,9 +170,9 @@ int configure_imu_device(void)
/* High-resolution FIFO mode: 20-bit data, FSR locked to 16g/2000dps */
rc |= inv_imu_enable_high_resolution_fifo(&icm_driver);
} else {
/* Standard mode: accel +/-4g, gyro +/-2000dps FSR */
/* Standard mode: accel +/-4g, gyro +/-500dps FSR */
rc |= inv_imu_set_accel_fsr(&icm_driver, ACCEL_CONFIG0_FS_SEL_4g);
rc |= inv_imu_set_gyro_fsr(&icm_driver, GYRO_CONFIG0_FS_SEL_2000dps);
rc |= inv_imu_set_gyro_fsr(&icm_driver, GYRO_CONFIG0_FS_SEL_500dps);
}
if (USE_LOW_NOISE_MODE) {
@@ -580,3 +582,423 @@ int imu_read_direct(void)
return 0;
}
/* --------------------------------------------------------------------------------------
* mtb? FIFO capture support
*
* Uses the ICM42670P internal FIFO at 25 Hz. The FIFO is started when mtb?
* begins, then drained after piezo raa: completion and sent as rim: packets.
* rim payload: u16 BE total_sample_count, then per sample accel(6B)+gyro(6B) from each 16B FIFO record.
*
* Timing (tune for MTB latency vs BLE reliability):
* - IMU_FIFO_ENABLE_SETTLE_MS: after accel/gyro on, before 2nd FIFO flush (first start only).
* - IMU_FIFO_RIM_POST_TX_MS: gap between rim notifications only (not after final packet).
* -------------------------------------------------------------------------------------- */
#define IMU_FIFO_PACKET_SIZE_BYTES FIFO_16BYTES_PACKET_SIZE
#define IMU_FIFO_MAX_PACKET_COUNT 258
#define IMU_FIFO_READ_RECORDS_PER_BURST 14
#define IMU_FIFO_ACCEL_OFFSET 1
#define IMU_FIFO_GYRO_OFFSET 7
#define IMU_FIFO_ENABLE_SETTLE_MS 35U
#define IMU_FIFO_RIM_POST_TX_MS 8U
#define RIM_SAMPLE_SIZE_BYTES 12
#define RIM_PACKET_HEADER_BYTES 6 /* "rim:" + u16 BE total_sample_count */
#define RIM_MAX_SAMPLE_BYTES (BLE_NUS_MAX_DATA_LEN - 2 - RIM_PACKET_HEADER_BYTES)
#define RIM_SAMPLES_PER_PACKET (RIM_MAX_SAMPLE_BYTES / RIM_SAMPLE_SIZE_BYTES)
static bool s_fifo_capture_active = false;
static void imu_serif_make(struct inv_imu_serif *serif)
{
serif->context = 0;
serif->read_reg = inv_io_hal_read_reg;
serif->write_reg = inv_io_hal_write_reg;
serif->max_read = 1024 * 32;
serif->max_write = 1024 * 32;
serif->serif_type = SERIF_TYPE;
}
static int imu_fifo_driver_prepare(void)
{
struct inv_imu_serif icm_serif;
int rc;
DBG_PRINTF("[IMU FIFO] prepare: TWI reinit + setup_imu_device\r\n");
inv_i2c_master_uninitialize();
inv_i2c_master_initialize();
imu_serif_make(&icm_serif);
rc = setup_imu_device(&icm_serif);
DBG_PRINTF("[IMU FIFO] prepare: setup_imu_device rc=%d\r\n", rc);
return rc;
}
static void imu_fifo_power_off(void)
{
DBG_PRINTF("[IMU FIFO] power_off: disable gyro/accel, FIFO off, active=0\r\n");
(void)inv_imu_disable_gyro(&icm_driver);
(void)inv_imu_disable_accel(&icm_driver);
(void)inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_DISABLED);
s_fifo_capture_active = false;
}
int imu_fifo_capture_start(void)
{
int rc;
DBG_PRINTF("[IMU FIFO] capture_start: enter active=%u\r\n", (unsigned)s_fifo_capture_active);
if (s_fifo_capture_active)
{
DBG_PRINTF("[IMU FIFO] capture_start: skip (already active)\r\n");
return 0;
}
rc = imu_fifo_driver_prepare();
if (rc != 0)
{
DBG_PRINTF("[IMU FIFO] prepare fail %d\r\n", rc);
return rc;
}
DBG_PRINTF("[IMU FIFO] capture_start: FSR/ODR/FIFO stream, settle %ums\r\n",
(unsigned)IMU_FIFO_ENABLE_SETTLE_MS);
rc |= inv_imu_set_accel_fsr(&icm_driver, ACCEL_CONFIG0_FS_SEL_4g);
rc |= inv_imu_set_gyro_fsr(&icm_driver, GYRO_CONFIG0_FS_SEL_500dps);
rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_25_HZ);
rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_25_HZ);
rc |= inv_imu_set_accel_ln_bw(&icm_driver, IMU_FIFO_MTB_ACCEL_LN_BW);
rc |= inv_imu_set_gyro_ln_bw(&icm_driver, IMU_FIFO_MTB_GYRO_LN_BW);
rc |= inv_imu_disable_high_resolution_fifo(&icm_driver);
rc |= inv_imu_configure_fifo(&icm_driver, INV_IMU_FIFO_ENABLED);
{
uint8_t fifo_cfg1;
rc |= inv_imu_read_reg(&icm_driver, FIFO_CONFIG1, 1, &fifo_cfg1);
fifo_cfg1 &= ~FIFO_CONFIG1_FIFO_MODE_MASK;
fifo_cfg1 |= FIFO_CONFIG1_FIFO_MODE_STREAM;
rc |= inv_imu_write_reg(&icm_driver, FIFO_CONFIG1, 1, &fifo_cfg1);
}
rc |= inv_imu_reset_fifo(&icm_driver);
rc |= inv_imu_enable_accel_low_noise_mode(&icm_driver);
rc |= inv_imu_enable_gyro_low_noise_mode(&icm_driver);
DBG_PRINTF("[IMU FIFO] capture_start: delay %ums\r\n", (unsigned)IMU_FIFO_ENABLE_SETTLE_MS);
dr_sd_delay_ms(IMU_FIFO_ENABLE_SETTLE_MS);
rc |= inv_imu_reset_fifo(&icm_driver);
if (rc != 0)
{
DBG_PRINTF("[IMU FIFO] start fail %d\r\n", rc);
imu_fifo_power_off();
return rc;
}
s_fifo_capture_active = true;
DBG_PRINTF("[IMU FIFO] capture_start: OK active=1 rc_accum=%d\r\n", rc);
return 0;
}
static void imu_fifo_send_rim_packets(uint16_t total_sample_count)
{
static uint8_t rim_buf[BLE_NUS_MAX_DATA_LEN];
uint16_t record_idx = 0;
uint16_t pkt = 0;
DBG_PRINTF("[IMU FIFO] rim_send: total_samples=%u max_per_pkt=%u\r\n",
(unsigned)total_sample_count, (unsigned)RIM_SAMPLES_PER_PACKET);
do
{
uint16_t sample_count = total_sample_count - record_idx;
uint16_t dst_idx = RIM_PACKET_HEADER_BYTES;
uint16_t i;
if (sample_count > RIM_SAMPLES_PER_PACKET)
{
sample_count = RIM_SAMPLES_PER_PACKET;
}
rim_buf[0] = 'r'; rim_buf[1] = 'i'; rim_buf[2] = 'm'; rim_buf[3] = ':';
rim_buf[4] = (uint8_t)(total_sample_count >> 8);
rim_buf[5] = (uint8_t)(total_sample_count & 0xFF);
for (i = 0; i < sample_count; i++)
{
const uint8_t *record = &icm_driver.fifo_data[(record_idx + i) * IMU_FIFO_PACKET_SIZE_BYTES];
memcpy(&rim_buf[dst_idx], &record[IMU_FIFO_ACCEL_OFFSET], 6);
dst_idx += 6;
memcpy(&rim_buf[dst_idx], &record[IMU_FIFO_GYRO_OFFSET], 6);
dst_idx += 6;
}
DBG_PRINTF("[IMU FIFO] rim_send: pkt=%u samples=%u bytes=%u words=%u\r\n",
(unsigned)pkt, (unsigned)sample_count, (unsigned)dst_idx, (unsigned)(dst_idx / 2));
dr_binary_tx_safe(rim_buf, dst_idx / 2);
record_idx += sample_count;
pkt++;
if (record_idx < total_sample_count)
{
dr_sd_delay_ms(IMU_FIFO_RIM_POST_TX_MS);
}
} while (record_idx < total_sample_count);
DBG_PRINTF("[IMU FIFO] rim_send: done packets=%u\r\n", (unsigned)pkt);
}
static int imu_fifo_read_records(uint16_t record_count)
{
int rc = 0;
uint16_t record_idx = 0;
uint16_t burst_n = 0;
DBG_PRINTF("[IMU FIFO] fifo_read: records=%u burst_max=%u\r\n",
(unsigned)record_count, (unsigned)IMU_FIFO_READ_RECORDS_PER_BURST);
while ((record_idx < record_count) && (rc == 0))
{
uint16_t burst_records = record_count - record_idx;
uint16_t burst_bytes;
if (burst_records > IMU_FIFO_READ_RECORDS_PER_BURST)
{
burst_records = IMU_FIFO_READ_RECORDS_PER_BURST;
}
burst_bytes = burst_records * IMU_FIFO_PACKET_SIZE_BYTES;
DBG_PRINTF("[IMU FIFO] fifo_read: burst=%u idx=%u n=%u bytes=%u\r\n",
(unsigned)burst_n, (unsigned)record_idx, (unsigned)burst_records, (unsigned)burst_bytes);
rc |= inv_imu_read_reg(&icm_driver,
FIFO_DATA,
burst_bytes,
&icm_driver.fifo_data[record_idx * IMU_FIFO_PACKET_SIZE_BYTES]);
record_idx += burst_records;
burst_n++;
}
DBG_PRINTF("[IMU FIFO] fifo_read: done bursts=%u rc=%d\r\n", (unsigned)burst_n, rc);
return rc;
}
/* Invensense inv_imu_get_data_from_fifo(): header 0x80 with all payload bytes zero = invalid placeholder. */
static bool imu_fifo_record_is_invalid_placeholder(const uint8_t *rec)
{
uint16_t i;
if (rec[0] != 0x80u)
{
return false;
}
for (i = 1u; i < IMU_FIFO_PACKET_SIZE_BYTES; i++)
{
if (rec[i] != 0u)
{
return false;
}
}
return true;
}
static uint16_t imu_fifo_compact_placeholder_records(uint8_t *fifo, uint16_t record_count)
{
uint16_t w;
uint16_t r;
const uint16_t sz = IMU_FIFO_PACKET_SIZE_BYTES;
for (r = 0, w = 0; r < record_count; r++)
{
uint8_t *rec = fifo + (r * sz);
if (imu_fifo_record_is_invalid_placeholder(rec))
{
continue;
}
if (w != r)
{
memcpy(fifo + (w * sz), rec, sz);
}
w++;
}
return w;
}
static int16_t imu_fifo_record_gyro_axis(const uint8_t *rec, uint8_t axis)
{
const uint8_t *p = &rec[IMU_FIFO_GYRO_OFFSET + ((uint16_t)axis * 2u)];
if (icm_driver.endianness_data == INTF_CONFIG0_DATA_BIG_ENDIAN)
{
return (int16_t)((uint16_t)p[0] << 8 | p[1]);
}
return (int16_t)((uint16_t)p[1] << 8 | p[0]);
}
/* Invensense: gyro axis == INVALID_VALUE_FIFO (0x8000) when not valid in this FIFO record. */
static bool imu_fifo_record_is_invalid_gyro(const uint8_t *rec)
{
uint8_t axis;
for (axis = 0; axis < 3u; axis++)
{
if (imu_fifo_record_gyro_axis(rec, axis) != INVALID_VALUE_FIFO)
{
return false;
}
}
return true;
}
static uint16_t imu_fifo_compact_invalid_gyro_records(uint8_t *fifo, uint16_t record_count)
{
uint16_t w;
uint16_t r;
const uint16_t sz = IMU_FIFO_PACKET_SIZE_BYTES;
for (r = 0, w = 0; r < record_count; r++)
{
uint8_t *rec = fifo + (r * sz);
if (imu_fifo_record_is_invalid_gyro(rec))
{
continue;
}
if (w != r)
{
memcpy(fifo + (w * sz), rec, sz);
}
w++;
}
return w;
}
static uint16_t imu_fifo_normalize_for_rim(uint8_t *fifo, uint16_t n)
{
const uint16_t sz = IMU_FIFO_PACKET_SIZE_BYTES;
#if (IMU_FIFO_RIM_TARGET_SAMPLES > 0)
if (n > (uint16_t)IMU_FIFO_RIM_TARGET_SAMPLES)
{
const uint16_t drop = (uint16_t)(n - (uint16_t)IMU_FIFO_RIM_TARGET_SAMPLES);
memmove(fifo, fifo + ((uint32_t)drop * sz), (uint32_t)IMU_FIFO_RIM_TARGET_SAMPLES * sz);
n = (uint16_t)IMU_FIFO_RIM_TARGET_SAMPLES;
DBG_PRINTF("[IMU FIFO] normalize: capped to newest %u samples\r\n",
(unsigned)IMU_FIFO_RIM_TARGET_SAMPLES);
}
(void)sz;
#elif (IMU_FIFO_RIM_MIN_SAMPLES > 0)
if (n < (uint16_t)IMU_FIFO_RIM_MIN_SAMPLES)
{
while (n < (uint16_t)IMU_FIFO_RIM_MIN_SAMPLES && n < IMU_FIFO_MAX_PACKET_COUNT)
{
memset(fifo + ((uint32_t)n * sz), 0, sz);
n++;
}
if (n < (uint16_t)IMU_FIFO_RIM_MIN_SAMPLES)
{
DBG_PRINTF("[IMU FIFO] normalize: MIN=%u unreachable, using %u\r\n",
(unsigned)IMU_FIFO_RIM_MIN_SAMPLES, (unsigned)n);
}
}
#else
(void)sz;
#endif
return n;
}
int imu_fifo_capture_stop_and_send_rim(void)
{
int rc = 0;
uint8_t count_raw[2] = {0};
uint16_t packet_count;
DBG_PRINTF("[IMU FIFO] stop_send: enter active=%u\r\n", (unsigned)s_fifo_capture_active);
if (!s_fifo_capture_active)
{
DBG_PRINTF("[IMU FIFO] stop_send: not active -> empty rim\r\n");
imu_fifo_send_rim_packets(0);
return -1;
}
rc |= inv_imu_switch_on_mclk(&icm_driver);
DBG_PRINTF("[IMU FIFO] stop_send: mclk on rc=%d\r\n", rc);
rc |= inv_imu_read_reg(&icm_driver, FIFO_COUNTH, 2, count_raw);
packet_count = (uint16_t)count_raw[0] | ((uint16_t)count_raw[1] << 8);
DBG_PRINTF("[IMU FIFO] stop_send: FIFO_COUNT raw[0]=0x%02X [1]=0x%02X -> records=%u\r\n",
count_raw[0], count_raw[1], (unsigned)packet_count);
if (packet_count > IMU_FIFO_MAX_PACKET_COUNT)
{
DBG_PRINTF("[IMU FIFO] stop_send: clamp %u -> %u\r\n",
(unsigned)packet_count, (unsigned)IMU_FIFO_MAX_PACKET_COUNT);
packet_count = IMU_FIFO_MAX_PACKET_COUNT;
}
if ((rc == 0) && (packet_count > 0))
{
uint16_t before_count = packet_count;
rc |= imu_fifo_read_records(packet_count);
if (rc == 0)
{
packet_count = imu_fifo_compact_placeholder_records(icm_driver.fifo_data, packet_count);
if (before_count != packet_count)
{
DBG_PRINTF("[IMU FIFO] stop_send: dropped %u invalid FIFO placeholder record(s)\r\n",
(unsigned)(before_count - packet_count));
}
before_count = packet_count;
packet_count = imu_fifo_compact_invalid_gyro_records(icm_driver.fifo_data, packet_count);
if (before_count != packet_count)
{
DBG_PRINTF("[IMU FIFO] stop_send: dropped %u invalid-gyro FIFO record(s)\r\n",
(unsigned)(before_count - packet_count));
}
}
}
else
{
DBG_PRINTF("[IMU FIFO] stop_send: skip read (rc=%d count=%u)\r\n", rc, (unsigned)packet_count);
}
if ((rc == 0) && (IMU_FIFO_RIM_DROP_LEADING_SAMPLES > 0U) && (packet_count > 0U))
{
const uint16_t drop_req = (uint16_t)IMU_FIFO_RIM_DROP_LEADING_SAMPLES;
const uint16_t drop = (drop_req < packet_count) ? drop_req : packet_count;
const uint16_t sz = IMU_FIFO_PACKET_SIZE_BYTES;
uint8_t *const fifo = icm_driver.fifo_data;
packet_count = (uint16_t)(packet_count - drop);
memmove(fifo, fifo + ((uint32_t)drop * sz), (uint32_t)packet_count * sz);
DBG_PRINTF("[IMU FIFO] stop_send: dropped %u leading sample(s), remain=%u\r\n",
(unsigned)drop, (unsigned)packet_count);
}
if (rc == 0)
{
const uint16_t after_norm = imu_fifo_normalize_for_rim(icm_driver.fifo_data, packet_count);
if (after_norm != packet_count)
{
DBG_PRINTF("[IMU FIFO] stop_send: normalize %u -> %u samples (TARGET/MIN)\r\n",
(unsigned)packet_count, (unsigned)after_norm);
}
packet_count = after_norm;
}
rc |= inv_imu_switch_off_mclk(&icm_driver);
DBG_PRINTF("[IMU FIFO] stop_send: mclk off rc=%d\r\n", rc);
if (rc != 0)
{
DBG_PRINTF("[IMU FIFO] drain fail %d\r\n", rc);
imu_fifo_send_rim_packets(0);
return rc;
}
DBG_PRINTF("[IMU FIFO] stop_send: rim TX records=%u\r\n", (unsigned)packet_count);
imu_fifo_send_rim_packets(packet_count);
DBG_PRINTF("[IMU FIFO] stop_send: OK\r\n");
return 0;
}
project/ble_peripheral/ble_app_bladder_patch/measurement/imu/app_raw/app_raw.h
+45
View File
@@ -97,5 +97,50 @@ void imu_callback(inv_imu_sensor_event_t *event);
*/
int imu_read_direct(void);
/**
* \brief Start IMU internal FIFO capture for mtb? test flow.
* Configures accel/gyro 100 Hz and flushes FIFO before capture.
*/
int imu_fifo_capture_start(void);
/**
* \brief Stop IMU FIFO capture, drain FIFO, and send raw FIFO bytes as rim: packets.
*/
int imu_fifo_capture_stop_and_send_rim(void);
/*
* mtb? / rim: binary layout (every BLE fragment)
* [ 'r' 'i' 'm' ':' ] [ total_sample_count u16 BE ] [ 12 * total_sample_count bytes ... ]
* total_sample_count is the same in each fragment; per-fragment sample count = (payload_len - 6) / 12.
*
* IMU_FIFO_RIM_TARGET_SAMPLES: if > 0, cap at newest N samples (drop older when FIFO has more).
* When fewer than N valid samples remain, send the actual count in the rim header (no zero pad).
* Set to 0 to send all valid samples after filtering (up to the driver buffer limit).
* IMU_FIFO_RIM_DROP_LEADING_SAMPLES: drop the oldest N FIFO-derived samples before MIN/TARGET
* (reduces startup + early-window transients; 0 = off).
* Override before including this header or via -D from the toolchain.
*/
#ifndef IMU_FIFO_RIM_TARGET_SAMPLES
#define IMU_FIFO_RIM_TARGET_SAMPLES 15U
#endif
#ifndef IMU_FIFO_RIM_MIN_SAMPLES
#define IMU_FIFO_RIM_MIN_SAMPLES 0U
#endif
#ifndef IMU_FIFO_RIM_DROP_LEADING_SAMPLES
#define IMU_FIFO_RIM_DROP_LEADING_SAMPLES 0U
#endif
/*
* mtb? FIFO path — ICM42670 UI low-noise filter bandwidth (inv_imu_set_*_ln_bw).
* Enum suffix is approximate -3dB BW in Hz; smaller => smoother, more phase lag.
* Match to imu_fifo_capture_start() ODR (e.g. 25Hz → _16 or _25 typical).
*/
#ifndef IMU_FIFO_MTB_ACCEL_LN_BW
#define IMU_FIFO_MTB_ACCEL_LN_BW ACCEL_CONFIG1_ACCEL_FILT_BW_16
#endif
#ifndef IMU_FIFO_MTB_GYRO_LN_BW
#define IMU_FIFO_MTB_GYRO_LN_BW GYRO_CONFIG1_GYRO_FILT_BW_16
#endif
#endif /* !_APP_RAW_H_ */
project/ble_peripheral/ble_app_bladder_patch/measurement/piezo/dr_piezo.c
+4 -4
View File
@@ -653,10 +653,10 @@ void dr_piezo_mux_init(void)
/*
* Select piezo channel (0~7)
* Channel mapping (EN_MUXA, EN_MUXB, SEL0, SEL1):
* CH0 = MUXA input0 (1,0,0,0) CH4 = MUXB input0 (0,1,1,1)
* CH1 = MUXA input2 (1,0,1,0) CH5 = MUXB input1 (0,1,0,1)
* CH2 = MUXA input1 (1,0,0,1) CH6 = MUXB input2 (0,1,1,0)
* CH3 = MUXA input3 (1,0,1,1) CH7 = MUXB input3 (0,1,0,0)
* CH0 = MUXA input0 (1,0,0,0) CH4 = MUXB input0 (0,1,0,0)
* CH1 = MUXA input2 (1,0,1,0) CH5 = MUXB input1 (0,1,1,0)
* CH2 = MUXA input1 (1,0,0,1)
* CH3 = MUXA input3 (1,0,1,1)
* MUX settling time (~1.3ms) required after channel switch.
*/
void dr_piezo_select_channel_start(uint8_t channel)
project/ble_peripheral/ble_app_bladder_patch/pca10056/s140/arm5_no_packs/ble_app_bladder_patch_s140.uvoptx
+1 -1
View File
@@ -3148,7 +3148,7 @@
<Group>
<GroupName>ICM42670</GroupName>
<tvExp>0</tvExp>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>