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 f2325a8..8f40ee8 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 @@ -50,7 +50,7 @@ /* - * Data output format selection + * SCALED_DATA_G_DPS : Data output format selection * 0 : Raw data output (raw accel, gyro, temp) * 1 : Scaled data output (g, dps, Celsius) */ @@ -353,15 +353,10 @@ void imu_callback(inv_imu_sensor_event_t *event) else temp_degc = 25 + ((float)event->temperature / 2); - /* - * Output scaled data via UART - */ - if (event->sensor_mask & (1 << INV_SENSOR_ACCEL) && event->sensor_mask & (1 << INV_SENSOR_GYRO)) - DBG_PRINTF("%u: %.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f, \t%.3f\r\n", - (uint32_t)timestamp, - accel_g[0], accel_g[1], accel_g[2], - temp_degc, - gyro_dps[0], gyro_dps[1], gyro_dps[2]); + (void)timestamp; + (void)accel_g; + (void)gyro_dps; + (void)temp_degc; #else /* @@ -382,11 +377,6 @@ void imu_callback(inv_imu_sensor_event_t *event) info_imu[5] = (uint16_t)gyro[2]; } - /* UART mode: output 6-axis data in text format with "Tp" prefix */ - else if(cmd_type_t == CMD_UART) { - //DBG_PRINTF("Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]); - } - /* * BLE mode: send 6-axis data as binary packet via BLE * ssp_data[0..2] = accel XYZ, ssp_data[3..5] = gyro XYZ @@ -402,7 +392,6 @@ void imu_callback(inv_imu_sensor_event_t *event) ssp_data[5] = (uint16_t)gyro[2]; format_data(imu_bin_buffer, "rsp:", ssp_data,12); - //DBG_PRINTF("Tp%d,%d,%d,%d,%d,%d\r\n\r\n", accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]); dr_binary_tx_safe(imu_bin_buffer,8); if(custom_add_data==true) { @@ -623,19 +612,16 @@ 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); @@ -646,11 +632,8 @@ 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; } @@ -661,8 +644,6 @@ int imu_fifo_capture_start(void) 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); @@ -681,7 +662,6 @@ int imu_fifo_capture_start(void) 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); @@ -693,7 +673,6 @@ int imu_fifo_capture_start(void) } s_fifo_capture_active = true; - DBG_PRINTF("[IMU FIFO] capture_start: OK active=1 rc_accum=%d\r\n", rc); return 0; } @@ -701,10 +680,6 @@ 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 { @@ -730,28 +705,19 @@ static void imu_fifo_send_rim_packets(uint16_t total_sample_count) 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)) { @@ -764,18 +730,14 @@ static int imu_fifo_read_records(uint16_t record_count) } 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; } @@ -881,8 +843,6 @@ static uint16_t imu_fifo_normalize_for_rim(uint8_t *fifo, uint16_t n) 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) @@ -893,11 +853,6 @@ static uint16_t imu_fifo_normalize_for_rim(uint8_t *fifo, uint16_t n) 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; @@ -911,56 +866,31 @@ int imu_fifo_capture_stop_and_send_rim(void) 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)) { @@ -971,24 +901,14 @@ int imu_fifo_capture_stop_and_send_rim(void) 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; + packet_count = imu_fifo_normalize_for_rim(icm_driver.fifo_data, packet_count); } rc |= inv_imu_switch_off_mclk(&icm_driver); - DBG_PRINTF("[IMU FIFO] stop_send: mclk off rc=%d\r\n", rc); if (rc != 0) { @@ -997,8 +917,6 @@ int imu_fifo_capture_stop_and_send_rim(void) 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; }