Compare commits
9 Commits
e36efea7aa
...
041201a1d4
| Author | SHA1 | Date | |
|---|---|---|---|
| 041201a1d4 | |||
| 06a6f5687b | |||
| c86be4112c | |||
| a752d14399 | |||
| 7ffb01e16e | |||
| 613116176e | |||
| 945da8911e | |||
| e749730f41 | |||
| f5e54269a1 |
@@ -340,6 +340,11 @@ int Cmd_mbb(const ParsedCmd *cmd)
|
||||
|
||||
maa_async_set_pre_capture_all(true);
|
||||
|
||||
if (!dr_piezo_is_power_on())
|
||||
{
|
||||
dr_piezo_power_on();
|
||||
}
|
||||
|
||||
err = maa_async_start(
|
||||
m_config.piezo_freq_option,
|
||||
m_config.piezo_delay_us,
|
||||
@@ -389,13 +394,13 @@ int Cmd_mtb(const ParsedCmd *cmd)
|
||||
|
||||
(void)imu_fifo_capture_start();
|
||||
|
||||
maa_async_set_pre_capture_all(true);
|
||||
|
||||
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,
|
||||
|
||||
@@ -140,8 +140,8 @@
|
||||
#define APP_ADV_DURATION_UNLIMITED 0 /* Unintended disconnect: unlimited advertising */
|
||||
#define APP_ADV_DURATION APP_ADV_DURATION_NORMAL /* Default adv duration (changed at runtime by disconnect reason) */
|
||||
|
||||
#define MIN_CONN_INTERVAL MSEC_TO_UNITS(30, UNIT_1_25_MS) /* Min connection interval: 30ms */
|
||||
#define MAX_CONN_INTERVAL MSEC_TO_UNITS(30, UNIT_1_25_MS) /* Max connection interval: 30ms */
|
||||
#define MIN_CONN_INTERVAL MSEC_TO_UNITS(15, UNIT_1_25_MS) /* Min connection interval: 15ms */
|
||||
#define MAX_CONN_INTERVAL MSEC_TO_UNITS(15, UNIT_1_25_MS) /* Max connection interval: 15ms */
|
||||
#define SLAVE_LATENCY 0 /* Slave latency: 0 (respond every connection event) */
|
||||
#define CONN_SUP_TIMEOUT MSEC_TO_UNITS(4000, UNIT_10_MS) /* Connection supervision timeout: 4s */
|
||||
#define FIRST_CONN_PARAMS_UPDATE_DELAY APP_TIMER_TICKS(5000) /* Wait 5s before first param update request */
|
||||
@@ -616,9 +616,6 @@ static void nus_data_handler(ble_nus_evt_t * p_evt)
|
||||
{
|
||||
if (p_evt->type == BLE_NUS_EVT_RX_DATA)
|
||||
{
|
||||
cmd_type_t = CMD_BLE;
|
||||
ble_got_new_data = true;
|
||||
|
||||
/* Central may have increased connection interval; re-request fast interval (once per 30s) */
|
||||
{
|
||||
static uint32_t last_update_tick = 0;
|
||||
@@ -635,6 +632,16 @@ static void nus_data_handler(ble_nus_evt_t * p_evt)
|
||||
}
|
||||
}
|
||||
|
||||
#if FEATURE_SECURE_CONNECTION && !BLE_DEV_MODE
|
||||
if (ble_connection_st != BLE_CONNECTED_ST)
|
||||
{
|
||||
return; /* Not authenticated: do not buffer command */
|
||||
}
|
||||
#endif
|
||||
|
||||
cmd_type_t = CMD_BLE;
|
||||
ble_got_new_data = true;
|
||||
|
||||
/* Only copy in callback; process in main loop */
|
||||
if (p_evt->params.rx_data.length <= BLE_NUS_MAX_DATA_LEN)
|
||||
{
|
||||
@@ -1060,6 +1067,7 @@ static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
|
||||
disc_reason, unintended_disc ? " [UNINTENDED]" : "");
|
||||
|
||||
ble_connection_st = 0;
|
||||
pending_cmd_len = 0; // Clear pending command buffer
|
||||
m_conn_handle = BLE_CONN_HANDLE_INVALID;
|
||||
m_tx_in_progress = false;
|
||||
|
||||
@@ -1090,12 +1098,13 @@ static void ble_evt_handler(ble_evt_t const * p_ble_evt, void * p_context)
|
||||
|
||||
case BLE_GAP_EVT_CONNECTED:
|
||||
DBG_PRINTF("[BLE] Connected\r\n");
|
||||
|
||||
#if FEATURE_SECURE_CONNECTION
|
||||
ble_connection_st = 1;
|
||||
|
||||
#if BLE_DEV_MODE
|
||||
/* Dev: no passkey/SEC — allow NUS TX/RX as soon as GAP is up (prod uses PM_EVT_CONN_SEC_SUCCEEDED). */
|
||||
ble_connection_st = BLE_CONNECTED_ST;
|
||||
battery_timer_start();
|
||||
#endif
|
||||
|
||||
|
||||
m_conn_handle = p_ble_evt->evt.gap_evt.conn_handle;
|
||||
err_code = nrf_ble_qwr_conn_handle_assign(&m_qwr, m_conn_handle);
|
||||
APP_ERROR_CHECK(err_code);
|
||||
|
||||
@@ -151,10 +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) */
|
||||
/* maa_async: inter-packet busy-wait (0 = rely on BLE_NUS_EVT_TX_RDY only). */
|
||||
#define MAA_ASYNC_POST_REB_MS 0U
|
||||
#define MAA_ASYNC_POST_RED_MS 0U
|
||||
#define MAA_ASYNC_PRE_RAA_MS 0U
|
||||
#define DR_ADC_AVG_INTER_BURST_GAP_US 500 /* Gap between averaged bursts to reduce residual echo carry-over */
|
||||
|
||||
/* Piezo MUX pins (8ch) */
|
||||
@@ -1348,8 +1348,8 @@ dr_adc_err_t maa_async_start(uint8_t freq_option, uint16_t delay_us, uint16_t nu
|
||||
/* capture all channels without BLE TX, then start transmission */
|
||||
g_maa_ctx.state = MAA_ASYNC_CAPTURING;
|
||||
|
||||
/* Warm up CH0 6 times and discard the results before the real capture starts. */
|
||||
for (uint8_t warmup = 0; warmup < 6; warmup++)
|
||||
/* Warm up CH0 before real capture (MAA_ASYNC_WARMUP_COUNT). */
|
||||
for (uint8_t warmup = 0; warmup < MAA_ASYNC_WARMUP_COUNT; warmup++)
|
||||
{
|
||||
err = dr_adc_capture_channel_only(
|
||||
g_maa_ctx.freq_option,
|
||||
|
||||
@@ -326,6 +326,11 @@ void dr_piezo_select_channel(uint8_t channel);
|
||||
#define MAA_NUM_CHANNELS 6 /* 4 -> 8 -> 6 jhChun 26.03.17*/
|
||||
#define MAA_SAMPLES_MAX 200
|
||||
|
||||
/* CH0 warm-up captures before maa/mbb/mtb (each costs ~1 MUX settle + burst + ADC). */
|
||||
#ifndef MAA_ASYNC_WARMUP_COUNT
|
||||
#define MAA_ASYNC_WARMUP_COUNT 5U
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Echo data for one channel
|
||||
*/
|
||||
|
||||
@@ -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) {
|
||||
@@ -599,8 +588,8 @@ int imu_read_direct(void)
|
||||
#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 IMU_FIFO_ENABLE_SETTLE_MS 20U
|
||||
#define IMU_FIFO_RIM_POST_TX_MS 0U
|
||||
#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)
|
||||
@@ -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,12 +644,10 @@ 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);
|
||||
rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_25_HZ);
|
||||
rc |= inv_imu_set_accel_frequency(&icm_driver, ACCEL_CONFIG0_ODR_50_HZ);
|
||||
rc |= inv_imu_set_gyro_frequency(&icm_driver, GYRO_CONFIG0_ODR_50_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);
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user