IMU FIFO 동시 진입 방지 및 MCLK ready timeout 추가
- IMU FIFO capture가 이미 활성화되어 있는 상태에서 msp?/mim? 등이 들어오는 경우 요청 무시 - MCLK_RDY 대기에 timeout 추가해 IMU/I2C 상태가 꼬였을 때 메인 루프가 멈추지 않도록 함
This commit is contained in:
+15
@@ -101,6 +101,7 @@ int inv_imu_switch_on_mclk(struct inv_imu_device *s)
|
||||
int status = 0;
|
||||
uint8_t data;
|
||||
struct inv_imu_transport *t = (struct inv_imu_transport *)s;
|
||||
uint64_t start;
|
||||
|
||||
/* set IDLE bit only if it is not set yet */
|
||||
if (t->need_mclk_cnt == 0) {
|
||||
@@ -112,9 +113,23 @@ int inv_imu_switch_on_mclk(struct inv_imu_device *s)
|
||||
if (status)
|
||||
return status;
|
||||
|
||||
start = inv_imu_get_time_us();
|
||||
|
||||
/* Check if MCLK is ready */
|
||||
do {
|
||||
status = inv_imu_read_reg(s, MCLK_RDY, 1, &data);
|
||||
|
||||
/* Bound the MCLK wait so a bad IMU/I2C state cannot hang the main loop. */
|
||||
if ((inv_imu_get_time_us() - start) >= 50000U)
|
||||
{
|
||||
status = 0;
|
||||
if (inv_imu_read_reg(s, PWR_MGMT0, 1, &data) == 0)
|
||||
{
|
||||
data &= ~PWR_MGMT0_IDLE_MASK;
|
||||
(void)inv_imu_write_reg(s, PWR_MGMT0, 1, &data);
|
||||
}
|
||||
return INV_ERROR_TIMEOUT;
|
||||
}
|
||||
} while ((status != 0) || !(data & MCLK_RDY_MCLK_RDY_MASK));
|
||||
} else {
|
||||
|
||||
|
||||
Reference in New Issue
Block a user