diff --git a/package-lock.json b/package-lock.json new file mode 100644 index 0000000..da12562 --- /dev/null +++ b/package-lock.json @@ -0,0 +1,6 @@ +{ + "name": "Vesiscan-Basic_imu", + "lockfileVersion": 3, + "requires": true, + "packages": {} +} 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 a3d88fa..bae5703 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,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); diff --git a/project/ble_peripheral/ble_app_bladder_patch/command/cmd_table.c b/project/ble_peripheral/ble_app_bladder_patch/command/cmd_table.c index 62e687e..632361a 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/command/cmd_table.c +++ b/project/ble_peripheral/ble_app_bladder_patch/command/cmd_table.c @@ -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 }, diff --git a/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_piezo.c b/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_piezo.c index 99afd7c..cc19695 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_piezo.c +++ b/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_piezo.c @@ -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 * diff --git a/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_piezo.h b/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_piezo.h index 9830a79..bca7bb9 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_piezo.h +++ b/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_piezo.h @@ -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 */ 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 b961e01..ed213ad 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 @@ -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(); +} diff --git a/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_sensor.h b/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_sensor.h index 254b9af..dc5be89 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_sensor.h +++ b/project/ble_peripheral/ble_app_bladder_patch/command/handlers/cmd_sensor.h @@ -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 */ diff --git a/project/ble_peripheral/ble_app_bladder_patch/measurement/adc121s051/dr_adc121s051.c b/project/ble_peripheral/ble_app_bladder_patch/measurement/adc121s051/dr_adc121s051.c index 615c7be..9c32c54 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/measurement/adc121s051/dr_adc121s051.c +++ b/project/ble_peripheral/ble_app_bladder_patch/measurement/adc121s051/dr_adc121s051.c @@ -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); 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 fab8a18..f780012 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 @@ -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 /* @@ -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; +} 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 5b9cd8d..d0b88d0 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,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_ */ diff --git a/project/ble_peripheral/ble_app_bladder_patch/measurement/piezo/dr_piezo.c b/project/ble_peripheral/ble_app_bladder_patch/measurement/piezo/dr_piezo.c index e9b95af..770f5e7 100644 --- a/project/ble_peripheral/ble_app_bladder_patch/measurement/piezo/dr_piezo.c +++ b/project/ble_peripheral/ble_app_bladder_patch/measurement/piezo/dr_piezo.c @@ -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) 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 64a0e3a..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 @@ -3148,7 +3148,7 @@ ICM42670 - 0 + 1 0 0 0