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 #if defined(USE_MPU_DATA_READY_SIGNAL)
  81 static void l3gd20ExtiHandler(extiCallbackRec_t *cb)
  82 {
  83     gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
  84     gyro->dataReady = true;
  85 }
  86
  87 static void l3gd20IntExtiInit(gyroDev_t *gyro)
  88 {
  89     if (gyro->mpuIntExtiTag == IO_TAG_NONE) {
  90         return;
  91     }
  92
  93     IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag);
  94
  95     IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0);
  96     EXTIHandlerInit(&gyro->exti, l3gd20ExtiHandler);
  97     EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING);
  98     EXTIEnable(mpuIntIO, true);
  99 }
 100 #endif
 101
 102 void l3gd20GyroInit(gyroDev_t *gyro)
 103 {
 104     spiSetClkDivisor(&gyro->dev, spiCalculateDivider(L3GD20_MAX_SPI_CLK_HZ));
 105
 106     spiWriteReg(&gyro->dev, CTRL_REG5_ADDR, BOOT);
 107
 108     delayMicroseconds(100);
 109
 110     spiWriteReg(&gyro->dev, CTRL_REG1_ADDR, MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_3);
 111
 112     delayMicroseconds(1);
 113
 114     spiWriteReg(&gyro->dev, CTRL_REG4_ADDR, BLOCK_DATA_UPDATE_CONTINUOUS | BLE_MSB | FULLSCALE_2000);
 115
 116     delay(100);
 117
 118 #if defined(USE_MPU_DATA_READY_SIGNAL)
 119     l3gd20IntExtiInit(gyro);
 120 #endif
 121
 122 }
 123
 124 static bool l3gd20GyroRead(gyroDev_t *gyro)
 125 {
 126     uint8_t buf[6];
 127
 128     const bool ack = spiReadRegMskBufRB(&gyro->dev, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD,buf, sizeof(buf));
 129     if (!ack) {
 130         return false;
 131     }
 132
 133     gyro->gyroADCRaw[0] = (int16_t)((buf[0] << 8) | buf[1]);
 134     gyro->gyroADCRaw[1] = (int16_t)((buf[2] << 8) | buf[3]);
 135     gyro->gyroADCRaw[2] = (int16_t)((buf[4] << 8) | buf[5]);
 136
 137     return true;
 138 }
 139
 140 // Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
 141 #define L3GD20_GYRO_SCALE_FACTOR  0.07f
 142
 143 uint8_t l3gd20Detect(const extDevice_t *dev)
 144 {
 145     UNUSED(dev);
 146
 147     return L3GD20_SPI; // blindly assume it's present, for now.
 148 }
 149
 150 bool l3gd20GyroDetect(gyroDev_t *gyro)
 151 {
 152     gyro->initFn = l3gd20GyroInit;
 153     gyro->readFn = l3gd20GyroRead;
 154
 155     gyro->scale = L3GD20_GYRO_SCALE_FACTOR;
 156
 157     return true;  // blindly assume it's present, for now.
 158 }
 159
 160 #endif // USE_GYRO_L3GD20