diff --git a/epicardium/modules/bhi.c b/epicardium/modules/bhi.c
index 77ad773f5e013bdee71a1a297a80c407139a5ae1..317fb451c4ce5d51e60723ac4ffb5f227335818d 100644
--- a/epicardium/modules/bhi.c
+++ b/epicardium/modules/bhi.c
@@ -262,12 +262,7 @@ static int bhi160_fetch_fifo(void)
 		/* Handle all full packets received in this transfer */
 		uint8_t *fifo_ptr   = bhi160_fifo;
 		uint16_t bytes_left = bytes_read;
-		while (ret == BHY_SUCCESS &&
-		       bytes_left > sizeof(bhy_data_generic_t)) {
-			/*
-			 * TODO: sizeof(bhy_data_generic_t) is probably
-			 * incorrect and makes some measurements arrive late.
-			 */
+		while (bytes_left > 0) {
 			bhy_data_generic_t sensor_data;
 			bhy_data_type_t data_type;
 			ret = bhy_parse_next_fifo_packet(
@@ -279,6 +274,8 @@ static int bhi160_fetch_fifo(void)
 
 			if (ret == BHY_SUCCESS) {
 				bhi160_handle_packet(data_type, &sensor_data);
+			} else {
+				break;
 			}
 		}