diff --git a/sw/airborne/peripherals/l3gd20.h b/sw/airborne/peripherals/l3gd20.h new file mode 100644 index 0000000000..607a297f56 --- /dev/null +++ b/sw/airborne/peripherals/l3gd20.h @@ -0,0 +1,58 @@ +/* + * Copyright (C) 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/l3gd20.h + * + * ST L3GD20 3-axis accelerometer driver common interface (I2C and SPI). + */ + +#ifndef L3GD20_H +#define L3GD20_H + +/* Include address and register definition */ +#include "peripherals/l3gd20_regs.h" + +enum L3gd20ConfStatus { + L3G_CONF_UNINIT = 0, + L3G_CONF_WHO_AM_I = 1, + L3G_CONF_WHO_AM_I_OK = 2, + L3G_CONF_REG4 = 3, + L3G_CONF_ENABLE = 4, + L3G_CONF_DONE = 5 +}; + +struct L3gd20Config { + bool_t spi_3_wire; ///< Set 3-wire SPI mode, if FALSE: 4-wire SPI mode + + enum L3gd20FullScale full_scale; ///< gyro full scale + enum L3gd20DRBW drbw; ///< Data rate and bandwidth +}; + +static inline void l3gd20_set_default_config(struct L3gd20Config *c) +{ + c->spi_3_wire = FALSE; + + c->drbw = L3GD20_DRBW_760Hz_100BW; + c->full_scale = L3GD20_FS_2000dps2; +} + +#endif /* L3GD20_H */ diff --git a/sw/airborne/peripherals/l3gd20_regs.h b/sw/airborne/peripherals/l3gd20_regs.h new file mode 100644 index 0000000000..c00f29905f --- /dev/null +++ b/sw/airborne/peripherals/l3gd20_regs.h @@ -0,0 +1,87 @@ +/* + * 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/l3gd20_regs.h + * + * ST L3GD20 3-axis accelerometer register definitions. + */ + +#ifndef L3GD20_REGS_H +#define L3GD20_REGS_H + +/* Registers */ +#define L3GD20_REG_WHO_AM_I 0x0F +#define L3GD20_REG_CTRL_REG1 0x20 +#define L3GD20_REG_CTRL_REG2 0x21 +#define L3GD20_REG_CTRL_REG3 0x22 +#define L3GD20_REG_CTRL_REG4 0x23 +#define L3GD20_REG_STATUS_REG 0x27 +#define L3GD20_REG_OUT_X_L 0x28 +#define L3GD20_REG_OUT_X_H 0x29 +#define L3GD20_REG_OUT_Y_L 0x2A +#define L3GD20_REG_OUT_Y_H 0x2B +#define L3GD20_REG_OUT_Z_L 0x2C +#define L3GD20_REG_OUT_Z_H 0x2D + +/** L3GD20 device identifier contained in L3GD20_REG_WHO_AM_I */ +#define L3GD20_WHO_AM_I 0xD4 + +#define L3GD20_DR_MASK 0xC0 +#define L3GD20_BW_MASK 0x30 + + +#define L3GD20_PD (1 << 3) +#define L3GD20_Xen (1 << 0) +#define L3GD20_Yen (1 << 1) +#define L3GD20_Zen (1 << 2) + +#define L3GD20_FS_MASK 0x30 +#define L3GD20_BDU (1 << 7) + +enum L3gd20DRBW { + L3GD20_DRBW_95Hz_12_5BW, + L3GD20_DRBW_95Hz_25BW, + L3GD20_DRBW_95Hz_25BW2, + L3GD20_DRBW_95Hz_25BW3, + L3GD20_DRBW_190Hz_12_5BW, + L3GD20_DRBW_190Hz_25BW, + L3GD20_DRBW_190Hz_50BW, + L3GD20_DRBW_190Hz_70BW, + L3GD20_DRBW_380Hz_20BW, + L3GD20_DRBW_380Hz_25BW, + L3GD20_DRBW_380Hz_50BW, + L3GD20_DRBW_380Hz_100BW, + L3GD20_DRBW_760Hz_30BW, + L3GD20_DRBW_760Hz_35BW, + L3GD20_DRBW_760Hz_50BW, + L3GD20_DRBW_760Hz_100BW +}; + +enum L3gd20FullScale { + L3GD20_FS_250dps = 0, + L3GD20_FS_500dps = 1, + L3GD20_FS_2000dps = 2, + L3GD20_FS_2000dps2 = 3, +}; + + +#endif /* L3GD20_REGS_H */ diff --git a/sw/airborne/peripherals/l3gd20_spi.c b/sw/airborne/peripherals/l3gd20_spi.c new file mode 100644 index 0000000000..966d5d22ab --- /dev/null +++ b/sw/airborne/peripherals/l3gd20_spi.c @@ -0,0 +1,172 @@ +/* + * Copyright (C) 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/l3gd20_spi.c + * + * Driver for L3GD20 3-axis accelerometer from ST using SPI. + */ + +#include "peripherals/l3gd20_spi.h" + +void l3gd20_spi_init(struct L3gd20_Spi *l3g, struct spi_periph *spi_p, uint8_t slave_idx) +{ + /* set spi_peripheral */ + l3g->spi_p = spi_p; + + /* configure spi transaction */ + l3g->spi_trans.cpol = SPICpolIdleHigh; + l3g->spi_trans.cpha = SPICphaEdge2; + l3g->spi_trans.dss = SPIDss8bit; + l3g->spi_trans.bitorder = SPIMSBFirst; + l3g->spi_trans.cdiv = SPIDiv64; + + l3g->spi_trans.select = SPISelectUnselect; + l3g->spi_trans.slave_idx = slave_idx; + l3g->spi_trans.output_length = 2; + l3g->spi_trans.input_length = 8; + // callback currently unused + l3g->spi_trans.before_cb = NULL; + l3g->spi_trans.after_cb = NULL; + l3g->spi_trans.input_buf = &(l3g->rx_buf[0]); + l3g->spi_trans.output_buf = &(l3g->tx_buf[0]); + + /* set inital status: Success or Done */ + l3g->spi_trans.status = SPITransDone; + + /* set default L3GD20 config options */ + l3gd20_set_default_config(&(l3g->config)); + + l3g->initialized = FALSE; + l3g->data_available = FALSE; + l3g->init_status = L3G_CONF_UNINIT; +} + + +static void l3gd20_spi_write_to_reg(struct L3gd20_Spi *l3g, uint8_t _reg, uint8_t _val) { + l3g->spi_trans.output_length = 2; + l3g->spi_trans.input_length = 0; + l3g->tx_buf[0] = _reg; + l3g->tx_buf[1] = _val; + spi_submit(l3g->spi_p, &(l3g->spi_trans)); +} + +// Configuration function called once before normal use +static void l3gd20_spi_send_config(struct L3gd20_Spi *l3g) +{ + uint8_t reg_val = 0; + + switch (l3g->init_status) { + case L3G_CONF_WHO_AM_I: + /* query device id */ + l3g->spi_trans.output_length = 1; + l3g->spi_trans.input_length = 2; + /* set read bit then reg address */ + l3g->tx_buf[0] = (1<<7 | L3GD20_REG_WHO_AM_I); + if (spi_submit(l3g->spi_p, &(l3g->spi_trans))) + l3g->init_status++; + break; + case L3G_CONF_REG4: + /* set SPI mode, Filtered Data Selection */ + reg_val = (l3g->config.spi_3_wire << 0) | (l3g->config.full_scale << 4); + l3gd20_spi_write_to_reg(l3g, L3GD20_REG_CTRL_REG4, reg_val); + l3g->init_status++; + break; + case L3G_CONF_ENABLE: + /* set data rate, range, enable measurement, is in standby after power up */ + reg_val = (l3g->config.drbw << 4) | + L3GD20_PD | // Power Down Control to active mode + L3GD20_Xen | L3GD20_Yen | L3GD20_Zen; // enable z,y,x axes + l3gd20_spi_write_to_reg(l3g, L3GD20_REG_CTRL_REG1, reg_val); + l3g->init_status++; + break; + case L3G_CONF_DONE: + l3g->initialized = TRUE; + l3g->spi_trans.status = SPITransDone; + break; + default: + break; + } +} + +void l3gd20_spi_start_configure(struct L3gd20_Spi *l3g) +{ + if (l3g->init_status == L3G_CONF_UNINIT) { + l3g->init_status++; + if (l3g->spi_trans.status == SPITransSuccess || l3g->spi_trans.status == SPITransDone) { + l3gd20_spi_send_config(l3g); + } + } +} + +void l3gd20_spi_read(struct L3gd20_Spi *l3g) +{ + if (l3g->initialized && l3g->spi_trans.status == SPITransDone) { + l3g->spi_trans.output_length = 1; + l3g->spi_trans.input_length = 8; + /* set read bit and multiple byte bit, then address */ + l3g->tx_buf[0] = (1<<7|1<<6|L3GD20_REG_STATUS_REG); + spi_submit(l3g->spi_p, &(l3g->spi_trans)); + } +} + +#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx])) + +void l3gd20_spi_event(struct L3gd20_Spi *l3g) +{ + if (l3g->initialized) { + if (l3g->spi_trans.status == SPITransFailed) { + l3g->spi_trans.status = SPITransDone; + } + else if (l3g->spi_trans.status == SPITransSuccess) { + // Successfull reading + if (bit_is_set(l3g->rx_buf[1], 3)) { + // new xyz data available + l3g->data_rates.rates.p = Int16FromBuf(l3g->rx_buf,2); + l3g->data_rates.rates.q = Int16FromBuf(l3g->rx_buf,4); + l3g->data_rates.rates.r = Int16FromBuf(l3g->rx_buf,6); + l3g->data_available = TRUE; + } + l3g->spi_trans.status = SPITransDone; + } + } + else if (l3g->init_status != L3G_CONF_UNINIT) { // Configuring but not yet initialized + switch (l3g->spi_trans.status) { + case SPITransFailed: + l3g->init_status--; // Retry config (TODO max retry) + case SPITransSuccess: + if (l3g->init_status == L3G_CONF_WHO_AM_I_OK) { + if (l3g->rx_buf[1] == L3GD20_WHO_AM_I) { + l3g->init_status++; + } + else { + l3g->init_status = L3G_CONF_WHO_AM_I; + } + } + case SPITransDone: + l3g->spi_trans.status = SPITransDone; + l3gd20_spi_send_config(l3g); + break; + default: + break; + } + } +} diff --git a/sw/airborne/peripherals/l3gd20_spi.h b/sw/airborne/peripherals/l3gd20_spi.h new file mode 100644 index 0000000000..2df40e190b --- /dev/null +++ b/sw/airborne/peripherals/l3gd20_spi.h @@ -0,0 +1,67 @@ +/* + * Copyright (C) 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/l3gd20_spi.h + * + * Driver for L3GD20 3-axis accelerometer from ST using SPI. + */ + +#ifndef L3GD20_SPI_H +#define L3GD20_SPI_H + +#include "std.h" +#include "math/pprz_algebra_int.h" +#include "mcu_periph/spi.h" + +/* Include common L3GD20 options and definitions */ +#include "peripherals/l3gd20.h" + +struct L3gd20_Spi { + struct spi_periph *spi_p; + struct spi_transaction spi_trans; + volatile uint8_t tx_buf[2]; + volatile uint8_t rx_buf[8]; + enum L3gd20ConfStatus init_status; ///< init status + bool_t initialized; ///< config done flag + volatile bool_t data_available; ///< data ready flag + union { + struct Int16Rates rates; ///< data vector in accel coordinate system + int16_t value[3]; ///< data values accessible by channel index + } data_rates; + struct L3gd20Config config; +}; + +// Functions +extern void l3gd20_spi_init(struct L3gd20_Spi *l3g, struct spi_periph *spi_p, uint8_t addr); +extern void l3gd20_spi_start_configure(struct L3gd20_Spi *l3g); +extern void l3gd20_spi_read(struct L3gd20_Spi *l3g); +extern void l3gd20_spi_event(struct L3gd20_Spi *l3g); + +/// convenience function: read or start configuration if not already initialized +static inline void l3gd20_spi_periodic(struct L3gd20_Spi *l3g) { + if (l3g->initialized) + l3gd20_spi_read(l3g); + else + l3gd20_spi_start_configure(l3g); +} + +#endif // L3GD20_SPI_H