diff --git a/flight/Modules/Sensors/sensors.c b/flight/Modules/Sensors/sensors.c index 57aad6620..425b73b44 100644 --- a/flight/Modules/Sensors/sensors.c +++ b/flight/Modules/Sensors/sensors.c @@ -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) { diff --git a/flight/PiOS/STM32F4xx/pios_hmc5883.c b/flight/PiOS/STM32F4xx/pios_hmc5883.c index 088026240..6922dfe9e 100644 --- a/flight/PiOS/STM32F4xx/pios_hmc5883.c +++ b/flight/PiOS/STM32F4xx/pios_hmc5883.c @@ -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 } diff --git a/flight/PiOS/STM32F4xx/pios_l3gd20.c b/flight/PiOS/STM32F4xx/pios_l3gd20.c new file mode 100644 index 000000000..c4df919e7 --- /dev/null +++ b/flight/PiOS/STM32F4xx/pios_l3gd20.c @@ -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 */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_l3gd20.h b/flight/PiOS/inc/pios_l3gd20.h new file mode 100644 index 000000000..2db1399e0 --- /dev/null +++ b/flight/PiOS/inc/pios_l3gd20.h @@ -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 */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h index 98e0b85d8..bc06f73ca 100644 --- a/flight/PiOS/pios.h +++ b/flight/PiOS/pios.h @@ -125,6 +125,9 @@ #if defined(PIOS_INCLUDE_MPU6000) #include #endif +#if defined(PIOS_INCLUDE_L3GD20) +#include +#endif #if defined(PIOS_INCLUDE_MS5611) #include #endif diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 4f94b3821..4aba28543 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -8,6 +8,9 @@ /* Begin PBXFileReference section */ 65003B31121249CA00C183DD /* pios_wdg.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_wdg.c; sourceTree = ""; }; + 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 = ""; }; + 650387E514B52B860045AFE4 /* pios_l3gd20.h */ = {isa = PBXFileReference; fileEncoding = 4; path = pios_l3gd20.h; sourceTree = ""; }; 65078B09136FCEE600536549 /* flightstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = flightstatus.xml; sourceTree = ""; }; 650D8E2112DFE16400D05CC9 /* actuator.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = actuator.c; sourceTree = ""; }; 650D8E2312DFE16400D05CC9 /* actuator.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = actuator.h; sourceTree = ""; }; @@ -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 */, diff --git a/flight/Revolution/System/inc/pios_config.h b/flight/Revolution/System/inc/pios_config.h index 87b4e0cc9..ff870f8cc 100644 --- a/flight/Revolution/System/inc/pios_config.h +++ b/flight/Revolution/System/inc/pios_config.h @@ -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 diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c index 517417ea4..e9ce2ec16 100644 --- a/flight/Revolution/System/pios_board.c +++ b/flight/Revolution/System/pios_board.c @@ -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);