From 81e7a24baa867ffae18da4e39544e7a2fbb9df7c Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Wed, 6 Apr 2016 16:49:15 +0200 Subject: [PATCH] [imu] Fix MPU start bug --- sw/airborne/peripherals/mpu60x0_spi.c | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) 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)); + } } }