flight/pios/common/pios_l3gd20.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup PIOS_L3GD20 L3GD20 Functions
   6  * @brief Deals with the hardware interface to the 3-axis gyro
   7  * @{
   8  *
   9  * @file       pios_l3gd20.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  11  * @brief      L3GD20 3-axis gyro chip
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  ******************************************************************************
  15  */
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 #include "pios.h"
  33 #include <pios_l3gd20.h>
  34 #ifdef PIOS_INCLUDE_L3GD20
  35
  36 #include "fifo_buffer.h"
  37
  38 /* Global Variables */
  39 enum pios_l3gd20_dev_magic {
  40     PIOS_L3GD20_DEV_MAGIC = 0x9d39bced,
  41 };
  42
  43 #define PIOS_L3GD20_MAX_DOWNSAMPLE 2
  44 struct l3gd20_dev {
  45     uint32_t     spi_id;
  46     uint32_t     slave_num;
  47     xQueueHandle queue;
  48     const struct pios_l3gd20_cfg *cfg;
  49     enum pios_l3gd20_filter bandwidth;
  50     enum pios_l3gd20_range range;
  51     enum pios_l3gd20_dev_magic   magic;
  52 };
  53
  54 // ! Global structure for this device device
  55 static struct l3gd20_dev *dev;
  56
  57 // ! Private functions
  58 static struct l3gd20_dev *PIOS_L3GD20_alloc(void);
  59 static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev *dev);
  60 static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const *cfg);
  61 static int32_t PIOS_L3GD20_SetReg(uint8_t address, uint8_t buffer);
  62 static int32_t PIOS_L3GD20_GetReg(uint8_t address);
  63 static int32_t PIOS_L3GD20_GetRegISR(uint8_t address, bool *woken);
  64 static int32_t PIOS_L3GD20_ClaimBus();
  65 static int32_t PIOS_L3GD20_ClaimBusISR(bool *woken);
  66 static int32_t PIOS_L3GD20_ReleaseBus();
  67 static int32_t PIOS_L3GD20_ReleaseBusISR(bool *woken);
  68
  69 volatile bool l3gd20_configured = false;
  70
  71 /* Local Variables */
  72
  73 /**
  74  * @brief Allocate a new device
  75  */
  76 static struct l3gd20_dev *PIOS_L3GD20_alloc(void)
  77 {
  78     struct l3gd20_dev *l3gd20_dev;
  79
  80     l3gd20_dev = (struct l3gd20_dev *)pios_malloc(sizeof(*l3gd20_dev));
  81     if (!l3gd20_dev) {
  82         return NULL;
  83     }
  84
  85     l3gd20_dev->magic = PIOS_L3GD20_DEV_MAGIC;
  86
  87     l3gd20_dev->queue = xQueueCreate(PIOS_L3GD20_MAX_DOWNSAMPLE, sizeof(struct pios_l3gd20_data));
  88     if (l3gd20_dev->queue == NULL) {
  89         vPortFree(l3gd20_dev);
  90         return NULL;
  91     }
  92
  93     return l3gd20_dev;
  94 }
  95
  96 /**
  97  * @brief Validate the handle to the spi device
  98  * @returns 0 for valid device or -1 otherwise
  99  */
 100 static int32_t PIOS_L3GD20_Validate(struct l3gd20_dev *vdev)
 101 {
 102     if (vdev == NULL) {
 103         return -1;
 104     }
 105     if (vdev->magic != PIOS_L3GD20_DEV_MAGIC) {
 106         return -2;
 107     }
 108     if (vdev->spi_id == 0) {
 109         return -3;
 110     }
 111     return 0;
 112 }
 113
 114 /**
 115  * @brief Initialize the MPU6050 3-axis gyro sensor.
 116  * @return none
 117  */
 118 #include <pios_board_info.h>
 119 int32_t PIOS_L3GD20_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_l3gd20_cfg *cfg)
 120 {
 121     dev = PIOS_L3GD20_alloc();
 122     if (dev == NULL) {
 123         return -1;
 124     }
 125
 126     dev->spi_id    = spi_id;
 127     dev->slave_num = slave_num;
 128     dev->cfg = cfg;
 129
 130     /* Configure the MPU6050 Sensor */
 131     PIOS_L3GD20_Config(cfg);
 132
 133     /* Set up EXTI */
 134     PIOS_EXTI_Init(cfg->exti_cfg);
 135
 136     // An initial read is needed to get it running
 137     struct pios_l3gd20_data data;
 138     PIOS_L3GD20_ReadGyros(&data);
 139
 140     return 0;
 141 }
 142
 143 /**
 144  * @brief Initialize the L3GD20 3-axis gyro sensor
 145  * \return none
 146  * \param[in] PIOS_L3GD20_ConfigTypeDef struct to be used to configure sensor.
 147  *
 148  */
 149 static void PIOS_L3GD20_Config(struct pios_l3gd20_cfg const *cfg)
 150 {
 151     // This register enables the channels and sets the bandwidth
 152     while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG1, PIOS_L3GD20_CTRL1_FASTEST |
 153                               PIOS_L3GD20_CTRL1_PD | PIOS_L3GD20_CTRL1_ZEN |
 154                               PIOS_L3GD20_CTRL1_YEN | PIOS_L3GD20_CTRL1_XEN) != 0) {
 155         ;
 156     }
 157
 158     // Disable the high pass filters
 159     while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG2, 0) != 0) {
 160         ;
 161     }
 162     // Set int2 to go high on data ready
 163     while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG3, 0x08) != 0) {
 164         ;
 165     }
 166     // Select SPI interface, 500 deg/s, endianness?
 167     while (PIOS_L3GD20_SetRange(cfg->range) != 0) {
 168         ;
 169     }
 170     // Enable FIFO, disable HPF
 171     while (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG5, 0x40) != 0) {
 172         ;
 173     }
 174     // Fifo stream mode
 175     while (PIOS_L3GD20_SetReg(PIOS_L3GD20_FIFO_CTRL_REG, 0x40) != 0) {
 176         ;
 177     }
 178 }
 179
 180 /**
 181  * @brief Sets the maximum range of the L3GD20
 182  * @returns 0 for success, -1 for invalid device, -2 if unable to set register
 183  */
 184 int32_t PIOS_L3GD20_SetRange(enum pios_l3gd20_range range)
 185 {
 186     if (PIOS_L3GD20_Validate(dev) != 0) {
 187         return -1;
 188     }
 189
 190     dev->range = range;
 191     if (PIOS_L3GD20_SetReg(PIOS_L3GD20_CTRL_REG4, dev->range) != 0) {
 192         return -2;
 193     }
 194
 195     return 0;
 196 }
 197
 198 /**
 199  * @brief Claim the SPI bus for the accel communications and select this chip
 200  * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
 201  */
 202 static int32_t PIOS_L3GD20_ClaimBus()
 203 {
 204     if (PIOS_L3GD20_Validate(dev) != 0) {
 205         return -1;
 206     }
 207
 208     if (PIOS_SPI_ClaimBus(dev->spi_id) != 0) {
 209         return -2;
 210     }
 211
 212     PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
 213     return 0;
 214 }
 215
 216 /**
 217  * @brief Claim the SPI bus for the accel communications and select this chip
 218  * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
 219  *                      task has is now eligible to run, else unchanged
 220  * @return 0 if successful, -1 for invalid device, -2 if unable to claim bus
 221  */
 222 static int32_t PIOS_L3GD20_ClaimBusISR(bool *woken)
 223 {
 224     if (PIOS_L3GD20_Validate(dev) != 0) {
 225         return -1;
 226     }
 227     if (PIOS_SPI_ClaimBusISR(dev->spi_id, woken) != 0) {
 228         return -2;
 229     }
 230     PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 0);
 231     return 0;
 232 }
 233
 234 /**
 235  * @brief Release the SPI bus for the accel communications and end the transaction
 236  * @return 0 if successful, -1 for invalid device
 237  */
 238 int32_t PIOS_L3GD20_ReleaseBus()
 239 {
 240     if (PIOS_L3GD20_Validate(dev) != 0) {
 241         return -1;
 242     }
 243     PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
 244     return PIOS_SPI_ReleaseBus(dev->spi_id);
 245 }
 246
 247 /**
 248  * @brief Release the SPI bus for the accel communications and end the transaction
 249  * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
 250  *                      task has is now eligible to run, else unchanged
 251  * @return 0 if successful, -1 for invalid device
 252  */
 253 int32_t PIOS_L3GD20_ReleaseBusISR(bool *woken)
 254 {
 255     if (PIOS_L3GD20_Validate(dev) != 0) {
 256         return -1;
 257     }
 258     PIOS_SPI_RC_PinSet(dev->spi_id, dev->slave_num, 1);
 259     return PIOS_SPI_ReleaseBusISR(dev->spi_id, woken);
 260 }
 261
 262 /**
 263  * @brief Read a register from L3GD20
 264  * @returns The register value or -1 if failure to get bus
 265  * @param reg[in] Register address to be read
 266  */
 267 static int32_t PIOS_L3GD20_GetReg(uint8_t reg)
 268 {
 269     uint8_t data;
 270
 271     if (PIOS_L3GD20_ClaimBus() != 0) {
 272         return -1;
 273     }
 274
 275     PIOS_SPI_TransferByte(dev->spi_id, (0x80 | reg)); // request byte
 276     data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response
 277
 278     PIOS_L3GD20_ReleaseBus();
 279     return data;
 280 }
 281
 282 /**
 283  * @brief Read a register from L3GD20 in an ISR context
 284  * @param reg[in] Register address to be read
 285  * @param woken[in,out] If non-NULL, will be set to true if woken was false and a higher priority
 286  *                      task has is now eligible to run, else unchanged
 287  * @return The register value or -1 if failure to get bus
 288  */
 289 static int32_t PIOS_L3GD20_GetRegISR(uint8_t reg, bool *woken)
 290 {
 291     uint8_t data;
 292
 293     if (PIOS_L3GD20_ClaimBusISR(woken) != 0) {
 294         return -1;
 295     }
 296     PIOS_SPI_TransferByte(dev->spi_id, (0x80 | reg)); // request byte
 297     data = PIOS_SPI_TransferByte(dev->spi_id, 0); // receive response
 298     PIOS_L3GD20_ReleaseBusISR(woken);
 299     return data;
 300 }
 301
 302 /**
 303  * @brief Writes one byte to the L3GD20
 304  * \param[in] reg Register address
 305  * \param[in] data Byte to write
 306  * \return 0 if operation was successful
 307  * \return -1 if unable to claim SPI bus
 308  * \return -2 if unable to claim i2c device
 309  */
 310 static int32_t PIOS_L3GD20_SetReg(uint8_t reg, uint8_t data)
 311 {
 312     if (PIOS_L3GD20_ClaimBus() != 0) {
 313         return -1;
 314     }
 315
 316     PIOS_SPI_TransferByte(dev->spi_id, 0x7f & reg);
 317     PIOS_SPI_TransferByte(dev->spi_id, data);
 318
 319     PIOS_L3GD20_ReleaseBus();
 320
 321     return 0;
 322 }
 323
 324 /**
 325  * @brief Read current X, Z, Y values (in that order)
 326  * \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings
 327  * \returns The number of samples remaining in the fifo
 328  */
 329 uint32_t l3gd20_irq = 0;
 330 int32_t PIOS_L3GD20_ReadGyros(struct pios_l3gd20_data *data)
 331 {
 332     uint8_t buf[7] = { PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0 };
 333     uint8_t rec[7];
 334
 335     if (PIOS_L3GD20_ClaimBus() != 0) {
 336         return -1;
 337     }
 338
 339     if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
 340         PIOS_L3GD20_ReleaseBus();
 341         data->gyro_x = 0;
 342         data->gyro_y = 0;
 343         data->gyro_z = 0;
 344         data->temperature = 0;
 345         return -2;
 346     }
 347
 348     PIOS_L3GD20_ReleaseBus();
 349
 350     memcpy((uint8_t *)&(data->gyro_x), &rec[1], 6);
 351     data->temperature = PIOS_L3GD20_GetReg(PIOS_L3GD20_OUT_TEMP);
 352
 353     return 0;
 354 }
 355
 356 /**
 357  * @brief Read the identification bytes from the MPU6050 sensor
 358  * \return ID read from MPU6050 or -1 if failure
 359  */
 360 int32_t PIOS_L3GD20_ReadID()
 361 {
 362     int32_t l3gd20_id = PIOS_L3GD20_GetReg(PIOS_L3GD20_WHOAMI);
 363
 364     if (l3gd20_id < 0) {
 365         return -1;
 366     }
 367     return l3gd20_id;
 368 }
 369
 370 /**
 371  * \brief Reads the queue handle
 372  * \return Handle to the queue or null if invalid device
 373  */
 374 xQueueHandle PIOS_L3GD20_GetQueue()
 375 {
 376     if (PIOS_L3GD20_Validate(dev) != 0) {
 377         return (xQueueHandle)NULL;
 378     }
 379
 380     return dev->queue;
 381 }
 382
 383 float PIOS_L3GD20_GetScale()
 384 {
 385     if (PIOS_L3GD20_Validate(dev) != 0) {
 386         return -1;
 387     }
 388
 389     switch (dev->range) {
 390     case PIOS_L3GD20_SCALE_250_DEG:
 391         return 0.00875f;
 392
 393     case PIOS_L3GD20_SCALE_500_DEG:
 394         return 0.01750f;
 395
 396     case PIOS_L3GD20_SCALE_2000_DEG:
 397         return 0.070f;
 398     }
 399     return 0;
 400 }
 401
 402 /**
 403  * @brief Run self-test operation.
 404  * \return 0 if test succeeded
 405  * \return non-zero value if test succeeded
 406  */
 407 uint8_t PIOS_L3GD20_Test(void)
 408 {
 409     int32_t l3gd20_id = PIOS_L3GD20_ReadID();
 410
 411     if (l3gd20_id < 0) {
 412         return -1;
 413     }
 414
 415     uint8_t id = l3gd20_id;
 416     if (id == 0xD4) {
 417         return 0;
 418     }
 419
 420     return -2;
 421 }
 422
 423 /**
 424  * @brief EXTI IRQ Handler.  Read all the data from onboard buffer
 425  * @return a boolean to the EXTI IRQ Handler wrapper indicating if a
 426  *         higher priority task is now eligible to run
 427  */
 428 bool PIOS_L3GD20_IRQHandler(void)
 429 {
 430     bool woken     = false;
 431     struct pios_l3gd20_data data;
 432     uint8_t buf[7] = { PIOS_L3GD20_GYRO_X_OUT_LSB | 0x80 | 0x40, 0, 0, 0, 0, 0, 0 };
 433     uint8_t rec[7];
 434
 435     l3gd20_irq++;
 436
 437     /* This code duplicates ReadGyros above but uses ClaimBusIsr */
 438     if (PIOS_L3GD20_ClaimBusISR(&woken) != 0) {
 439         return woken;
 440     }
 441
 442     if (PIOS_SPI_TransferBlock(dev->spi_id, &buf[0], &rec[0], sizeof(buf), NULL) < 0) {
 443         PIOS_L3GD20_ReleaseBusISR(&woken);
 444         return woken;
 445     }
 446
 447     PIOS_L3GD20_ReleaseBusISR(&woken);
 448
 449     memcpy((uint8_t *)&(data.gyro_x), &rec[1], 6);
 450     data.temperature = PIOS_L3GD20_GetRegISR(PIOS_L3GD20_OUT_TEMP, &woken);
 451
 452     signed portBASE_TYPE higherPriorityTaskWoken;
 453     xQueueSendToBackFromISR(dev->queue, (void *)&data, &higherPriorityTaskWoken);
 454
 455     return woken || higherPriorityTaskWoken == pdTRUE;
 456 }
 457
 458 #endif /* PIOS_INCLUDE_L3GD20 */
 459
 460 /**
 461  * @}
 462  * @}
 463  */