Added FIFO timeout to LIS2DW

diff --git a/watch-library/shared/driver/lis2dw.c b/watch-library/shared/driver/lis2dw.c
index 339af08d8a9d3fdf445d38e273388f5b3e936779..fbf1ac75d2e425f93a5593cf0e21c4baf986b991 100644 (file)
--- 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();
     }