diff --git a/firmware/src/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/src/drivers/bosch_bmi160/bosch_bmi160.c
index d682af0..9d067b1 100644
--- a/firmware/src/drivers/bosch_bmi160/bosch_bmi160.c
+++ b/firmware/src/drivers/bosch_bmi160/bosch_bmi160.c
@@ -687,9 +687,16 @@
     }
 
     T(spiInUse) = true;
-    spiMasterRxTx(T(spiDev), T(cs), T(packets), T(mRegCnt), mode, callback, cookie);
+
+    // Reset variables before issuing SPI transaction.
+    // SPI may finish before spiMasterRxTx finish
+    uint8_t regCount = T(mRegCnt);
     T(mRegCnt) = 0;
     T(mWbufCnt) = 0;
+
+    if (spiMasterRxTx(T(spiDev), T(cs), T(packets), regCount, mode, callback, cookie) < 0) {
+        ERROR_PRINT("spiMasterRxTx failed!\n");
+    }
 }
 
 
@@ -3198,6 +3205,13 @@
         return;
     }
 
+    if (T(mRegCnt)) {
+        //chunked read are always executed as a single command. This should never happen.
+        ERROR_PRINT("SPI queue not empty at chunkedReadInit, regcnt = %d", T(mRegCnt));
+        // In case it did happen, we do not want to write crap to BMI160.
+        T(mRegCnt) = 0;
+    }
+
     T(mWbufCnt) = index;
     if (T(mWbufCnt) > FIFO_READ_SIZE) {
         // drop data to prevent bigger issue