diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c
index 40f28e1cbd..26fbff9364 100644
--- a/sw/airborne/peripherals/mpu60x0_spi.c
+++ b/sw/airborne/peripherals/mpu60x0_spi.c
@@ -77,10 +77,20 @@ static void mpu60x0_spi_write_to_reg(void *mpu, uint8_t _reg, uint8_t _val)
 void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu)
 {
   if (mpu->config.init_status == MPU60X0_CONF_UNINIT) {
-    mpu->config.init_status++;
-    if (mpu->spi_trans.status == SPITransSuccess || mpu->spi_trans.status == SPITransDone) {
+
+    // First check if we found the chip (succesfull WHO_AM_I response)
+    if(mpu->spi_trans.status == SPITransSuccess && mpu->rx_buf[1] == MPU60X0_WHOAMI_REPLY) {
+      mpu->config.init_status++;
+      mpu->spi_trans.status = SPITransDone;
       mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void *)mpu, &(mpu->config));
     }
+    // Send WHO_AM_I to check if chip is there
+    else if(mpu->spi_trans.status != SPITransRunning && mpu->spi_trans.status != SPITransPending) {
+      mpu->spi_trans.output_length = 1;
+      mpu->spi_trans.input_length = 2;
+      mpu->tx_buf[0] = MPU60X0_REG_WHO_AM_I | MPU60X0_SPI_READ;
+      spi_submit(mpu->spi_p, &(mpu->spi_trans));
+    }
   }
 }