src/main/drivers/accgyro_spi_mpu6000.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18
  19 /*
  20  * Authors:
  21  * Dominic Clifton - Cleanflight implementation
  22  * John Ihlein - Initial FF32 code
  23 */
  24
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27
  28 #include "platform.h"
  29
  30 #include "common/axis.h"
  31 #include "common/maths.h"
  32
  33 #include "system.h"
  34 #include "io.h"
  35 #include "exti.h"
  36 #include "bus_spi.h"
  37
  38 #include "gyro_sync.h"
  39
  40 #include "sensor.h"
  41 #include "accgyro.h"
  42 #include "accgyro_mpu.h"
  43
  44 #if defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_SPI_MPU6000)
  45
  46 #include "accgyro_spi_mpu6000.h"
  47
  48 static void mpu6000AccAndGyroInit(void);
  49
  50 static bool mpuSpi6000InitDone = false;
  51
  52
  53 // Bits
  54 #define BIT_SLEEP                   0x40
  55 #define BIT_H_RESET                 0x80
  56 #define BITS_CLKSEL                 0x07
  57 #define MPU_CLK_SEL_PLLGYROX        0x01
  58 #define MPU_CLK_SEL_PLLGYROZ        0x03
  59 #define MPU_EXT_SYNC_GYROX          0x02
  60 #define BITS_FS_250DPS              0x00
  61 #define BITS_FS_500DPS              0x08
  62 #define BITS_FS_1000DPS             0x10
  63 #define BITS_FS_2000DPS             0x18
  64 #define BITS_FS_2G                  0x00
  65 #define BITS_FS_4G                  0x08
  66 #define BITS_FS_8G                  0x10
  67 #define BITS_FS_16G                 0x18
  68 #define BITS_FS_MASK                0x18
  69 #define BITS_DLPF_CFG_256HZ         0x00
  70 #define BITS_DLPF_CFG_188HZ         0x01
  71 #define BITS_DLPF_CFG_98HZ          0x02
  72 #define BITS_DLPF_CFG_42HZ          0x03
  73 #define BITS_DLPF_CFG_20HZ          0x04
  74 #define BITS_DLPF_CFG_10HZ          0x05
  75 #define BITS_DLPF_CFG_5HZ           0x06
  76 #define BITS_DLPF_CFG_2100HZ_NOLPF  0x07
  77 #define BITS_DLPF_CFG_MASK          0x07
  78 #define BIT_INT_ANYRD_2CLEAR        0x10
  79 #define BIT_RAW_RDY_EN              0x01
  80 #define BIT_I2C_IF_DIS              0x10
  81 #define BIT_INT_STATUS_DATA         0x01
  82 #define BIT_GYRO                    3
  83 #define BIT_ACC                     2
  84 #define BIT_TEMP                    1
  85
  86 // Product ID Description for MPU6000
  87 // high 4 bits low 4 bits
  88 // Product Name Product Revision
  89 #define MPU6000ES_REV_C4 0x14
  90 #define MPU6000ES_REV_C5 0x15
  91 #define MPU6000ES_REV_D6 0x16
  92 #define MPU6000ES_REV_D7 0x17
  93 #define MPU6000ES_REV_D8 0x18
  94 #define MPU6000_REV_C4 0x54
  95 #define MPU6000_REV_C5 0x55
  96 #define MPU6000_REV_D6 0x56
  97 #define MPU6000_REV_D7 0x57
  98 #define MPU6000_REV_D8 0x58
  99 #define MPU6000_REV_D9 0x59
 100 #define MPU6000_REV_D10 0x5A
 101
 102 #define DISABLE_MPU6000       IOHi(mpuSpi6000CsPin)
 103 #define ENABLE_MPU6000        IOLo(mpuSpi6000CsPin)
 104
 105 static IO_t mpuSpi6000CsPin = IO_NONE;
 106
 107 bool mpu6000WriteRegister(uint8_t reg, uint8_t data)
 108 {
 109     ENABLE_MPU6000;
 110     spiTransferByte(MPU6000_SPI_INSTANCE, reg);
 111     spiTransferByte(MPU6000_SPI_INSTANCE, data);
 112     DISABLE_MPU6000;
 113
 114     return true;
 115 }
 116
 117 bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
 118 {
 119     ENABLE_MPU6000;
 120     spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction
 121     spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length);
 122     DISABLE_MPU6000;
 123
 124     return true;
 125 }
 126
 127 void mpu6000SpiGyroInit(uint8_t lpf)
 128 {
 129     mpuIntExtiInit();
 130
 131     mpu6000AccAndGyroInit();
 132
 133     spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
 134
 135     // Accel and Gyro DLPF Setting
 136     mpu6000WriteRegister(MPU6000_CONFIG, lpf);
 137     delayMicroseconds(1);
 138
 139     spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);  // 18 MHz SPI clock
 140
 141     int16_t data[3];
 142     mpuGyroRead(data);
 143
 144     if (((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) {
 145         failureMode(FAILURE_GYRO_INIT_FAILED);
 146     }
 147 }
 148
 149 void mpu6000SpiAccInit(acc_t *acc)
 150 {
 151     mpuIntExtiInit();
 152
 153     acc->acc_1G = 512 * 4;
 154 }
 155
 156 bool mpu6000SpiDetect(void)
 157 {
 158     uint8_t in;
 159     uint8_t attemptsRemaining = 5;
 160
 161 #ifdef MPU6000_CS_PIN
 162     mpuSpi6000CsPin = IOGetByTag(IO_TAG(MPU6000_CS_PIN));
 163 #endif
 164     IOInit(mpuSpi6000CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
 165     IOConfigGPIO(mpuSpi6000CsPin, SPI_IO_CS_CFG);
 166
 167     spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
 168
 169     mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET);
 170
 171     do {
 172         delay(150);
 173
 174         mpu6000ReadRegister(MPU_RA_WHO_AM_I, 1, &in);
 175         if (in == MPU6000_WHO_AM_I_CONST) {
 176             break;
 177         }
 178         if (!attemptsRemaining) {
 179             return false;
 180         }
 181     } while (attemptsRemaining--);
 182
 183
 184     mpu6000ReadRegister(MPU_RA_PRODUCT_ID, 1, &in);
 185
 186     /* look for a product ID we recognise */
 187
 188     // verify product revision
 189     switch (in) {
 190         case MPU6000ES_REV_C4:
 191         case MPU6000ES_REV_C5:
 192         case MPU6000_REV_C4:
 193         case MPU6000_REV_C5:
 194         case MPU6000ES_REV_D6:
 195         case MPU6000ES_REV_D7:
 196         case MPU6000ES_REV_D8:
 197         case MPU6000_REV_D6:
 198         case MPU6000_REV_D7:
 199         case MPU6000_REV_D8:
 200         case MPU6000_REV_D9:
 201         case MPU6000_REV_D10:
 202             return true;
 203     }
 204
 205     return false;
 206 }
 207
 208 static void mpu6000AccAndGyroInit(void) {
 209
 210     if (mpuSpi6000InitDone) {
 211         return;
 212     }
 213
 214     spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
 215
 216     // Device Reset
 217     mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET);
 218     delay(150);
 219
 220     mpu6000WriteRegister(MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
 221     delay(150);
 222
 223     // Clock Source PPL with Z axis gyro reference
 224     mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
 225     delayMicroseconds(15);
 226
 227     // Disable Primary I2C Interface
 228     mpu6000WriteRegister(MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
 229     delayMicroseconds(15);
 230
 231     mpu6000WriteRegister(MPU_RA_PWR_MGMT_2, 0x00);
 232     delayMicroseconds(15);
 233
 234     // Accel Sample Rate 1kHz
 235     // Gyroscope Output Rate =  1kHz when the DLPF is enabled
 236     mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops());
 237     delayMicroseconds(15);
 238
 239     // Gyro +/- 1000 DPS Full Scale
 240     mpu6000WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
 241     delayMicroseconds(15);
 242
 243     // Accel +/- 8 G Full Scale
 244     mpu6000WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
 245     delayMicroseconds(15);
 246
 247
 248     mpu6000WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR
 249     delayMicroseconds(15);
 250
 251 #ifdef USE_MPU_DATA_READY_SIGNAL
 252     mpu6000WriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
 253     delayMicroseconds(15);
 254 #endif
 255
 256     spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);
 257     delayMicroseconds(1);
 258
 259     mpuSpi6000InitDone = true;
 260 }
 261
 262 bool mpu6000SpiAccDetect(acc_t *acc)
 263 {
 264     if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
 265         return false;
 266     }
 267
 268     acc->init = mpu6000SpiAccInit;
 269     acc->read = mpuAccRead;
 270
 271     return true;
 272 }
 273
 274 bool mpu6000SpiGyroDetect(gyro_t *gyro)
 275 {
 276     if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
 277         return false;
 278     }
 279
 280     gyro->init = mpu6000SpiGyroInit;
 281     gyro->read = mpuGyroRead;
 282     gyro->intStatus = checkMPUDataReady;
 283     // 16.4 dps/lsb scalefactor
 284     gyro->scale = 1.0f / 16.4f;
 285
 286     return true;
 287 }
 288
 289 #endif