diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index 70d2ea22ae..6a0ec61b45 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -36,6 +36,19 @@ #include "math/pprz_trig_int.h" #include + +struct Uint8Vect3 { + uint8_t x; + uint8_t y; + uint8_t z; +}; + +struct Int8Vect3 { + int8_t x; + int8_t y; + int8_t z; +}; + struct Uint16Vect3 { uint16_t x; uint16_t y; diff --git a/sw/airborne/peripherals/lis302dl.h b/sw/airborne/peripherals/lis302dl.h new file mode 100644 index 0000000000..a71fabc96a --- /dev/null +++ b/sw/airborne/peripherals/lis302dl.h @@ -0,0 +1,66 @@ +/* + * 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/lis302dl.h + * + * ST LIS302DL 3-axis accelerometer driver common interface (I2C and SPI). + */ + +#ifndef LIS302DL_H +#define LIS302DL_H + +/* Include address and register definition */ +#include "peripherals/lis302dl_regs.h" + +enum Lis302dlConfStatus { + LIS_CONF_UNINIT = 0, + LIS_CONF_WHO_AM_I = 1, + LIS_CONF_WHO_AM_I_OK = 2, + LIS_CONF_REG2 = 3, + LIS_CONF_REG3 = 4, + LIS_CONF_ENABLE = 5, + LIS_CONF_DONE = 6 +}; + +struct Lis302dlConfig { + bool_t int_invert; ///< Invert Interrupt FALSE: active high, TRUE: active low + bool_t spi_3_wire; ///< Set 3-wire SPI mode, if FALSE: 4-wire SPI mode + + /** Filtered Data Selection. + * FALSE: internal filter bypassed; + * TRUE: data from internal filter sent to output register */ + bool_t filt_data; + enum Lis302dlRanges range; ///< g Range + enum Lis302dlRates rate; ///< Data Output Rate +}; + +static inline void lis302dl_set_default_config(struct Lis302dlConfig *c) +{ + c->int_invert = TRUE; + c->filt_data = FALSE; + c->spi_3_wire = FALSE; + + c->rate = LIS302DL_RATE_100HZ; + c->range = LIS302DL_RANGE_2G; +} + +#endif /* LIS302DL_H */ diff --git a/sw/airborne/peripherals/lis302dl_regs.h b/sw/airborne/peripherals/lis302dl_regs.h new file mode 100644 index 0000000000..059c682cff --- /dev/null +++ b/sw/airborne/peripherals/lis302dl_regs.h @@ -0,0 +1,55 @@ +/* + * 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/lis302dl_regs.h + * + * ST LIS302DL 3-axis accelerometer register definitions. + */ + +#ifndef LIS302DL_REGS_H +#define LIS302DL_REGS_H + +/* Registers */ +#define LIS302DL_REG_WHO_AM_I 0x0F +#define LIS302DL_REG_CTRL_REG1 0x20 +#define LIS302DL_REG_CTRL_REG2 0x21 +#define LIS302DL_REG_CTRL_REG3 0x22 +#define LIS302DL_REG_STATUS 0x27 +#define LIS302DL_REG_OUTX 0x29 +#define LIS302DL_REG_OUTY 0x2B +#define LIS302DL_REG_OUTZ 0x2D + +/** LIS302DL device identifier contained in LIS302DL_REG_WHO_AM_I */ +#define LIS302DL_WHO_AM_I 0x3B + +enum Lis302dlRates { + LIS302DL_RATE_100HZ = 0, + LIS302DL_RATE_400HZ = 1 +}; + +enum Lis302dlRanges { + LIS302DL_RANGE_2G = 0, + LIS302DL_RANGE_8G = 1 +}; + + +#endif /* LIS302DL_REGS_H */ diff --git a/sw/airborne/peripherals/lis302dl_spi.c b/sw/airborne/peripherals/lis302dl_spi.c new file mode 100644 index 0000000000..55348f41e0 --- /dev/null +++ b/sw/airborne/peripherals/lis302dl_spi.c @@ -0,0 +1,174 @@ +/* + * 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/lis302dl_spi.c + * + * Driver for LIS302DL 3-axis accelerometer from ST using SPI. + */ + +#include "peripherals/lis302dl_spi.h" + +void lis302dl_spi_init(struct Lis302dl_Spi *lis, struct spi_periph *spi_p, uint8_t slave_idx) +{ + /* set spi_peripheral */ + lis->spi_p = spi_p; + + /* configure spi transaction */ + lis->spi_trans.cpol = SPICpolIdleHigh; + lis->spi_trans.cpha = SPICphaEdge2; + lis->spi_trans.dss = SPIDss8bit; + lis->spi_trans.bitorder = SPIMSBFirst; + lis->spi_trans.cdiv = SPIDiv64; + + lis->spi_trans.select = SPISelectUnselect; + lis->spi_trans.slave_idx = slave_idx; + lis->spi_trans.output_length = 2; + lis->spi_trans.input_length = 8; + // callback currently unused + lis->spi_trans.before_cb = NULL; + lis->spi_trans.after_cb = NULL; + lis->spi_trans.input_buf = &(lis->rx_buf[0]); + lis->spi_trans.output_buf = &(lis->tx_buf[0]); + + /* set inital status: Success or Done */ + lis->spi_trans.status = SPITransDone; + + /* set default LIS302DL config options */ + lis302dl_set_default_config(&(lis->config)); + + lis->initialized = FALSE; + lis->data_available = FALSE; + lis->init_status = LIS_CONF_UNINIT; +} + + +static void lis302dl_spi_write_to_reg(struct Lis302dl_Spi *lis, uint8_t _reg, uint8_t _val) { + lis->spi_trans.output_length = 2; + lis->spi_trans.input_length = 0; + lis->tx_buf[0] = _reg; + lis->tx_buf[1] = _val; + spi_submit(lis->spi_p, &(lis->spi_trans)); +} + +// Configuration function called once before normal use +static void lis302dl_spi_send_config(struct Lis302dl_Spi *lis) +{ + uint8_t reg_val = 0; + + switch (lis->init_status) { + case LIS_CONF_WHO_AM_I: + /* query device id */ + lis->spi_trans.output_length = 1; + lis->spi_trans.input_length = 2; + /* set read bit then reg address */ + lis->tx_buf[0] = (1<<7 | LIS302DL_REG_WHO_AM_I); + if (spi_submit(lis->spi_p, &(lis->spi_trans))) + lis->init_status++; + break; + case LIS_CONF_REG2: + /* set SPI mode, Filtered Data Selection */ + reg_val = (lis->config.spi_3_wire << 7) | (lis->config.filt_data << 4); + lis302dl_spi_write_to_reg(lis, LIS302DL_REG_CTRL_REG2, reg_val); + lis->init_status++; + break; + case LIS_CONF_REG3: + /* Interrupt active high/low */ + lis302dl_spi_write_to_reg(lis, LIS302DL_REG_CTRL_REG3, (lis->config.int_invert << 7)); + lis->init_status++; + break; + case LIS_CONF_ENABLE: + /* set data rate, range, enable measurement, is in standby after power up */ + reg_val = (lis->config.rate << 7) | + (1 << 6) | // Power Down Control to active mode + (lis->config.range << 5) | + 0x5; // enable z,y,x axes + lis302dl_spi_write_to_reg(lis, LIS302DL_REG_CTRL_REG1, reg_val); + lis->init_status++; + break; + case LIS_CONF_DONE: + lis->initialized = TRUE; + lis->spi_trans.status = SPITransDone; + break; + default: + break; + } +} + +void lis302dl_spi_start_configure(struct Lis302dl_Spi *lis) +{ + if (lis->init_status == LIS_CONF_UNINIT) { + lis->init_status++; + if (lis->spi_trans.status == SPITransSuccess || lis->spi_trans.status == SPITransDone) { + lis302dl_spi_send_config(lis); + } + } +} + +void lis302dl_spi_read(struct Lis302dl_Spi *lis) +{ + if (lis->initialized && lis->spi_trans.status == SPITransDone) { + lis->spi_trans.output_length = 1; + lis->spi_trans.input_length = 8; + /* set read bit and multiple byte bit, then address */ + lis->tx_buf[0] = (1<<7|1<<6|LIS302DL_REG_STATUS); + spi_submit(lis->spi_p, &(lis->spi_trans)); + } +} + +void lis302dl_spi_event(struct Lis302dl_Spi *lis) +{ + if (lis->initialized) { + if (lis->spi_trans.status == SPITransFailed) { + lis->spi_trans.status = SPITransDone; + } + else if (lis->spi_trans.status == SPITransSuccess) { + // Successfull reading + if (bit_is_set(lis->rx_buf[1], 3)) { + // new xyz data available + lis->data.vect.x = lis->rx_buf[3]; + lis->data.vect.y = lis->rx_buf[5]; + lis->data.vect.z = lis->rx_buf[7]; + lis->data_available = TRUE; + } + lis->spi_trans.status = SPITransDone; + } + } + else if (lis->init_status != LIS_CONF_UNINIT) { // Configuring but not yet initialized + switch (lis->spi_trans.status) { + case SPITransFailed: + lis->init_status--; // Retry config (TODO max retry) + case SPITransSuccess: + if (lis->init_status == LIS_CONF_WHO_AM_I_OK) { + if (lis->rx_buf[1] == LIS302DL_WHO_AM_I) + lis->init_status++; + else + lis->init_status = LIS_CONF_WHO_AM_I; + } + case SPITransDone: + lis->spi_trans.status = SPITransDone; + lis302dl_spi_send_config(lis); + break; + default: + break; + } + } +} diff --git a/sw/airborne/peripherals/lis302dl_spi.h b/sw/airborne/peripherals/lis302dl_spi.h new file mode 100644 index 0000000000..375f81ca1d --- /dev/null +++ b/sw/airborne/peripherals/lis302dl_spi.h @@ -0,0 +1,68 @@ +/* + * 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/lis302dl_spi.h + * + * Driver for LIS302DL 3-axis accelerometer from ST using SPI. + */ + +#ifndef LIS302DL_SPI_H +#define LIS302DL_SPI_H + +#include "std.h" +#include "math/pprz_algebra_int.h" +#include "mcu_periph/spi.h" + +/* Include common LIS302DL options and definitions */ +#include "peripherals/lis302dl.h" + + +struct Lis302dl_Spi { + struct spi_periph *spi_p; + struct spi_transaction spi_trans; + volatile uint8_t tx_buf[2]; + volatile uint8_t rx_buf[8]; + enum Lis302dlConfStatus init_status; ///< init status + bool_t initialized; ///< config done flag + volatile bool_t data_available; ///< data ready flag + union { + struct Int8Vect3 vect; ///< data vector in accel coordinate system + int8_t value[3]; ///< data values accessible by channel index + } data; + struct Lis302dlConfig config; +}; + +// Functions +extern void lis302dl_spi_init(struct Lis302dl_Spi *lis, struct spi_periph *spi_p, uint8_t addr); +extern void lis302dl_spi_start_configure(struct Lis302dl_Spi *lis); +extern void lis302dl_spi_read(struct Lis302dl_Spi *lis); +extern void lis302dl_spi_event(struct Lis302dl_Spi *lis); + +/// convenience function: read or start configuration if not already initialized +static inline void lis302dl_spi_periodic(struct Lis302dl_Spi *lis) { + if (lis->initialized) + lis302dl_spi_read(lis); + else + lis302dl_spi_start_configure(lis); +} + +#endif // LIS302DL_SPI_H