mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-05 23:49:00 +08:00
[peripherals] add driver for L3G4200 gyro
This commit is contained in:
eduardo
Felix Ruess
@@ -0,0 +1,144 @@
|
||||
/*
|
||||
*
|
||||
* Copyright (C) 2011 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* 2013 Felix Ruess <felix.ruess@gmail.com>
|
||||
* 2013 Eduardo Lavratti <agressiva@hotmail.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/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)
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
/*
|
||||
* Copyright (C) 2011 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
* 2013 Felix Ruess <felix.ruess@gmail.com>
|
||||
* 2013 Eduardo Lavratti <agressiva@hotmail.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/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
|
||||
@@ -0,0 +1,81 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Eduardo Lavratti <agressiva@hotmail.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/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 */
|
||||
Reference in New Issue
Block a user