rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Add files for new L3DG20 sensor

Browse Source
This commit is contained in:
James Cotton 2012-01-04 19:47:57 -06:00
parent e1c21462b0
commit 0cc18e7dcf
8 changed files with 612 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
flight/Modules/Sensors/sensors.c
View File

@ -136,6 +136,11 @@ int32_t mag_test;
/**
* The sensor task. This polls the gyros at 500 Hz and pumps that data to
* stabilization and to the attitude loop
*
* This function has a lot of if/defs right now to allow these configurations:
* 1. BMA180 accel and MPU6000 gyro
* 2. MPU6000 gyro and accel
* 3. BMA180 accel and L3GD20 gyro
*/
static void SensorsTask(void *parameters)
{
@ -143,11 +148,10 @@ static void SensorsTask(void *parameters)
portTickType lastSysTime;
uint32_t accel_samples;
uint32_t gyro_samples;
struct pios_bma180_data accel;
struct pios_mpu6000_data gyro;
int32_t accel_accum[3] = {0, 0, 0};
int32_t gyro_accum[3] = {0,0,0};
float scaling;
float gyro_scaling;
float accel_scaling;
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
@ -155,7 +159,13 @@ static void SensorsTask(void *parameters)
settingsUpdatedCb(&ev);
accel_test = PIOS_BMA180_Test();
#if defined(PIOS_INCLUDE_MPU6000)
gyro_test = PIOS_MPU6000_Test();
#elif defined(PIOS_INCLUDE_L3GD20)
gyro_test = PIOS_L3GD20_Test();
#else
#error No gyro defined
#endif
mag_test = PIOS_HMC5883_Test();
if(accel_test < 0 || gyro_test < 0 || mag_test < 0) {
@ -176,19 +186,21 @@ static void SensorsTask(void *parameters)
lastSysTime = xTaskGetTickCount();
while (1) {
// TODO: add timeouts to the sensor reads and set an error if the fail
int32_t read_good;
int32_t count;
for (int i = 0; i < 3; i++) {
accel_accum[i] = 0;
gyro_accum[i] = 0;
}
accel_samples = 0;
gyro_samples = 0;
// Make sure we get one sample
#if !defined(PIOS_MPU6000_ACCEL)
struct pios_bma180_data accel;
count = 0;
while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0);
while(read_good == 0) {
@ -200,14 +212,14 @@ static void SensorsTask(void *parameters)
read_good = PIOS_BMA180_ReadFifo(&accel);
}
accel_samples = count;
accel_samples = count;
accel_scaling = PIOS_BMA180_GetScale();
#endif
#if defined(PIOS_MPU6000_ACCEL)
accel_accum[0] = accel_accum[1] = accel_accum[2] = 0;
#endif
// Using MPU6000 gyro and possibly accel
#if defined(PIOS_INCLUDE_MPU6000)
struct pios_mpu6000_data gyro;
// Make sure we get one sample
count = 0;
while((read_good = PIOS_MPU6000_ReadFifo(&gyro)) != 0);
while(read_good == 0) {
@ -221,35 +233,59 @@ static void SensorsTask(void *parameters)
accel_accum[0] += gyro.accel_x;
accel_accum[1] += gyro.accel_y;
accel_accum[2] += gyro.accel_z;
accel_samples = count;
#endif
read_good = PIOS_MPU6000_ReadFifo(&gyro);
}
gyro_samples = count;
gyro_scaling = PIOS_MPU6000_GetScale();
#if defined(PIOS_MPU6000_ACCEL)
accel_samples = count;
accel_scaling = PIOS_MPU6000_GetAccelScale();
#endif
// Using L3DG20 gyro
#elif defined(PIOS_INCLUDE_L3GD20)
struct pios_l3gd20_data gyro;
count = 0;
while((read_good = PIOS_L3GD20_ReadFifo(&gyro)) != 0);
while(read_good == 0) {
count++;
gyro_accum[0] += gyro.gyro_x;
gyro_accum[1] += gyro.gyro_y;
gyro_accum[2] += gyro.gyro_z;
read_good = PIOS_L3GD20_ReadFifo(&gyro);
}
gyro_samples = count;
gyro_scaling = PIOS_L3GD20_GetScale();
#else
#error No gyro defined
#endif
float accels[3] = {(float) accel_accum[1] / accel_samples, (float) accel_accum[0] / accel_samples, -(float) accel_accum[2] / accel_samples};
// Not the swaping of channel orders
#if defined(PIOS_MPU6000_ACCEL)
scaling = PIOS_MPU6000_GetAccelScale();
accel_scaling = PIOS_MPU6000_GetAccelScale();
#else
scaling = PIOS_BMA180_GetScale();
accel_scaling = PIOS_BMA180_GetScale();
#endif
AccelsData accelsData; // Skip get as we set all the fields
accelsData.x = accels[0] * scaling * accel_scale[0] - accel_bias[0];
accelsData.y = accels[1] * scaling * accel_scale[1] - accel_bias[1];
accelsData.z = accels[2] * scaling * accel_scale[2] - accel_bias[2];
accelsData.x = accels[0] * accel_scaling * accel_scale[0] - accel_bias[0];
accelsData.y = accels[1] * accel_scaling * accel_scale[1] - accel_bias[1];
accelsData.z = accels[2] * accel_scaling * accel_scale[2] - accel_bias[2];
accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f;
AccelsSet(&accelsData);
float gyros[3] = {(float) gyro_accum[1] / gyro_samples, (float) gyro_accum[0] / gyro_samples, -(float) gyro_accum[2] / gyro_samples};
scaling = PIOS_MPU6000_GetScale();
GyrosData gyrosData; // Skip get as we set all the fields
gyrosData.x = gyros[0] * scaling;
gyrosData.y = gyros[1] * scaling;
gyrosData.z = gyros[2] * scaling;
gyrosData.x = gyros[0] * gyro_scaling;
gyrosData.y = gyros[1] * gyro_scaling;
gyrosData.z = gyros[2] * gyro_scaling;
gyrosData.temperature = 35.0f + ((float) gyro.temperature + 512.0f) / 340.0f;
if (bias_correct_gyro) {

11
flight/PiOS/STM32F4xx/pios_hmc5883.c
View File

@ -409,17 +409,28 @@ void PIOS_HMC5883_IRQHandler(void)
* Soon this will be generic in pios_exti and the BMA180 will register
* against it. Right now this is crap!
*/
#if defined(PIOS_INCLUDE_MPU6000)
extern void PIOS_MPU6000_IRQHandler();
#elif defined(PIOS_INCLUDE_L3GD20)
extern void PIOS_L3GD20_IRQHandler();
#endif
void EXTI9_5_IRQHandler(void)
{
if (EXTI_GetITStatus(dev_cfg->eoc_exti.init.EXTI_Line) != RESET) {
PIOS_HMC5883_IRQHandler();
EXTI_ClearITPendingBit(dev_cfg->eoc_exti.init.EXTI_Line);
}
#if defined(PIOS_INCLUDE_MPU6000)
if (EXTI_GetITStatus(EXTI_Line8) != RESET) {
PIOS_MPU6000_IRQHandler();
EXTI_ClearITPendingBit(EXTI_Line8);
}
#elif defined(PIOS_INCLUDE_L3GD20)
if (EXTI_GetITStatus(EXTI_Line8) != RESET) {
PIOS_L3GD20_IRQHandler();
EXTI_ClearITPendingBit(EXTI_Line8);
}
#endif
}

373
flight/PiOS/STM32F4xx/pios_l3gd20.c Normal file
View File

@ -0,0 +1,373 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_L3GD20 L3GD20 Functions
* @brief Deals with the hardware interface to the 3-axis gyro
* @{
*
* @file pios_l3gd20.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief L3GD20 3-axis gyro chip
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************
*/
/*
* This program 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 3 of the License, or
* (at your option) any later version.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
/* Project Includes */
#include "pios.h"
#if defined(PIOS_INCLUDE_L3GD20)
/* Global Variables */
uint32_t pios_spi_gyro;
/* Local Variables */
#define DEG_TO_RAD (M_PI / 180.0)
static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const * cfg);
static int32_t PIOS_L3GD20_SetReg(uint8_t address, uint8_t buffer);
static int32_t PIOS_L3GD20_GetReg(uint8_t address);
#define PIOS_L3GD20_MAX_DOWNSAMPLE 100
static int16_t pios_l3gd20_buffer[PIOS_L3GD20_MAX_DOWNSAMPLE * sizeof(struct pios_l3gd20_data)];
static t_fifo_buffer pios_l3gd20_fifo;
volatile bool l3gd20_configured = false;
static struct pios_l3gd20_cfg const * cfg;
#define GRAV 9.81f
/**
* @brief Initialize the MPU6050 3-axis gyro sensor.
* @return none
*/
void PIOS_L3GD20_Init(const struct pios_l3gd20_cfg * new_cfg)
{
cfg = new_cfg;
fifoBuf_init(&pios_l3gd20_fifo, (uint8_t *) pios_l3gd20_buffer, sizeof(pios_l3gd20_buffer));
/* Configure the MPU6050 Sensor */
PIOS_SPI_SetPrescalar(pios_spi_gyro, SPI_BaudRatePrescaler_256);
PIOS_L3GD20_Config(cfg);
PIOS_SPI_SetPrescalar(pios_spi_gyro, SPI_BaudRatePrescaler_8);
/* Configure EOC pin as input floating */
GPIO_Init(cfg->drdy.gpio, &cfg->drdy.init);
/* Configure the End Of Conversion (EOC) interrupt */
SYSCFG_EXTILineConfig(cfg->eoc_exti.port_source, cfg->eoc_exti.pin_source);
EXTI_Init(&cfg->eoc_exti.init);
/* Enable and set EOC EXTI Interrupt to the lowest priority */
NVIC_Init(&cfg->eoc_irq.init);
}
/**
* @brief Initialize the L3GD20 3-axis gyro sensor
* \return none
* \param[in] PIOS_L3GD20_ConfigTypeDef struct to be used to configure sensor.
*
*/
static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const * cfg)
{
}
/**
* @brief Claim the SPI bus for the accel communications and select this chip
* @return 0 if successful, -1 if unable to claim bus
*/
int32_t PIOS_L3GD20_ClaimBus()
{
if(PIOS_SPI_ClaimBus(pios_spi_gyro) != 0)
return -1;
PIOS_SPI_RC_PinSet(pios_spi_gyro,0,0);
return 0;
}
/**
* @brief Release the SPI bus for the accel communications and end the transaction
* @return 0 if successful
*/
int32_t PIOS_L3GD20_ReleaseBus()
{
PIOS_SPI_RC_PinSet(pios_spi_gyro,0,1);
return PIOS_SPI_ReleaseBus(pios_spi_gyro);
}
/**
* @brief Connect to the correct SPI bus
*/
void PIOS_L3GD20_Attach(uint32_t spi_id)
{
pios_spi_gyro = spi_id;
}
/**
* @brief Read a register from L3GD20
* @returns The register value or -1 if failure to get bus
* @param reg[in] Register address to be read
*/
static int32_t PIOS_L3GD20_GetReg(uint8_t reg)
{
uint8_t data;
if(PIOS_L3GD20_ClaimBus() != 0)
return -1;
PIOS_SPI_TransferByte(pios_spi_gyro,(0x80 | reg) ); // request byte
data = PIOS_SPI_TransferByte(pios_spi_gyro,0 ); // receive response
PIOS_L3GD20_ReleaseBus();
return data;
}
/**
* @brief Writes one byte to the L3GD20
* \param[in] reg Register address
* \param[in] data Byte to write
* \return 0 if operation was successful
* \return -1 if unable to claim SPI bus
* \return -2 if unable to claim i2c device
*/
static int32_t PIOS_L3GD20_SetReg(uint8_t reg, uint8_t data)
{
if(PIOS_L3GD20_ClaimBus() != 0)
return -1;
if(PIOS_SPI_TransferByte(pios_spi_gyro, 0x7f & reg) != 0) {
PIOS_L3GD20_ReleaseBus();
return -2;
}
if(PIOS_SPI_TransferByte(pios_spi_gyro, data) != 0) {
PIOS_L3GD20_ReleaseBus();
return -3;
}
PIOS_L3GD20_ReleaseBus();
return 0;
}
/**
* @brief Read current X, Z, Y values (in that order)
* \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
* \returns The number of samples remaining in the fifo
*/
int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data * data)
{
uint8_t buf[7] = {PIOS_L3GD20_GYRO_X_OUT_MSB | 0x80, 0, 0, 0, 0, 0, 0};
uint8_t rec[7];
if(PIOS_L3GD20_ClaimBus() != 0)
return -1;
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &buf[0], &rec[0], sizeof(buf), NULL) < 0)
return -2;
PIOS_L3GD20_ReleaseBus();
data->gyro_x = rec[1] << 8 | rec[2];
data->gyro_y = rec[3] << 8 | rec[4];
data->gyro_z = rec[5] << 8 | rec[6];
return 0;
}
/**
* @brief Read the identification bytes from the MPU6050 sensor
* \return ID read from MPU6050 or -1 if failure
*/
int32_t PIOS_L3GD20_ReadID()
{
int32_t l3gd20_id = PIOS_L3GD20_GetReg(PIOS_L3GD20_WHOAMI);
if(l3gd20_id < 0)
return -1;
return l3gd20_id;
}
/**
* \brief Reads the data from the MPU6050 FIFO
* \param[out] buffer destination buffer
* \param[in] len maximum number of bytes which should be read
* \note This returns the data as X, Y, Z the temperature
* \return number of bytes transferred if operation was successful
* \return -1 if error during I2C transfer
*/
int32_t PIOS_L3GD20_ReadFifo(struct pios_l3gd20_data * buffer)
{
if(fifoBuf_getUsed(&pios_l3gd20_fifo) < sizeof(*buffer))
return -1;
fifoBuf_getData(&pios_l3gd20_fifo, (uint8_t *) buffer, sizeof(*buffer));
return 0;
}
float PIOS_L3GD20_GetScale()
{
switch (cfg->gyro_range) {
case PIOS_L3GD20_SCALE_250_DEG:
return 1.0f / 131.0f;
case PIOS_L3GD20_SCALE_500_DEG:
return 1.0f / 65.5f;
case PIOS_L3GD20_SCALE_1000_DEG:
return 1.0f / 32.8f;
case PIOS_L3GD20_SCALE_2000_DEG:
return 1.0f / 16.4f;
}
return 0;
}
/**
* @brief Run self-test operation.
* \return 0 if test succeeded
* \return non-zero value if test succeeded
*/
uint8_t PIOS_L3GD20_Test(void)
{
/* Verify that ID matches (MPU6050 ID is 0x69) */
int32_t L3GD20_id = PIOS_L3GD20_ReadID();
if(L3GD20_id < 0)
return -1;
if(L3GD20_id != 0x68);
return -2;
return 0;
}
/**
* @brief Run self-test operation.
* \return 0 if test succeeded
* \return non-zero value if test succeeded
*/
static int32_t PIOS_L3GD20_FifoDepth(void)
{
uint8_t L3GD20_send_buf[3] = {PIOS_L3GD20_FIFO_CNT_MSB | 0x80, 0, 0};
uint8_t L3GD20_rec_buf[3];
if(PIOS_L3GD20_ClaimBus() != 0)
return -1;
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &L3GD20_send_buf[0], &L3GD20_rec_buf[0], sizeof(L3GD20_send_buf), NULL) < 0) {
PIOS_L3GD20_ReleaseBus();
return -1;
}
PIOS_L3GD20_ReleaseBus();
return (L3GD20_rec_buf[1] << 8) | L3GD20_rec_buf[2];
}
/**
* @brief IRQ Handler. Read all the data from onboard buffer
*/
uint32_t l3gd20_irq = 0;
int32_t l3gd20_count;
uint32_t l3gd20_fifo_full = 0;
uint8_t l3gd20_last_read_count = 0;
uint32_t l3gd20_fails = 0;
uint32_t l3gd20_interval_us;
uint32_t l3gd20_time_us;
uint32_t l3gd20_transfer_size;
void PIOS_L3GD20_IRQHandler(void)
{
static uint32_t timeval;
l3gd20_interval_us = PIOS_DELAY_DiffuS(timeval);
timeval = PIOS_DELAY_GetRaw();
if(!l3gd20_configured)
return;
l3gd20_count = PIOS_L3GD20_FifoDepth();
if(l3gd20_count < sizeof(struct pios_l3gd20_data))
return;
if(PIOS_L3GD20_ClaimBus() != 0)
return;
uint8_t l3gd20_send_buf[1+sizeof(struct pios_l3gd20_data)] = {PIOS_L3GD20_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0};
uint8_t l3gd20_rec_buf[1+sizeof(struct pios_l3gd20_data)];
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &l3gd20_send_buf[0], &l3gd20_rec_buf[0], sizeof(l3gd20_send_buf), NULL) < 0) {
PIOS_L3GD20_ReleaseBus();
l3gd20_fails++;
return;
}
PIOS_L3GD20_ReleaseBus();
struct pios_l3gd20_data data;
if(fifoBuf_getFree(&pios_l3gd20_fifo) < sizeof(data)) {
l3gd20_fifo_full++;
return;
}
// In the case where extras samples backed up grabbed an extra
if (l3gd20_count >= (sizeof(data) * 2)) {
if(PIOS_L3GD20_ClaimBus() != 0)
return;
uint8_t l3gd20_send_buf[1+sizeof(struct pios_l3gd20_data)] = {PIOS_L3GD20_FIFO_REG | 0x80, 0, 0, 0, 0, 0, 0, 0, 0};
uint8_t l3gd20_rec_buf[1+sizeof(struct pios_l3gd20_data)];
if(PIOS_SPI_TransferBlock(pios_spi_gyro, &l3gd20_send_buf[0], &l3gd20_rec_buf[0], sizeof(l3gd20_send_buf), NULL) < 0) {
PIOS_L3GD20_ReleaseBus();
l3gd20_fails++;
return;
}
PIOS_L3GD20_ReleaseBus();
struct pios_l3gd20_data data;
if(fifoBuf_getFree(&pios_l3gd20_fifo) < sizeof(data)) {
l3gd20_fifo_full++;
return;
}
}
data.temperature = l3gd20_rec_buf[1] << 8 | l3gd20_rec_buf[2];
data.gyro_x = l3gd20_rec_buf[3] << 8 | l3gd20_rec_buf[4];
data.gyro_y = l3gd20_rec_buf[5] << 8 | l3gd20_rec_buf[6];
data.gyro_z = l3gd20_rec_buf[7] << 8 | l3gd20_rec_buf[8];
fifoBuf_putData(&pios_l3gd20_fifo, (uint8_t *) &data, sizeof(data));
l3gd20_irq++;
l3gd20_time_us = PIOS_DELAY_DiffuS(timeval);
}
#endif /* L3GD20 */
/**
* @}
* @}
*/

154
flight/PiOS/inc/pios_l3gd20.h Normal file
View File

@ -0,0 +1,154 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_L3GD20 L3GD20 Functions
* @brief Deals with the hardware interface to the 3-axis gyro
* @{
*
* @file PIOS_L3GD20.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief L3GD20 3-axis gyor function headers
* @see The GNU Public License (GPL) Version 3
*
******************************************************************************
*/
/*
* This program 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 3 of the License, or
* (at your option) any later version.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_L3GD20_H
#define PIOS_L3GD20_H
#include "pios.h"
/* MPU6050 Addresses */
#define PIOS_L3GD20_SMPLRT_DIV_REG 0X19
#define PIOS_L3GD20_DLPF_CFG_REG 0X1A
#define PIOS_L3GD20_GYRO_CFG_REG 0X1B
#define PIOS_L3GD20_ACCEL_CFG_REG 0X1C
#define PIOS_L3GD20_FIFO_EN_REG 0x23
#define PIOS_L3GD20_INT_CFG_REG 0x37
#define PIOS_L3GD20_INT_EN_REG 0x38
#define PIOS_L3GD20_INT_STATUS_REG 0x3A
#define PIOS_L3GD20_ACCEL_X_OUT_MSB 0x3B
#define PIOS_L3GD20_ACCEL_X_OUT_LSB 0x3C
#define PIOS_L3GD20_ACCEL_Y_OUT_MSB 0x3D
#define PIOS_L3GD20_ACCEL_Y_OUT_LSB 0x3E
#define PIOS_L3GD20_ACCEL_Z_OUT_MSB 0x3F
#define PIOS_L3GD20_ACCEL_Z_OUT_LSB 0x40
#define PIOS_L3GD20_TEMP_OUT_MSB 0x41
#define PIOS_L3GD20_TEMP_OUT_LSB 0x42
#define PIOS_L3GD20_GYRO_X_OUT_MSB 0x43
#define PIOS_L3GD20_GYRO_X_OUT_LSB 0x44
#define PIOS_L3GD20_GYRO_Y_OUT_MSB 0x45
#define PIOS_L3GD20_GYRO_Y_OUT_LSB 0x46
#define PIOS_L3GD20_GYRO_Z_OUT_MSB 0x47
#define PIOS_L3GD20_GYRO_Z_OUT_LSB 0x48
#define PIOS_L3GD20_USER_CTRL_REG 0x6A
#define PIOS_L3GD20_PWR_MGMT_REG 0x6B
#define PIOS_L3GD20_FIFO_CNT_MSB 0x72
#define PIOS_L3GD20_FIFO_CNT_LSB 0x73
#define PIOS_L3GD20_FIFO_REG 0x74
#define PIOS_L3GD20_WHOAMI 0x75
/* FIFO enable for storing different values */
#define PIOS_L3GD20_FIFO_TEMP_OUT 0x80
#define PIOS_L3GD20_FIFO_GYRO_X_OUT 0x40
#define PIOS_L3GD20_FIFO_GYRO_Y_OUT 0x20
#define PIOS_L3GD20_FIFO_GYRO_Z_OUT 0x10
#define PIOS_L3GD20_ACCEL_OUT 0x08
/* Interrupt Configuration */
#define PIOS_L3GD20_INT_ACTL 0x80
#define PIOS_L3GD20_INT_OPEN 0x40
#define PIOS_L3GD20_INT_LATCH_EN 0x20
#define PIOS_L3GD20_INT_CLR_ANYRD 0x10
#define PIOS_L3GD20_INTEN_OVERFLOW 0x10
#define PIOS_L3GD20_INTEN_DATA_RDY 0x01
/* Interrupt status */
#define PIOS_L3GD20_INT_STATUS_FIFO_FULL 0x80
#define PIOS_L3GD20_INT_STATUS_IMU_RDY 0X04
#define PIOS_L3GD20_INT_STATUS_DATA_RDY 0X01
/* User control functionality */
#define PIOS_L3GD20_USERCTL_FIFO_EN 0X40
#define PIOS_L3GD20_USERCTL_FIFO_RST 0X02
#define PIOS_L3GD20_USERCTL_GYRO_RST 0X01
/* Power management and clock selection */
#define PIOS_L3GD20_PWRMGMT_IMU_RST 0X80
#define PIOS_L3GD20_PWRMGMT_INTERN_CLK 0X00
#define PIOS_L3GD20_PWRMGMT_PLL_X_CLK 0X01
#define PIOS_L3GD20_PWRMGMT_PLL_Y_CLK 0X02
#define PIOS_L3GD20_PWRMGMT_PLL_Z_CLK 0X03
#define PIOS_L3GD20_PWRMGMT_STOP_CLK 0X07
enum pios_l3gd20_range {
PIOS_L3GD20_SCALE_250_DEG = 0x00,
PIOS_L3GD20_SCALE_500_DEG = 0x08,
PIOS_L3GD20_SCALE_1000_DEG = 0x10,
PIOS_L3GD20_SCALE_2000_DEG = 0x18
};
enum pios_l3gd20_filter {
PIOS_L3GD20_LOWPASS_256_HZ = 0x00,
PIOS_L3GD20_LOWPASS_188_HZ = 0x01,
PIOS_L3GD20_LOWPASS_98_HZ = 0x02,
PIOS_L3GD20_LOWPASS_42_HZ = 0x03,
PIOS_L3GD20_LOWPASS_20_HZ = 0x04,
PIOS_L3GD20_LOWPASS_10_HZ = 0x05,
PIOS_L3GD20_LOWPASS_5_HZ = 0x06
};
struct pios_l3gd20_data {
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
int16_t temperature;
};
struct pios_l3gd20_cfg {
struct stm32_gpio drdy;
struct stm32_exti eoc_exti;
struct stm32_irq eoc_irq;
uint8_t Fifo_store; /* FIFO storage of different readings (See datasheet page 31 for more details) */
uint8_t Smpl_rate_div; /* Sample rate divider to use (See datasheet page 32 for more details) */
uint8_t interrupt_cfg; /* Interrupt configuration (See datasheet page 35 for more details) */
uint8_t interrupt_en; /* Interrupt configuration (See datasheet page 35 for more details) */
uint8_t User_ctl; /* User control settings (See datasheet page 41 for more details) */
uint8_t Pwr_mgmt_clk; /* Power management and clock selection (See datasheet page 32 for more details) */
enum pios_l3gd20_range gyro_range;
enum pios_l3gd20_filter filter;
};
/* Public Functions */
extern void PIOS_L3GD20_Init(const struct pios_l3gd20_cfg * cfg);
extern void PIOS_L3GD20_Attach(uint32_t spi_id);
extern int32_t PIOS_L3GD20_ReadFifo(struct pios_l3gd20_data * buffer);
extern int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data * buffer);
extern int32_t PIOS_L3GD20_ReadID();
extern uint8_t PIOS_L3GD20_Test();
extern float PIOS_L3GD20_GetScale();
#endif /* PIOS_L3GD20_H */
/**
* @}
* @}
*/

3
flight/PiOS/pios.h
View File

@ -125,6 +125,9 @@
#if defined(PIOS_INCLUDE_MPU6000)
#include <pios_mpu6000.h>
#endif
#if defined(PIOS_INCLUDE_L3GD20)
#include <pios_l3gd20.h>
#endif
#if defined(PIOS_INCLUDE_MS5611)
#include <pios_ms5611.h>
#endif

5
flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
View File

@ -8,6 +8,9 @@
/* Begin PBXFileReference section */
65003B31121249CA00C183DD /* pios_wdg.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_wdg.c; sourceTree = "<group>"; };
6502584212CA4D2600583CDF /* insgps13state.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps13state.c; path = ../../AHRS/insgps13state.c; sourceTree = SOURCE_ROOT; };
650387E414B52B680045AFE4 /* pios_l3gd20.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_l3gd20.c; sourceTree = "<group>"; };
650387E514B52B860045AFE4 /* pios_l3gd20.h */ = {isa = PBXFileReference; fileEncoding = 4; path = pios_l3gd20.h; sourceTree = "<group>"; };
65078B09136FCEE600536549 /* flightstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = flightstatus.xml; sourceTree = "<group>"; };
650D8E2112DFE16400D05CC9 /* actuator.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = actuator.c; sourceTree = "<group>"; };
650D8E2312DFE16400D05CC9 /* actuator.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = actuator.h; sourceTree = "<group>"; };
@ -3874,6 +3877,7 @@
65904ECD14613B6100FD9482 /* pios_i2c.c */,
65904ECE14613B6100FD9482 /* pios_iap.c */,
65904ED014613B6100FD9482 /* pios_irq.c */,
650387E414B52B680045AFE4 /* pios_l3gd20.c */,
65904ED114613B6100FD9482 /* pios_led.c */,
6534B5581474F7B1003DF47C /* pios_mpu6000.c */,
65FAB9071481F9F6000FF8B2 /* pios_ms5611.c */,
@ -8455,6 +8459,7 @@
6526645A122DF972006F9A3C /* pios_i2c_priv.h */,
65FA9B8014709E9E0019A260 /* pios_initcall.h */,
65E8F04411EFF25C00BBF654 /* pios_irq.h */,
650387E514B52B860045AFE4 /* pios_l3gd20.h */,
65E8F04511EFF25C00BBF654 /* pios_led.h */,
6534B55B1476D3A8003DF47C /* pios_ms5611.h */,
6534B5571474F78B003DF47C /* pios_mpu6000.h */,

3
flight/Revolution/System/inc/pios_config.h
View File

@ -57,8 +57,9 @@
/* Select the sensors to include */
#define PIOS_INCLUDE_BMA180
#define PIOS_INCLUDE_HMC5883
#define PIOS_INCLUDE_MPU6000
//#define PIOS_INCLUDE_MPU6000
//#define PIOS_MPU6000_ACCEL
#define PIOS_INCLUDE_L3GD20
#define PIOS_INCLUDE_MS5611
//#define PIOS_INCLUDE_HCSR04

7
flight/Revolution/System/pios_board.c
View File

@ -1671,8 +1671,15 @@ void PIOS_Board_Init(void) {
PIOS_DELAY_WaitmS(500);
#if defined(PIOS_INCLUDE_MPU6000)
PIOS_MPU6000_Attach(pios_spi_gyro_id);
PIOS_MPU6000_Init(&pios_mpu6000_cfg);
#elif defined(PIOS_INCLUDE_L3GD20)
PIOS_L3GD20_Attach(pios_spi_gyro_id);
// PIOS_L3GD20_Init(&pios_l3gd20_cfg);
#else
PIOS_Assert(0);
#endif
PIOS_BMA180_Attach(pios_spi_accel_id);
PIOS_BMA180_Init(&pios_bma180_cfg);