diff --git a/sw/airborne/peripherals/mpu60X0_i2c.c b/sw/airborne/peripherals/mpu60X0_i2c.c new file mode 100644 index 0000000000..716b79061d --- /dev/null +++ b/sw/airborne/peripherals/mpu60X0_i2c.c @@ -0,0 +1,120 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/mpu60x0_i2c.c + * + * Driver for the MPU-60X0 using SPI. + * + */ + +#include "peripherals/mpu60x0_i2c.h" + +void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr) +{ + /* set i2c_peripheral */ + mpu->i2c_p = i2c_p; + + /* slave address */ + mpu->i2c_trans.slave_addr = addr; + /* set inital status: Success or Done */ + mpu->i2c_trans.status = I2CTransDone; + + /* set default MPU60X0 config options */ + mpu60x0_set_default_config(&(mpu->config)); + + mpu->data_available = FALSE; + mpu->config.initialized = FALSE; + mpu->config.init_status = MPU60X0_CONF_UNINIT; +} + + +static void mpu60x0_i2c_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) { + struct Mpu60x0_I2c* mpu_i2c = (struct Mpu60x0_I2c*)(mpu); + mpu_i2c->i2c_trans.buf[0] = _reg; + mpu_i2c->i2c_trans.buf[1] = _val; + mpu_i2c->i2c_trans.len_r = 0; + mpu_i2c->i2c_trans.len_w = 2; + mpu_i2c->i2c_trans.type = I2CTransTx; + i2c_submit(mpu_i2c->i2c_p, &(mpu_i2c->i2c_trans)); +} + +// Configuration function called once before normal use +void mpu60x0_i2c_start_configure(struct Mpu60x0_I2c *mpu) +{ + if (mpu->config.init_status == MPU60X0_CONF_UNINIT) { + mpu->config.init_status++; + if (mpu->i2c_trans.status == I2CTransSuccess || mpu->i2c_trans.status == I2CTransDone) { + mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void*)mpu, mpu->config); + } + } +} + +void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu) +{ + if (mpu->config.initialized && mpu->i2c_trans.status == I2CTransDone) { + mpu->i2c_trans.len_w = 1; + mpu->i2c_trans.len_r = 15; // FIXME external data + mpu->i2c_trans.type = I2CTransTxRx; + /* set read bit and multiple byte bit, then address */ + mpu->i2c_trans.buf[0] = MPU60X0_REG_INT_STATUS; + i2c_submit(mpu->i2c_p, &(mpu->i2c_trans)); + } +} + +#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1])) + +void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu) +{ + if (mpu->config.initialized) { + if (mpu->i2c_trans.status == I2CTransFailed) { + mpu->i2c_trans.status = I2CTransDone; + } + else if (mpu->i2c_trans.status == I2CTransSuccess) { + // Successfull reading + if (bit_is_set(mpu->i2c_trans.buf[0],0)) { + // new data + mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf,1); + mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf,3); + mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf,5); + mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf,9); + mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf,11); + mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf,13); + mpu->data_available = TRUE; + } + mpu->i2c_trans.status = I2CTransDone; + } + } + else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized + switch (mpu->i2c_trans.status) { + case I2CTransFailed: + mpu->config.init_status--; // Retry config (TODO max retry) + case I2CTransSuccess: + case I2CTransDone: + mpu->i2c_trans.status = I2CTransDone; + mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void*)mpu, mpu->config); + if (mpu->config.initialized) mpu->i2c_trans.status = I2CTransDone; + break; + default: + break; + } + } +} diff --git a/sw/airborne/peripherals/mpu60X0_i2c.h b/sw/airborne/peripherals/mpu60X0_i2c.h new file mode 100644 index 0000000000..f7d95eff61 --- /dev/null +++ b/sw/airborne/peripherals/mpu60X0_i2c.h @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/mpu60x0_i2c.h + * + * Driver for the MPU-60X0 using I2C. + */ + +#ifndef MPU60X0_I2C_H +#define MPU60X0_I2C_H + +#include "std.h" +#include "math/pprz_algebra_int.h" +#include "mcu_periph/i2c.h" + +/* Include common MPU60X0 options and definitions */ +#include "peripherals/mpu60x0.h" + + +struct Mpu60x0_I2c { + struct i2c_periph *i2c_p; + struct i2c_transaction i2c_trans; + volatile bool_t data_available; ///< data ready flag + union { + struct Int16Vect3 vect; ///< accel data vector in accel coordinate system + int16_t value[3]; ///< accel data values accessible by channel index + } data_accel; + union { + struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system + int16_t value[3]; ///< rates data values accessible by channel index + } data_rates; + struct Mpu60x0Config config; +}; + +// Functions +extern void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr); +extern void mpu60x0_i2c_start_configure(struct Mpu60x0_I2c *mpu); +extern void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu); +extern void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu); + +/// convenience function: read or start configuration if not already initialized +static inline void mpu60x0_i2c_periodic(struct Mpu60x0_I2c *mpu) { + if (mpu->config.initialized) + mpu60x0_i2c_read(mpu); + else + mpu60x0_i2c_start_configure(mpu); +} + +#endif // MPU60X0_I2C_H diff --git a/sw/airborne/peripherals/mpu60X0_spi.c b/sw/airborne/peripherals/mpu60X0_spi.c index 40144349c5..2522d3ddf3 100644 --- a/sw/airborne/peripherals/mpu60X0_spi.c +++ b/sw/airborne/peripherals/mpu60X0_spi.c @@ -78,7 +78,7 @@ 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) { - mpu60x0_spi_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, mpu->config); + mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, mpu->config); } } } @@ -104,7 +104,7 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu) } else if (mpu->spi_trans.status == SPITransSuccess) { // Successfull reading - if (bit_is_set(mpu->rx_buf,0)) { + if (bit_is_set(mpu->rx_buf[0],0)) { // new data mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf,1); mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf,3); @@ -124,7 +124,7 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu) case SPITransSuccess: case SPITransDone: mpu->spi_trans.status = SPITransDone; - mpu60x0_spi_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, mpu->config); + mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, mpu->config); if (mpu->config.initialized) mpu->spi_trans.status = SPITransDone; break; default: