diff --git a/flight/PiOS/STM32F10x/pios_l3gd20.c b/flight/PiOS/STM32F10x/pios_l3gd20.c new file mode 100644 index 000000000..3913eeaa8 --- /dev/null +++ b/flight/PiOS/STM32F10x/pios_l3gd20.c @@ -0,0 +1,342 @@ +/** + ****************************************************************************** + * @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_16); + + /* 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); + + // An initial read is needed to get it running + struct pios_l3gd20_data data; + PIOS_L3GD20_ReadGyros(&data); + +} + +/** + * @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) +{ + // This register enables the channels and sets the bandwidth + PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG1, PIOS_L3GD20_CTRL1_FASTEST | + PIOS_L3GD20_CTRL1_PD | PIOS_L3GD20_CTRL1_ZEN | + PIOS_L3GD20_CTRL1_YEN | PIOS_L3GD20_CTRL1_XEN); + + // Disable the high pass filters + PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG2, 0); + + // Set int2 to go high on data ready + PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG3, 0x08); + + // Select SPI interface, 500 deg/s, endianness? + PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG4, 0x10); + + // Enable FIFO, disable HPF + PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG5, 0x40); + + // Fifo stream mode + PIOS_L3GD20_SetReg(PIOS_L3GD20_FIFO_CTRL_REG, 0x40); +} + +/** + * @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 + */ +uint32_t l3gd20_irq = 0; +int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data * data) +{ + + uint8_t buf[7] = {PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 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) { + PIOS_L3GD20_ReleaseBus(); + data->gyro_x = 0; + data->gyro_y = 0; + data->gyro_z = 0; + data->temperature = 0; + return -2; + } + + PIOS_L3GD20_ReleaseBus(); + + memcpy((uint8_t *) &(data->gyro_x), &rec[1], 6); + data->temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP); + + 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() +{ + return 0.01750f; + switch (cfg->gyro_range) { + case PIOS_L3GD20_SCALE_250_DEG: + return 0.00875f; + case PIOS_L3GD20_SCALE_500_DEG: + return 0.01750f; + case PIOS_L3GD20_SCALE_2000_DEG: + return 0.070f; + } + 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) +{ + int32_t l3gd20_id = PIOS_L3GD20_ReadID(); + if(l3gd20_id < 0) + return -1; + + uint8_t id = l3gd20_id; + if(id == 0xD4) + return 0; + + return -2; +} + +/** + * @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];*/ + return 0; +} + +/** +* @brief IRQ Handler. Read all the data from onboard buffer +*/ +int32_t l3gd20_count; +uint32_t l3gd20_fifo_full = 0; + +void PIOS_L3GD20_IRQHandler(void) +{ + struct pios_l3gd20_data data; + PIOS_L3GD20_ReadGyros(&data); + + data.temperature = l3gd20_irq; + + if(fifoBuf_getFree(&pios_l3gd20_fifo) < sizeof(data)) { + l3gd20_fifo_full++; + return; + } + + fifoBuf_putData(&pios_l3gd20_fifo, (uint8_t *) &data, sizeof(data)); + + l3gd20_irq++; + +} + +#endif /* L3GD20 */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_l3gd20.h b/flight/PiOS/inc/pios_l3gd20.h new file mode 100644 index 000000000..090c325e7 --- /dev/null +++ b/flight/PiOS/inc/pios_l3gd20.h @@ -0,0 +1,150 @@ +/** + ****************************************************************************** + * @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" + +/* L3GD20 Addresses */ +#define PIOS_L3GD20_WHOAMI 0x0F +#define PIOS_L3GD20_CTRL_REG1 0X20 +#define PIOS_L3GD20_CTRL_REG2 0X21 +#define PIOS_L3GD20_CTRL_REG3 0X22 +#define PIOS_L3GD20_CTRL_REG4 0X23 +#define PIOS_L3GD20_CTRL_REG5 0X24 +#define PIOS_L3GD20_REFERENCE 0X25 +#define PIOS_L3GD20_OUT_TEMP 0x26 +#define PIOS_L3GD20_STATUS_REG 0x27 +#define PIOS_L3GD20_GYRO_X_OUT_LSB 0x28 +#define PIOS_L3GD20_GYRO_X_OUT_MSB 0x29 +#define PIOS_L3GD20_GYRO_Y_OUT_LSB 0x2A +#define PIOS_L3GD20_GYRO_Y_OUT_MSB 0x2B +#define PIOS_L3GD20_GYRO_Z_OUT_LSB 0x2C +#define PIOS_L3GD20_GYRO_Z_OUT_MSB 0x2D +#define PIOS_L3GD20_FIFO_CTRL_REG 0x2E +#define PIOS_L3GD20_FIFO_SRC_REG 0x2F +#define PIOS_L3GD20_INT1_CFG 0x30 +#define PIOS_L3GD20_INT1_SRC 0x31 +#define PIOS_L3GD20_INT1_TSH_XH 0x32 +#define PIOS_L3GD20_INT1_TSH_XL 0x33 +#define PIOS_L3GD20_INT1_TSH_YH 0x34 +#define PIOS_L3GD20_INT1_TSH_YL 0x35 +#define PIOS_L3GD20_INT1_TSH_ZH 0x36 +#define PIOS_L3GD20_INT1_TSH_ZL 0x37 +#define PIOS_L3GD20_INT1_DURATION 0x38 + +/* Ctrl1 flags */ +#define PIOS_L3GD20_CTRL1_FASTEST 0xF0 +#define PIOS_L3GD20_CTRL1_PD 0x08 +#define PIOS_L3GD20_CTRL1_ZEN 0x04 +#define PIOS_L3GD20_CTRL1_YEN 0x02 +#define PIOS_L3GD20_CTRL1_XEN 0x01 + +/* 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_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; + enum pios_l3gd20_range gyro_range; +}; + +/* 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/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index ed4340a11..44dc969ed 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -2933,6 +2933,8 @@ 65E8F0EE11EFF25C00BBF654 /* startup_stm32f10x_HD.S */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.asm; name = startup_stm32f10x_HD.S; path = ../../PiOS/STM32F10x/startup_stm32f10x_HD.S; sourceTree = SOURCE_ROOT; }; 65E8F0F111EFF25C00BBF654 /* startup_stm32f10x_MD.S */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.asm; name = startup_stm32f10x_MD.S; path = ../../PiOS/STM32F10x/startup_stm32f10x_MD.S; sourceTree = SOURCE_ROOT; }; 65EA2E171273C55200636061 /* ratedesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ratedesired.xml; sourceTree = ""; }; + 65F5FB9914C9FAC500261DE0 /* pios_l3gd20.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_l3gd20.h; sourceTree = ""; }; + 65F5FB9A14C9FB2C00261DE0 /* pios_l3gd20.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_l3gd20.c; sourceTree = ""; }; 65F93C3912EE09280047DB36 /* aes.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = aes.c; sourceTree = ""; }; 65F93C3B12EE09280047DB36 /* aes.lst */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; path = aes.lst; sourceTree = ""; }; 65F93C3C12EE09280047DB36 /* aes.o */ = {isa = PBXFileReference; lastKnownFileType = "compiled.mach-o.objfile"; path = aes.o; sourceTree = ""; }; @@ -7726,6 +7728,7 @@ 65E8F04311EFF25C00BBF654 /* pios_i2c.h */, 6526645A122DF972006F9A3C /* pios_i2c_priv.h */, 65E8F04411EFF25C00BBF654 /* pios_irq.h */, + 65F5FB9914C9FAC500261DE0 /* pios_l3gd20.h */, 65E8F04511EFF25C00BBF654 /* pios_led.h */, 65E8F04611EFF25C00BBF654 /* pios_opahrs.h */, 65E8F04711EFF25C00BBF654 /* pios_opahrs_proto.h */, @@ -7778,6 +7781,7 @@ 65E8F0E011EFF25C00BBF654 /* pios_gpio.c */, 65E8F0E111EFF25C00BBF654 /* pios_i2c.c */, 65E8F0E211EFF25C00BBF654 /* pios_irq.c */, + 65F5FB9A14C9FB2C00261DE0 /* pios_l3gd20.c */, 65E8F0E311EFF25C00BBF654 /* pios_led.c */, 65E8F0E411EFF25C00BBF654 /* pios_ppm.c */, 65E8F0E511EFF25C00BBF654 /* pios_pwm.c */,