src/main/drivers/accgyro/accgyro_spi_l3gd20.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23
  24 #include "platform.h"
  25
  26 #ifdef USE_GYRO_L3GD20
  27
  28 #include "build/build_config.h"
  29 #include "build/debug.h"
  30
  31 #include "common/maths.h"
  32
  33 #include "drivers/bus_spi.h"
  34 #include "drivers/io.h"
  35 #include "drivers/nvic.h"
  36 #include "drivers/sensor.h"
  37 #include "drivers/time.h"
  38
  39 #include "drivers/accgyro/accgyro.h"
  40
  41 #include "accgyro_spi_l3gd20.h"
  42
  43 // 10 MHz max SPI frequency
  44 #define L3GD20_MAX_SPI_CLK_HZ 10000000
  45
  46 #define READ_CMD               ((uint8_t)0x80)
  47 #define MULTIPLEBYTE_CMD       ((uint8_t)0x40)
  48 #define DUMMY_BYTE             ((uint8_t)0x00)
  49
  50 #define CTRL_REG1_ADDR         0x20
  51 #define CTRL_REG4_ADDR         0x23
  52 #define CTRL_REG5_ADDR         0x24
  53 #define OUT_TEMP_ADDR          0x26
  54 #define OUT_X_L_ADDR           0x28
  55
  56 #define MODE_ACTIVE                   ((uint8_t)0x08)
  57
  58 #define OUTPUT_DATARATE_1             ((uint8_t)0x00)
  59 #define OUTPUT_DATARATE_2             ((uint8_t)0x40)
  60 #define OUTPUT_DATARATE_3             ((uint8_t)0x80)
  61 #define OUTPUT_DATARATE_4             ((uint8_t)0xC0)
  62
  63 #define AXES_ENABLE                   ((uint8_t)0x07)
  64
  65 #define BANDWIDTH_1                   ((uint8_t)0x00)
  66 #define BANDWIDTH_2                   ((uint8_t)0x10)
  67 #define BANDWIDTH_3                   ((uint8_t)0x20)
  68 #define BANDWIDTH_4                   ((uint8_t)0x30)
  69
  70 #define FULLSCALE_250                 ((uint8_t)0x00)
  71 #define FULLSCALE_500                 ((uint8_t)0x10)
  72 #define FULLSCALE_2000                ((uint8_t)0x20)
  73
  74 #define BLOCK_DATA_UPDATE_CONTINUOUS  ((uint8_t)0x00)
  75
  76 #define BLE_MSB                       ((uint8_t)0x40)
  77
  78 #define BOOT                          ((uint8_t)0x80)
  79
  80 static void l3gd20ExtiHandler(extiCallbackRec_t *cb)
  81 {
  82     gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
  83     gyro->dataReady = true;
  84 }
  85
  86 static void l3gd20IntExtiInit(gyroDev_t *gyro)
  87 {
  88     if (gyro->mpuIntExtiTag == IO_TAG_NONE) {
  89         return;
  90     }
  91
  92     IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag);
  93
  94     IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0);
  95     EXTIHandlerInit(&gyro->exti, l3gd20ExtiHandler);
  96     EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING);
  97     EXTIEnable(mpuIntIO);
  98 }
  99
 100 void l3gd20GyroInit(gyroDev_t *gyro)
 101 {
 102     extDevice_t *dev = &gyro->dev;
 103
 104     spiSetClkDivisor(dev, spiCalculateDivider(L3GD20_MAX_SPI_CLK_HZ));
 105
 106     spiWriteReg(dev, CTRL_REG5_ADDR, BOOT);
 107
 108     delayMicroseconds(100);
 109
 110     spiWriteReg(dev, CTRL_REG1_ADDR, MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_3);
 111
 112     delayMicroseconds(1);
 113
 114     spiWriteReg(dev, CTRL_REG4_ADDR, BLOCK_DATA_UPDATE_CONTINUOUS | BLE_MSB | FULLSCALE_2000);
 115
 116     delay(100);
 117
 118     l3gd20IntExtiInit(gyro);
 119 }
 120
 121 static bool l3gd20GyroRead(gyroDev_t *gyro)
 122 {
 123     uint8_t buf[6];
 124     extDevice_t *dev = &gyro->dev;
 125
 126     const bool ack = spiReadRegMskBufRB(dev, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD,buf, sizeof(buf));
 127     if (!ack) {
 128         return false;
 129     }
 130
 131     gyro->gyroADCRaw[0] = (int16_t)((buf[0] << 8) | buf[1]);
 132     gyro->gyroADCRaw[1] = (int16_t)((buf[2] << 8) | buf[3]);
 133     gyro->gyroADCRaw[2] = (int16_t)((buf[4] << 8) | buf[5]);
 134
 135     return true;
 136 }
 137
 138 // Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
 139 #define L3GD20_GYRO_SCALE_FACTOR  0.07f
 140
 141 uint8_t l3gd20Detect(const extDevice_t *dev)
 142 {
 143     UNUSED(dev);
 144
 145     return L3GD20_SPI; // blindly assume it's present, for now.
 146 }
 147
 148 bool l3gd20GyroDetect(gyroDev_t *gyro)
 149 {
 150     gyro->initFn = l3gd20GyroInit;
 151     gyro->readFn = l3gd20GyroRead;
 152
 153     gyro->scale = L3GD20_GYRO_SCALE_FACTOR;
 154
 155     return true;  // blindly assume it's present, for now.
 156 }
 157
 158 #endif // USE_GYRO_L3GD20