diff --git a/sw/airborne/peripherals/l3g4200.c b/sw/airborne/peripherals/l3g4200.c new file mode 100644 index 0000000000..d57ff6c241 --- /dev/null +++ b/sw/airborne/peripherals/l3g4200.c @@ -0,0 +1,144 @@ +/* + * + * Copyright (C) 2011 Gautier Hattenberger + * 2013 Felix Ruess + * 2013 Eduardo Lavratti + * + * 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/l3g4200.c + * + * Driver for L3G4200 from ST. + */ + +#include "peripherals/l3g4200.h" +#include "std.h" + +void l3g4200_set_default_config(struct L3g4200Config *c) { + c->ctrl_reg1 = L3G4200_DEFAULT_CTRL_REG1; + c->ctrl_reg5 = L3G4200_DEFAULT_CTRL_REG5; +} + +/** + * Initialize L3g4200 struct and set default config options. + * @param l3g L3g4200 struct + * @param i2c_p I2C periperal to use + * @param addr I2C address of L3G4200 + */ + +void l3g4200_init(struct L3g4200 *l3g, struct i2c_periph *i2c_p, uint8_t addr) +{ + /* set i2c_peripheral */ + l3g->i2c_p = i2c_p; + /* set i2c address */ + l3g->i2c_trans.slave_addr = addr; + l3g->i2c_trans.status = I2CTransDone; + /* set default config options */ + l3g4200_set_default_config(&(l3g->config)); + l3g->initialized = FALSE; + l3g->init_status = L3G_CONF_UNINIT; +} + +static void l3g4200_i2c_tx_reg(struct L3g4200 *l3g, uint8_t reg, uint8_t val) +{ + l3g->i2c_trans.type = I2CTransTx; + l3g->i2c_trans.buf[0] = reg; + l3g->i2c_trans.buf[1] = val; + l3g->i2c_trans.len_r = 0; + l3g->i2c_trans.len_w = 2; + i2c_submit(l3g->i2c_p, &(l3g->i2c_trans)); +} + +// Configuration function called once before normal use +static void l3g4200_send_config(struct L3g4200 *l3g) +{ + switch (l3g->init_status) { + case L3G_CONF_REG1: + l3g4200_i2c_tx_reg(l3g, L3G4200_REG_CTRL_REG1, l3g->config.ctrl_reg1); + l3g->init_status++; + break; + case L3G_CONF_REG5: + l3g4200_i2c_tx_reg(l3g, L3G4200_REG_CTRL_REG5, l3g->config.ctrl_reg5); + l3g->init_status++; + break; + case L3G_CONF_DONE: + l3g->initialized = TRUE; + l3g->i2c_trans.status = I2CTransDone; + break; + default: + break; + } +} + +// Configure +void l3g4200_start_configure(struct L3g4200 *l3g) +{ + if (l3g->init_status == L3G_CONF_UNINIT) { + l3g->init_status++; + if (l3g->i2c_trans.status == I2CTransSuccess || l3g->i2c_trans.status == I2CTransDone) { + l3g4200_send_config(l3g); + } + } +} + +// Normal reading +void l3g4200_read(struct L3g4200 *l3g) +{ + if (l3g->initialized && l3g->i2c_trans.status == I2CTransDone) { + l3g->i2c_trans.buf[0] = L3G4200_REG_STATUS_REG; + l3g->i2c_trans.type = I2CTransTxRx; + l3g->i2c_trans.len_r = 9; + l3g->i2c_trans.len_w = 1; + i2c_submit(l3g->i2c_p, &(l3g->i2c_trans)); + } +} + +#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1])) + +void l3g4200_event(struct L3g4200 *l3g) +{ + if (l3g->initialized) { + if (l3g->i2c_trans.status == I2CTransFailed) { + l3g->i2c_trans.status = I2CTransDone; + } + else if (l3g->i2c_trans.status == I2CTransSuccess) { + // Successfull reading and new data available + if (l3g->i2c_trans.buf[0] & 0x01) { // ver oque é o sinal antes do & + // New data available + l3g->data.rates.p = Int16FromBuf(l3g->i2c_trans.buf,3); + l3g->data.rates.q = Int16FromBuf(l3g->i2c_trans.buf,5); + l3g->data.rates.r = Int16FromBuf(l3g->i2c_trans.buf,7); + l3g->data_available = TRUE; + } + l3g->i2c_trans.status = I2CTransDone; + } + } + else if (l3g->init_status != L3G_CONF_UNINIT) { // Configuring but not yet initialized + if (l3g->i2c_trans.status == I2CTransSuccess || l3g->i2c_trans.status == I2CTransDone) { + l3g->i2c_trans.status = I2CTransDone; + l3g4200_send_config(l3g); + } + if (l3g->i2c_trans.status == I2CTransFailed) { + l3g->init_status--; + l3g->i2c_trans.status = I2CTransDone; + l3g4200_send_config(l3g); // Retry config (TODO max retry) + } + } +} diff --git a/sw/airborne/peripherals/l3g4200.h b/sw/airborne/peripherals/l3g4200.h new file mode 100644 index 0000000000..16b572ee52 --- /dev/null +++ b/sw/airborne/peripherals/l3g4200.h @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2011 Gautier Hattenberger + * 2013 Felix Ruess + * 2013 Eduardo Lavratti + * + * 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/l3g4200.h + * + * Driver for the gyro L3G4200 From ST. + */ +#ifndef L3G4200_H +#define L3G4200_H + +#include "std.h" +#include "math/pprz_algebra_int.h" +#include "mcu_periph/i2c.h" + +/* Address and register definitions */ +#include "peripherals/l3g4200_regs.h" + + +/// Default Output rate 800hz +#define L3G4200_DEFAULT_DR L3G4200_DR_800Hz +/// Default digital lowpass filter 35hz +#define L3G4200_DEFAULT_DLPF L3G4200_DLPF_2 + + +/* Default conf */ +#define L3G4200_DEFAULT_CTRL_REG1 0x8f // 400hz ODR, 20hz filter, run! +#define L3G4200_DEFAULT_CTRL_REG5 0x02 // low pass filter enable + +struct L3g4200Config { + uint8_t ctrl_reg1; ///< + uint8_t ctrl_reg5; ///< +}; + +/** config status states */ +enum L3g4200ConfStatus { + L3G_CONF_UNINIT, + L3G_CONF_REG1, + L3G_CONF_REG5, + L3G_CONF_DONE +}; + +struct L3g4200 { + struct i2c_periph *i2c_p; + struct i2c_transaction i2c_trans; + bool_t initialized; ///< config done flag + enum L3g4200ConfStatus init_status; ///< init status + volatile bool_t data_available; ///< data ready flag + union { + struct Int32Rates rates; ///< data as angular rates in gyro coordinate system + int32_t value[3]; ///< data values accessible by channel index + } data; + struct L3g4200Config config; +}; + +// Functions +extern void l3g4200_init(struct L3g4200 *itg, struct i2c_periph *i2c_p, uint8_t i2c_address); +extern void l3g4200_set_default_config(struct L3g4200Config *conf); +extern void l3g4200_start_configure(struct L3g4200 *l3g); +extern void l3g4200_read(struct L3g4200 *l3g); +extern void l3g4200_event(struct L3g4200 *l3g); + +/// convenience function: read or start configuration if not already initialized +static inline void l3g4200_periodic(struct L3g4200 *l3g) { + if (l3g->initialized) + l3g4200_read(l3g); + else + l3g4200_start_configure(l3g); +} + +#endif // L3G4200_H diff --git a/sw/airborne/peripherals/l3g4200_regs.h b/sw/airborne/peripherals/l3g4200_regs.h new file mode 100644 index 0000000000..46f8486d20 --- /dev/null +++ b/sw/airborne/peripherals/l3g4200_regs.h @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2013 Eduardo Lavratti + * + * 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/l3g4200_regs.h + * Register defs for L3G4200 gyros. + */ + +#ifndef L3G4200_REGS_H +#define L3G4200_REGS_H + +/* default I2C address */ +#define L3G4200_ADDR 0xD2 +#define L3G4200_ADDR_ALT 0xD0 + +/* Registers */ +#define L3G4200_WHO_AM_I 0x0F + +#define L3G4200_REG_CTRL_REG1 0x20 +#define L3G4200_REG_CTRL_REG2 0x21 +#define L3G4200_REG_CTRL_REG3 0x22 +#define L3G4200_REG_CTRL_REG4 0x23 +#define L3G4200_REG_CTRL_REG5 0x24 +#define L3G4200_REG_REFERENCE 0x25 +#define L3G4200_REG_OUT_TEMP 0x26 +#define L3G4200_REG_STATUS_REG 0x27 + +#define L3G4200_REG_OUT_X_L 0x28 +#define L3G4200_REG_OUT_X_H 0x29 +#define L3G4200_REG_OUT_Y_L 0x2A +#define L3G4200_REG_OUT_Y_H 0x2B +#define L3G4200_REG_OUT_Z_L 0x2C +#define L3G4200_REG_OUT_Z_H 0x2D + +#define L3G4200_REG_FIFO_CTRL_REG 0x2E +#define L3G4200_REG_FIFO_SRC_REG 0x2F + +#define L3G4200_REG_INT1_CFG 0x30 +#define L3G4200_REG_INT1_SRC 0x31 +#define L3G4200_REG_INT1_THS_XH 0x32 +#define L3G4200_REG_INT1_THS_XL 0x33 +#define L3G4200_REG_INT1_THS_YH 0x34 +#define L3G4200_REG_INT1_THS_YL 0x35 +#define L3G4200_REG_INT1_THS_ZH 0x36 +#define L3G4200_REG_INT1_THS_ZL 0x37 +#define L3G4200_REG_INT1_DURATION 0x38 + +/** Output Data Rate Options */ +enum L3g4200_DR { + L3G4200_DR_100Hz = 0x0, + L3G4200_DR_200Hz = 0x1, + L3G4200_DR_400Hz = 0x2, + L3G4200_DR_800Hz = 0x3 +}; +/** Digital Low Pass Filter Options */ +enum L3g4200_DLPF { + L3G4200_DLPF_1 = 0x0, + L3G4200_DLPF_2 = 0x1, + L3G4200_DLPF_3 = 0x2, + L3G4200_DLPF_4 = 0x3 +}; + +#endif /* L3G4200_REGS_H */