mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-05 23:49:00 +08:00
[peripherals] l3gd20 gyro sensor added
This commit is contained in:
memeruiz
Felix Ruess
@@ -0,0 +1,58 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* 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 */
|
||||
@@ -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 */
|
||||
@@ -0,0 +1,172 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* 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
|
||||
Reference in New Issue
Block a user