diff --git a/sw/airborne/peripherals/lsm303dlhc.c b/sw/airborne/peripherals/lsm303dlhc.c new file mode 100644 index 0000000000..a3eea7b057 --- /dev/null +++ b/sw/airborne/peripherals/lsm303dlhc.c @@ -0,0 +1,255 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * 2013 Felix Ruess + * + * 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/lsm303dlhc.c + * + * Driver for ST LSM303DLHC 3D accelerometer and magnetometer. + */ + +#include "peripherals/lsm303dlhc.h" +#include "std.h" + +/* LSM303DLHC default conf */ +#ifndef LSM303DLHC_DEFAULT_ODR +#define LSM303DLHC_DEFAULT_ODR 0x90 //90 //normal 1.344khz, low power 5.376khz +#endif + +#ifndef LSM303DLHC_DEFAULT_LP +#define LSM303DLHC_DEFAULT_LP 0x00 //low power disabled +#endif + +#ifndef LSM303DLHC_DEFAULT_FS +#define LSM303DLHC_DEFAULT_FS 0x00 // +- 2G +#endif + +#ifndef LSM303DLHC_DEFAULT_HR +#define LSM303DLHC_DEFAULT_HR 0x04 // high res enabled +#endif + +#ifndef LSM303DLHC_DEFAULT_DO +#define LSM303DLHC_DEFAULT_DO (0x6 << 2) // Data Output Rate (75Hz) +#endif + +#ifndef LSM303DLHC_DEFAULT_GN +#define LSM303DLHC_DEFAULT_GN (0x1 << 5) // Gain configuration (1 -> +- 1.3 Gauss) +#endif + +#ifndef LSM303DLHC_DEFAULT_MD +#define LSM303DLHC_DEFAULT_MD 0x00 // Continious conversion mode +#endif + +static void lsm303dlhc_acc_set_default_config(struct Lsm303dlhcAccConfig *c) +{ + c->rate = LSM303DLHC_DEFAULT_ODR; + c->lp_mode = LSM303DLHC_DEFAULT_LP; + c->scale = LSM303DLHC_DEFAULT_FS; + c->hres = LSM303DLHC_DEFAULT_HR; +} + +static void lsm303dlhc_mag_set_default_config(struct Lsm303dlhcMagConfig *c) +{ + c->rate = (LSM303DLHC_DEFAULT_DO & LSM303DLHC_DO0_MASK); + c->gain = (LSM303DLHC_DEFAULT_GN & LSM303DLHC_GN_MASK); + c->mode = (LSM303DLHC_DEFAULT_MD & LSM303DLHC_MD_MASK); +} + +/** + * Initialize Lsm303dlhc struct and set default config options. + * @param lsm Lsm303dlhc struct + * @param i2c_p I2C peripheral to use + * @param addr I2C address of Lsm303dlhc + */ +void lsm303dlhc_init(struct Lsm303dlhc *lsm, struct i2c_periph *i2c_p, uint8_t addr) +{ + /* set i2c_peripheral */ + lsm->i2c_p = i2c_p; + /* set i2c address */ + lsm->i2c_trans.slave_addr = addr; + lsm->i2c_trans.status = I2CTransDone; + /* set default config options */ + if (addr == LSM303DLHC_ACC_ADDR) { + lsm303dlhc_acc_set_default_config(&(lsm->config.acc)); + lsm->init_status.acc = LSM_CONF_ACC_UNINIT; + } else { + lsm303dlhc_mag_set_default_config(&(lsm->config.mag)); + lsm->init_status.mag = LSM_CONF_MAG_UNINIT; + } + lsm->initialized = FALSE; +} + +static void lsm303dlhc_i2c_tx_reg(struct Lsm303dlhc *lsm, uint8_t reg, uint8_t val) +{ + lsm->i2c_trans.type = I2CTransTx; + lsm->i2c_trans.buf[0] = reg; + lsm->i2c_trans.buf[1] = val; + lsm->i2c_trans.len_r = 0; + lsm->i2c_trans.len_w = 2; + i2c_submit(lsm->i2c_p, &(lsm->i2c_trans)); +} + +/// Configuration function called once before normal use +static void lsm303dlhc_send_config(struct Lsm303dlhc *lsm) +{ + if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) { + switch (lsm->init_status.acc) { + case LSM_CONF_ACC_CTRL_REG4_A: + lsm303dlhc_i2c_tx_reg(lsm, LSM303DLHC_REG_CTRL_REG4_A, + (lsm->config.acc.scale & LSM303DLHC_FS_MASK) | + (lsm->config.acc.hres & LSM303DLHC_DEFAULT_HR)); + lsm->init_status.acc++; + break; + case LSM_CONF_ACC_CTRL_REG1_A: + lsm303dlhc_i2c_tx_reg(lsm, LSM303DLHC_REG_CTRL_REG1_A, + (lsm->config.acc.rate & LSM303DLHC_ODR_MASK) | + (lsm->config.acc.lp_mode & LSM303DLHC_LPen) | + LSM303DLHC_Xen | LSM303DLHC_Yen | LSM303DLHC_Zen); + lsm->init_status.acc++; + break; + case LSM_CONF_ACC_CTRL_REG3_A: + lsm303dlhc_i2c_tx_reg(lsm, LSM303DLHC_REG_CTRL_REG3_A, LSM303DLHC_I1_DRDY1); + lsm->init_status.acc++; + break; + case LSM_CONF_ACC_DONE: + lsm->initialized = TRUE; + lsm->i2c_trans.status = I2CTransDone; + lsm303dlhc_read(lsm); + break; + default: + break; + } + } else { + switch (lsm->init_status.mag) { + case LSM_CONF_MAG_CRA_REG_M: + lsm303dlhc_i2c_tx_reg(lsm, LSM303DLHC_REG_CRA_REG_M, lsm->config.mag.rate); + lsm->init_status.mag++; + break; + case LSM_CONF_MAG_CRB_REG_M: + lsm303dlhc_i2c_tx_reg(lsm, LSM303DLHC_REG_CRB_REG_M, lsm->config.mag.gain); + lsm->init_status.mag++; + break; + case LSM_CONF_MAG_MR_REG_M: + lsm303dlhc_i2c_tx_reg(lsm, LSM303DLHC_REG_MR_REG_M, lsm->config.mag.mode); + lsm->init_status.mag++; + break; + case LSM_CONF_MAG_DONE: + lsm->initialized = TRUE; + lsm->i2c_trans.status = I2CTransDone; + break; + default: + break; + } + } +} + +// Configure +void lsm303dlhc_start_configure(struct Lsm303dlhc *lsm) +{ + if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) { + if (lsm->init_status.acc == LSM_CONF_ACC_UNINIT) { + lsm->init_status.acc++; + if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) { + lsm303dlhc_send_config(lsm); + } + } + } else { + if (lsm->init_status.mag == LSM_CONF_MAG_UNINIT) { + lsm->init_status.mag++; + if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) { + lsm303dlhc_send_config(lsm); + } + } + } +} + +// Normal reading +void lsm303dlhc_read(struct Lsm303dlhc *lsm) +{ + if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) { + //if ((lsm->init_status.acc == LSM_CONF_ACC_CLR_INT_READ) && (lsm->i2c_trans.status == I2CTransDone)){ + if (!(lsm->initialized) || (lsm->initialized && lsm->i2c_trans.status == I2CTransDone)){ + lsm->i2c_trans.buf[0] = LSM303DLHC_REG_OUT_X_L_A | 0x80; + lsm->i2c_trans.type = I2CTransTxRx; + lsm->i2c_trans.len_r = 6; + lsm->i2c_trans.len_w = 1; + i2c_submit(lsm->i2c_p, &(lsm->i2c_trans)); + } + } + else { + if (lsm->initialized && lsm->i2c_trans.status == I2CTransDone){ + lsm->i2c_trans.buf[0] = LSM303DLHC_REG_OUT_X_H_M; + lsm->i2c_trans.type = I2CTransTxRx; + lsm->i2c_trans.len_r = 6; + lsm->i2c_trans.len_w = 1; + i2c_submit(lsm->i2c_p, &(lsm->i2c_trans)); + } + } +} + +#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx])) + +void lsm303dlhc_event(struct Lsm303dlhc *lsm) +{ + if (lsm->initialized) { + if (lsm->i2c_trans.status == I2CTransFailed) { + lsm->i2c_trans.status = I2CTransDone; + } + else if (lsm->i2c_trans.status == I2CTransSuccess) { + lsm->data.vect.x = Int16FromBuf(lsm->i2c_trans.buf,0); + lsm->data.vect.y = Int16FromBuf(lsm->i2c_trans.buf,2); + lsm->data.vect.z = Int16FromBuf(lsm->i2c_trans.buf,4); + lsm->data_available = TRUE; + lsm->i2c_trans.status = I2CTransDone; + } + else { + } + } + else + { + if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) { + if (lsm->init_status.acc != LSM_CONF_ACC_UNINIT) { // Configuring but not yet initialized + if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) { + lsm->i2c_trans.status = I2CTransDone; + lsm303dlhc_send_config(lsm); + } + if (lsm->i2c_trans.status == I2CTransFailed) { + lsm->init_status.acc--; + lsm->i2c_trans.status = I2CTransDone; + lsm303dlhc_send_config(lsm); // Retry config (TODO max retry) + } + } + } + else { + if (lsm->init_status.mag != LSM_CONF_MAG_UNINIT) { // Configuring but not yet initialized + if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) { + lsm->i2c_trans.status = I2CTransDone; + lsm303dlhc_send_config(lsm); + } + if (lsm->i2c_trans.status == I2CTransFailed) { + lsm->init_status.mag--; + lsm->i2c_trans.status = I2CTransDone; + lsm303dlhc_send_config(lsm); // Retry config (TODO max retry) + } + } + } + } +} diff --git a/sw/airborne/peripherals/lsm303dlhc.h b/sw/airborne/peripherals/lsm303dlhc.h new file mode 100644 index 0000000000..13c598b538 --- /dev/null +++ b/sw/airborne/peripherals/lsm303dlhc.h @@ -0,0 +1,107 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * 2013 Felix Ruess + * + * 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/lsm303dlhc.h + * + * Driver for ST LSM303DLHC 3D accelerometer and magnetometer. + */ + +#ifndef LSM303DLHC_H +#define LSM303DLHC_H + +#include "std.h" +#include "mcu_periph/i2c.h" +#include "math/pprz_algebra_int.h" + +/* Address and register definitions */ +#include "peripherals/lsm303dlhc_regs.h" + +struct Lsm303dlhcAccConfig { + uint8_t rate; ///< Data Output Rate Bits(6 -> 50Hz with HMC5843, 75Hz with HMC5883) + uint8_t lp_mode; ///< Low power mode + uint8_t scale; ///< full scale selection + uint8_t hres; ///< high resolution output mode +}; + +struct Lsm303dlhcMagConfig { + uint8_t rate; ///< Data Output Rate Bits(6 -> 50Hz with HMC5843, 75Hz with HMC5883) + uint8_t gain; ///< Gain configuration (1 -> +- 1 Gauss) + uint8_t mode; ///< Measurement mode +}; + +/** config status states */ +enum Lsm303dlhcAccConfStatus { + LSM_CONF_ACC_UNINIT, + LSM_CONF_ACC_CTRL_REG4_A, + LSM_CONF_ACC_CTRL_REG1_A, + LSM_CONF_ACC_CTRL_REG3_A, + LSM_CONF_ACC_DONE +}; + +/** config status states */ +enum Lsm303dlhcMagConfStatus { + LSM_CONF_MAG_UNINIT, + LSM_CONF_MAG_CRA_REG_M, + LSM_CONF_MAG_CRB_REG_M, + LSM_CONF_MAG_MR_REG_M, + LSM_CONF_MAG_DONE +}; + +struct Lsm303dlhc { + struct i2c_periph *i2c_p; + struct i2c_transaction i2c_trans; + bool_t initialized; ///< config done flag + union { + enum Lsm303dlhcAccConfStatus acc; ///< init status + enum Lsm303dlhcMagConfStatus mag; ///< init status + } init_status; + volatile bool_t data_available; ///< data ready flag + union { + struct Int16Vect3 vect; ///< data vector in acc coordinate system + int16_t value[3]; ///< data values accessible by channel index + } data; + union { + struct Lsm303dlhcAccConfig acc; + struct Lsm303dlhcMagConfig mag; + } config; +}; + + + +// TODO IRQ handling + +// Functions +extern void lsm303dlhc_init(struct Lsm303dlhc *lsm, struct i2c_periph *i2c_p, uint8_t addr); +extern void lsm303dlhc_start_configure(struct Lsm303dlhc *lsm); +extern void lsm303dlhc_read(struct Lsm303dlhc *lsm); +extern void lsm303dlhc_event(struct Lsm303dlhc *lsm); + +/// convenience function: read or start configuration if not already initialized +static inline void lsm303dlhc_periodic(struct Lsm303dlhc *lsm) { + if (lsm->initialized) + lsm303dlhc_read(lsm); + else + lsm303dlhc_start_configure(lsm); +} + +#endif /* LSM303DLHC_H */ diff --git a/sw/airborne/peripherals/lsm303dlhc_regs.h b/sw/airborne/peripherals/lsm303dlhc_regs.h new file mode 100644 index 0000000000..1a325ee0ad --- /dev/null +++ b/sw/airborne/peripherals/lsm303dlhc_regs.h @@ -0,0 +1,100 @@ +/* + * Copyright (C) 2014 Federico Ruiz Ugalde + * + * 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/lsm303dlhc_regs.h + * Register defs for ST LSM303DLHC 3D accelerometer and magnetometer. + */ + +#ifndef LSM303DLHC_REGS_H +#define LSM303DLHC_REGS_H + +/* default I2C address */ +#define LSM303DLHC_ACC_ADDR 0x32 +#define LSM303DLHC_MAG_ADDR 0x3C + +/* Registers */ +#define LSM303DLHC_REG_CTRL_REG1_A 0x20 +#define LSM303DLHC_REG_CTRL_REG2_A 0x21 +#define LSM303DLHC_REG_CTRL_REG3_A 0x22 +#define LSM303DLHC_REG_CTRL_REG4_A 0x23 +#define LSM303DLHC_REG_CTRL_REG5_A 0x24 +#define LSM303DLHC_REG_CTRL_REG6_A 0x25 +#define LSM303DLHC_REG_REF_DATA_CAP_A 0x26 +#define LSM303DLHC_REG_STATUS_REG_A 0x27 +#define LSM303DLHC_REG_OUT_X_L_A 0x28 +#define LSM303DLHC_REG_OUT_X_H_A 0x29 +#define LSM303DLHC_REG_OUT_Y_L_A 0x2A +#define LSM303DLHC_REG_OUT_Y_H_A 0x2B +#define LSM303DLHC_REG_OUT_Z_L_A 0x2C +#define LSM303DLHC_REG_OUT_Z_H_A 0x2D +#define LSM303DLHC_REG_FIFO_CTRL_REG_A 0x2E +#define LSM303DLHC_REG_FIFO_SRC_REG_A 0x2F +#define LSM303DLHC_REG_INT1_CFG_A 0x30 +#define LSM303DLHC_REG_INT1_SRC_A 0x31 +#define LSM303DLHC_REG_INT1_THS_A 0x32 +#define LSM303DLHC_REG_INT1_DURATION_A 0x33 +#define LSM303DLHC_REG_INT2_CFG_A 0x34 +#define LSM303DLHC_REG_INT2_SRC_A 0x35 +#define LSM303DLHC_REG_INT2_THS_A 0x36 +#define LSM303DLHC_REG_INT2_DURATION_A 0x37 +#define LSM303DLHC_REG_CLICK_CFG_A 0x38 +#define LSM303DLHC_REG_CLICK_SRC_A 0x39 +#define LSM303DLHC_REG_CLICK_THS_A 0x3A +#define LSM303DLHC_REG_TIME_LIMIT_A 0x3B +#define LSM303DLHC_REG_TIME_LATENCY_A 0x3C +#define LSM303DLHC_REG_TIME_WINDOW_A 0x3D +#define LSM303DLHC_REG_CRA_REG_M 0x00 +#define LSM303DLHC_REG_CRB_REG_M 0x01 +#define LSM303DLHC_REG_MR_REG_M 0x02 +#define LSM303DLHC_REG_OUT_X_H_M 0x03 +#define LSM303DLHC_REG_OUT_X_L_M 0x04 +#define LSM303DLHC_REG_OUT_Z_H_M 0x05 +#define LSM303DLHC_REG_OUT_Z_L_M 0x06 +#define LSM303DLHC_REG_OUT_Y_H_M 0x07 +#define LSM303DLHC_REG_OUT_Y_L_M 0x08 +#define LSM303DLHC_REG_SR_REG_M 0x09 +#define LSM303DLHC_REG_IRA_REG_M 0x0A +#define LSM303DLHC_REG_IRB_REG_M 0x0B +#define LSM303DLHC_REG_IRC_REG_M 0x0C +#define LSM303DLHC_REG_TEMP_OUT_H_M 0x31 +#define LSM303DLHC_REG_TEMP_OUT_L_M 0x32 + +/* Bit definitions */ +#define LSM303DLHC_ODR_MASK 0xF0 +#define LSM303DLHC_LPen (1 << 3) +#define LSM303DLHC_Xen (1 << 0) +#define LSM303DLHC_Yen (1 << 1) +#define LSM303DLHC_Zen (1 << 2) + +#define LSM303DLHC_FS_MASK 0x30 +#define LSM303DLHC_HR (1 << 3) +#define LSM303DLHC_BDU (1 << 7) + +#define LSM303DLHC_I1_DRDY1 (1 << 4) + +#define LSM303DLHC_DO0_MASK 0x1C +#define LSM303DLHC_GN_MASK 0xE0 +#define LSM303DLHC_MD_MASK 0x03 + + + +#endif // LSM303DLHC_REGS_H