IMU FIFO 커맨드 mim? 추가
This commit is contained in:
@@ -986,3 +986,48 @@ int imu_fifo_capture_stop_and_send_rim(void)
|
||||
imu_fifo_send_rim_packets(packet_count);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int imu_fifo_capture_wait_samples_and_send_rim(uint16_t target_samples, uint32_t timeout_ms)
|
||||
{
|
||||
int rc = 0;
|
||||
uint8_t count_raw[2] = {0};
|
||||
uint16_t packet_count = 0;
|
||||
uint32_t waited_ms = 0;
|
||||
const uint32_t poll_ms = 10U;
|
||||
|
||||
if (target_samples == 0U)
|
||||
{
|
||||
return imu_fifo_capture_stop_and_send_rim();
|
||||
}
|
||||
|
||||
if (!s_fifo_capture_active)
|
||||
{
|
||||
imu_fifo_send_rim_packets(0);
|
||||
return -1;
|
||||
}
|
||||
|
||||
while (waited_ms <= timeout_ms)
|
||||
{
|
||||
rc = inv_imu_read_reg(&icm_driver, FIFO_COUNTH, 2, count_raw);
|
||||
if (rc != 0)
|
||||
{
|
||||
DBG_PRINTF("[IMU FIFO] count read fail %d\r\n", rc);
|
||||
imu_fifo_send_rim_packets(0);
|
||||
imu_fifo_power_off();
|
||||
return rc;
|
||||
}
|
||||
|
||||
packet_count = (uint16_t)count_raw[0] | ((uint16_t)count_raw[1] << 8);
|
||||
if (packet_count > target_samples)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
dr_sd_delay_ms(poll_ms);
|
||||
waited_ms += poll_ms;
|
||||
}
|
||||
|
||||
rc = imu_fifo_capture_stop_and_send_rim();
|
||||
imu_fifo_capture_disable();
|
||||
return rc;
|
||||
}
|
||||
|
||||
@@ -122,6 +122,13 @@ int imu_fifo_capture_start(void);
|
||||
*/
|
||||
int imu_fifo_capture_stop_and_send_rim(void);
|
||||
|
||||
/**
|
||||
* \brief Wait until at least target_samples FIFO records are available, then drain
|
||||
* FIFO and send rim: packets.
|
||||
* \return 0=success, negative=error
|
||||
*/
|
||||
int imu_fifo_capture_wait_samples_and_send_rim(uint16_t target_samples, uint32_t timeout_ms);
|
||||
|
||||
/**
|
||||
* \brief Stop IMU FIFO capture without sending rim: packets.
|
||||
*/
|
||||
|
||||
Reference in New Issue
Block a user