src/main/drivers/accgyro/accgyro_spi_mpu9250.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 /*
  22  * Authors:
  23  * Dominic Clifton - Cleanflight implementation
  24  * John Ihlein - Initial FF32 code
  25  * Kalyn Doerr (RS2K) - Robust rewrite
  26 */
  27
  28 #include <stdbool.h>
  29 #include <stdint.h>
  30
  31 #include "platform.h"
  32
  33 #include "common/axis.h"
  34 #include "common/maths.h"
  35
  36 #include "drivers/accgyro/accgyro.h"
  37 #include "drivers/accgyro/accgyro_mpu.h"
  38 #include "drivers/accgyro/accgyro_spi_mpu9250.h"
  39 #include "drivers/bus_spi.h"
  40 #include "drivers/exti.h"
  41 #include "drivers/io.h"
  42 #include "drivers/light_led.h"
  43 #include "drivers/sensor.h"
  44 #include "drivers/time.h"
  45 #include "drivers/system.h"
  46
  47 // 20 MHz max SPI frequency
  48 #define MPU9250_MAX_SPI_CLK_HZ 20000000
  49
  50 static void mpu9250AccAndGyroInit(gyroDev_t *gyro);
  51
  52
  53 bool mpu9250SpiWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data)
  54 {
  55     delayMicroseconds(1);
  56     spiWriteRegBuf(dev, reg, &data, sizeof(data));
  57     delayMicroseconds(1);
  58
  59     return true;
  60 }
  61
  62 static bool mpu9250SpiSlowReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length)
  63 {
  64     delayMicroseconds(1);
  65     spiReadRegBuf(dev, reg | 0x80, data, length);
  66     delayMicroseconds(1);
  67
  68     return true;
  69 }
  70
  71 void mpu9250SpiGyroInit(gyroDev_t *gyro)
  72 {
  73     extDevice_t *dev = &gyro->dev;
  74
  75     mpuGyroInit(gyro);
  76
  77     mpu9250AccAndGyroInit(gyro);
  78
  79     spiSetClkDivisor(dev, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ)); //high speed now that we don't need to write to the slow registers
  80
  81     mpuGyroRead(gyro);
  82
  83     if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1)) {
  84         failureMode(FAILURE_GYRO_INIT_FAILED);
  85     }
  86 }
  87
  88 void mpu9250SpiAccInit(accDev_t *acc)
  89 {
  90     acc->acc_1G = 512 * 4;
  91 }
  92
  93 bool mpu9250SpiWriteRegisterVerify(const extDevice_t *dev, uint8_t reg, uint8_t data)
  94 {
  95     mpu9250SpiWriteRegister(dev, reg, data);
  96     delayMicroseconds(100);
  97
  98     uint8_t attemptsRemaining = 20;
  99     do {
 100         uint8_t in;
 101         mpu9250SpiSlowReadRegisterBuffer(dev, reg, &in, 1);
 102         if (in == data) {
 103             return true;
 104         } else {
 105             mpu9250SpiWriteRegister(dev, reg, data);
 106             delayMicroseconds(100);
 107         }
 108     } while (attemptsRemaining--);
 109     return false;
 110 }
 111
 112 static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
 113 {
 114     extDevice_t *dev = &gyro->dev;
 115
 116     mpu9250SpiWriteRegister(dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
 117     delay(50);
 118
 119     mpu9250SpiWriteRegisterVerify(dev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
 120
 121     mpu9250SpiWriteRegisterVerify(dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
 122
 123     mpu9250SpiWriteRegisterVerify(dev, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
 124
 125     mpu9250SpiWriteRegisterVerify(dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops);
 126
 127     mpu9250SpiWriteRegisterVerify(dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
 128     mpu9250SpiWriteRegisterVerify(dev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR, BYPASS_EN
 129
 130     mpu9250SpiWriteRegisterVerify(dev, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
 131 }
 132
 133 uint8_t mpu9250SpiDetect(const extDevice_t *dev)
 134 {
 135     mpu9250SpiWriteRegister(dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
 136
 137     uint8_t attemptsRemaining = 20;
 138     do {
 139         delay(150);
 140         const uint8_t in = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
 141         if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) {
 142             break;
 143         }
 144         if (!attemptsRemaining) {
 145             return MPU_NONE;
 146         }
 147     } while (attemptsRemaining--);
 148
 149     return MPU_9250_SPI;
 150 }
 151
 152 bool mpu9250SpiAccDetect(accDev_t *acc)
 153 {
 154     if (acc->mpuDetectionResult.sensor != MPU_9250_SPI) {
 155         return false;
 156     }
 157
 158     acc->initFn = mpu9250SpiAccInit;
 159     acc->readFn = mpuAccReadSPI;
 160
 161     return true;
 162 }
 163
 164 bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
 165 {
 166     if (gyro->mpuDetectionResult.sensor != MPU_9250_SPI) {
 167         return false;
 168     }
 169
 170     gyro->initFn = mpu9250SpiGyroInit;
 171     gyro->readFn = mpuGyroReadSPI;
 172
 173     gyro->scale = GYRO_SCALE_2000DPS;
 174
 175     return true;
 176 }