From 6b63c45950d97cbbad15c428d176415afb3680bc Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 12 Mar 2013 22:28:50 +0100 Subject: [PATCH] [mpu] attempt to make a generic configuration (spi/i2c) --- sw/airborne/peripherals/mpu60X0.h | 46 +++++++++++++++++ sw/airborne/peripherals/mpu60X0_spi.c | 74 +++++++-------------------- sw/airborne/peripherals/mpu60X0_spi.h | 4 +- 3 files changed, 65 insertions(+), 59 deletions(-) diff --git a/sw/airborne/peripherals/mpu60X0.h b/sw/airborne/peripherals/mpu60X0.h index 5c3ff29e9f6..2647aee7f05 100644 --- a/sw/airborne/peripherals/mpu60X0.h +++ b/sw/airborne/peripherals/mpu60X0.h @@ -66,6 +66,8 @@ struct Mpu60x0Config { bool_t i2c_bypass; ///< bypass mpu i2c bool_t drdy_int_enable; ///< Enable Data Ready Interrupt uint8_t clk_sel; ///< Clock select + enum Mpu60x0ConfStatus init_status; ///< init status + bool_t initialized; ///< config done flag }; static inline void mpu60X0_set_default_config(struct Mpu60x0Config *c) @@ -79,4 +81,48 @@ static inline void mpu60X0_set_default_config(struct Mpu60x0Config *c) c->clk_sel = MPU60X0_DEFAULT_CLK_SEL; } +/// Configuration function prototype +typedef void (*Mpu60x0ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val); + +/// Configuration sequence called once before normal use +static inline void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config) +{ + switch (config->init_status) { + case MPU60X0_CONF_SD: + mpu_set(mpu, MPU60X0_REG_SMPLRT_DIV, config->smplrt_div); + config->init_status++; + break; + case MPU60X0_CONF_CONFIG: + mpu_set(mpu, MPU60X0_REG_CONFIG, config->dlpf_cfg); + config->init_status++; + break; + case MPU60X0_CONF_GYRO: + mpu_set(mpu, MPU60X0_REG_GYRO_CONFIG, (config->gyro_range<<3)); + config->init_status++; + break; + case MPU60X0_CONF_ACCEL: + mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG, (config->accel_range<<3)); + config->init_status++; + break; + case MPU60X0_CONF_INT_PIN: + mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (config->i2c_bypass<<1)); + config->init_status++; + break; + case MPU60X0_CONF_INT_ENABLE: + mpu_set(mpu, MPU60X0_REG_INT_ENABLE, (config->drdy_int_enable<<0)); + config->init_status++; + break; + case MPU60X0_CONF_PWR: + mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->config.clk_sel)|(0<<6)); + config->init_status++; + break; + case MPU60X0_CONF_DONE: + config->initialized = TRUE; + break; + default: + break; + } +} + + #endif // MPU60X0_H diff --git a/sw/airborne/peripherals/mpu60X0_spi.c b/sw/airborne/peripherals/mpu60X0_spi.c index f02a6027cfe..40144349c5e 100644 --- a/sw/airborne/peripherals/mpu60X0_spi.c +++ b/sw/airborne/peripherals/mpu60X0_spi.c @@ -57,74 +57,35 @@ void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t /* set default MPU60X0 config options */ mpu60x0_set_default_config(&(mpu->config)); - mpu->initialized = FALSE; mpu->data_available = FALSE; - mpu->init_status = MPU60X0_CONF_UNINIT; + mpu->config.initialized = FALSE; + mpu->config.init_status = MPU60X0_CONF_UNINIT; } -static void mpu60x0_spi_write_to_reg(struct Mpu60x0_Spi *mpu, uint8_t _reg, uint8_t _val) { - mpu->spi_trans.output_length = 2; - mpu->spi_trans.input_length = 0; - mpu->tx_buf[0] = _reg; - mpu->tx_buf[1] = _val; - spi_submit(mpu->spi_p, &(mpu->spi_trans)); +static void mpu60x0_spi_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) { + struct Mpu60x0_Spi* mpu_spi = (struct Mpu60x0_Spi*)(mpu); + mpu_spi->spi_trans.output_length = 2; + mpu_spi->spi_trans.input_length = 0; + mpu_spi->tx_buf[0] = _reg; + mpu_spi->tx_buf[1] = _val; + spi_submit(mpu_spi->spi_p, &(mpu_spi->spi_trans)); } // Configuration function called once before normal use -static void mpu60x0_spi_send_config(struct Mpu60x0_Spi *mpu) -{ - switch (mpu->init_status) { - case MPU60X0_CONF_SD: - mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_SMPLRT_DIV, mpu->config.smplrt_div); - mpu->init_status++; - break; - case MPU60X0_CONF_CONFIG: - mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_CONFIG, mpu->config.dlpf_cfg); - mpu->init_status++; - break; - case MPU60X0_CONF_GYRO: - mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_GYRO_CONFIG, (mpu->config.gyro_range<<3)); - mpu->init_status++; - break; - case MPU60X0_CONF_ACCEL: - mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_ACCEL_CONFIG, (mpu->config.accel_range<<3)); - mpu->init_status++; - break; - case MPU60X0_CONF_INT_PIN: - mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_INT_PIN_CFG, (mpu->config.i2c_bypass<<1)); - mpu->init_status++; - break; - case MPU60X0_CONF_INT_ENABLE: - mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_INT_ENABLE, (mpu->config.drdy_int_enable<<0)); - mpu->init_status++; - break; - case MPU60X0_CONF_PWR: - mpu60x0_spi_write_to_reg(mpu, MPU60X0_REG_PWR_MGMT_1, ((mpu->config.clk_sel)|(0<<6)); - mpu->init_status++; - break; - case MPU60X0_CONF_DONE: - mpu->initialized = TRUE; - mpu->spi_trans.status = SPITransDone; - break; - default: - break; - } -} - void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu) { - if (mpu->init_status == MPU60X0_CONF_UNINIT) { - mpu->init_status++; + if (mpu->config.init_status == MPU60X0_CONF_UNINIT) { + mpu->config.init_status++; if (mpu->spi_trans.status == SPITransSuccess || mpu->spi_trans.status == SPITransDone) { - mpu60x0_spi_send_config(mpu); + mpu60x0_spi_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, mpu->config); } } } void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu) { - if (mpu->initialized && mpu->spi_trans.status == SPITransDone) { + if (mpu->config.initialized && mpu->spi_trans.status == SPITransDone) { mpu->spi_trans.output_length = 1; mpu->spi_trans.input_length = 15; // FIXME external data /* set read bit and multiple byte bit, then address */ @@ -137,7 +98,7 @@ void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu) void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu) { - if (mpu->initialized) { + if (mpu->config.initialized) { if (mpu->spi_trans.status == SPITransFailed) { mpu->spi_trans.status = SPITransDone; } @@ -156,14 +117,15 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu) mpu->spi_trans.status = SPITransDone; } } - else if (mpu->init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized + else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized switch (mpu->spi_trans.status) { case SPITransFailed: - mpu->init_status--; // Retry config (TODO max retry) + mpu->config.init_status--; // Retry config (TODO max retry) case SPITransSuccess: case SPITransDone: mpu->spi_trans.status = SPITransDone; - mpu60x0_spi_send_config(mpu); + mpu60x0_spi_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, mpu->config); + if (mpu->config.initialized) mpu->spi_trans.status = SPITransDone; break; default: break; diff --git a/sw/airborne/peripherals/mpu60X0_spi.h b/sw/airborne/peripherals/mpu60X0_spi.h index eba5ca2e134..6eac22267cf 100644 --- a/sw/airborne/peripherals/mpu60X0_spi.h +++ b/sw/airborne/peripherals/mpu60X0_spi.h @@ -43,8 +43,6 @@ struct Mpu60x0_Spi { struct spi_transaction spi_trans; volatile uint8_t tx_buf[MPU60X0_BUFFER_LEN]; // FIXME correct length volatile uint8_t rx_buf[MPU60X0_BUFFER_LEN]; // FIXME idem - enum Mpu60x0ConfStatus init_status; ///< init status - bool_t initialized; ///< config done flag volatile bool_t data_available; ///< data ready flag union { struct Int16Vect3 vect; ///< accel data vector in accel coordinate system @@ -65,7 +63,7 @@ extern void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu); /// convenience function: read or start configuration if not already initialized static inline void mpu60x0_spi_periodic(struct Mpu60x0_Spi *mpu) { - if (mpu->initialized) + if (mpu->config.initialized) mpu60x0_spi_read(mpu); else mpu60x0_spi_start_configure(mpu);