diff --git a/watch-library/shared/driver/lis2dw.c b/watch-library/shared/driver/lis2dw.c index 339af08d..fbf1ac75 100644 --- a/watch-library/shared/driver/lis2dw.c +++ b/watch-library/shared/driver/lis2dw.c @@ -277,6 +277,7 @@ inline void lis2dw_disable_fifo(void) { #endif } +#define FIFO_TIMEOUT 102 // This is timeout seconds * 128[RTC_CNT_HZ]. So 800ms is 102 bool lis2dw_read_fifo(lis2dw_fifo_t *fifo_data) { #ifdef I2C_SERCOM uint8_t temp = watch_i2c_read8(LIS2DW_ADDRESS, LIS2DW_REG_FIFO_SAMPLE); @@ -284,7 +285,11 @@ bool lis2dw_read_fifo(lis2dw_fifo_t *fifo_data) { fifo_data->count = temp & LIS2DW_FIFO_SAMPLE_COUNT; + rtc_counter_t timeout_counter = watch_rtc_get_counter() + FIFO_TIMEOUT; for(int i = 0; i < fifo_data->count; i++) { + if (watch_rtc_get_counter() > timeout_counter) { + break; + } fifo_data->readings[i] = lis2dw_get_raw_reading(); }