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