diff --git a/imu/bmi160_wrapper.c b/imu/bmi160_wrapper.c
index 009b92e9..fe52e327 100644
--- a/imu/bmi160_wrapper.c
+++ b/imu/bmi160_wrapper.c
@@ -85,7 +85,9 @@ static bool reset_init_bmi(BMI_STATE *s) {
 		s->sensor.gyro_cfg.odr = BMI160_GYRO_ODR_1600HZ;
 	}
 
+	chThdSleepMilliseconds(50);
 	int8_t res = bmi160_set_sens_conf(&(s->sensor));
+	chThdSleepMilliseconds(50);
 
 	return res == BMI160_OK;
 }
diff --git a/imu/imu.c b/imu/imu.c
index b1ba2eea..03986429 100644
--- a/imu/imu.c
+++ b/imu/imu.c
@@ -559,8 +559,6 @@ int8_t user_spi_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *data, uint16_t l
 
 	reg_addr = (reg_addr | BMI160_SPI_RD_MASK);
 
-	chThdSleepMicroseconds(200); // #FIXME Wont work without this- Why?
-
 	chMtxLock(&m_spi_bb.mutex);
 	spi_bb_begin(&m_spi_bb);
 	spi_bb_exchange_8(&m_spi_bb, reg_addr);
diff --git a/spi_bb.c b/spi_bb.c
index 7e9d2099..d9f0592d 100644
--- a/spi_bb.c
+++ b/spi_bb.c
@@ -96,7 +96,7 @@ void spi_bb_end(spi_bb_state *s) {
 }
 
 void spi_bb_delay(void) {
-	for (volatile int i = 0; i < 40; i++) {
+	for (volatile int i = 0; i < 6; i++) {
 		__NOP();
 	}
 }