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
  48 // 1 MHz max SPI frequency for initialisation
  49 #define MPU9250_MAX_SPI_INIT_CLK_HZ 1000000
  50 // 20 MHz max SPI frequency
  51 #define MPU9250_MAX_SPI_CLK_HZ 20000000
  52
  53 static void mpu9250AccAndGyroInit(gyroDev_t *gyro);
  54
  55
  56 bool mpu9250SpiWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data)
  57 {
  58     delayMicroseconds(1);
  59     spiWriteRegBuf(dev, reg, &data, sizeof(data));
  60     delayMicroseconds(1);
  61
  62     return true;
  63 }
  64
  65 static bool mpu9250SpiSlowReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length)
  66 {
  67     delayMicroseconds(1);
  68     spiReadRegBuf(dev, reg | 0x80, data, length);
  69     delayMicroseconds(1);
  70
  71     return true;
  72 }
  73
  74 void mpu9250SpiGyroInit(gyroDev_t *gyro)
  75 {
  76     mpuGyroInit(gyro);
  77
  78     mpu9250AccAndGyroInit(gyro);
  79
  80     spiSetClkDivisor(&gyro->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 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     spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU9250_MAX_SPI_INIT_CLK_HZ)); //low speed for writing to slow registers
 116
 117     mpu9250SpiWriteRegister(&gyro->dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
 118     delay(50);
 119
 120     mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
 121
 122     mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
 123
 124     mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
 125
 126     mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops);
 127
 128     mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
 129     mpu9250SpiWriteRegisterVerify(&gyro->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 #if defined(USE_MPU_DATA_READY_SIGNAL)
 132     mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
 133 #endif
 134
 135     spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ));
 136 }
 137
 138 uint8_t mpu9250SpiDetect(const extDevice_t *dev)
 139 {
 140
 141     spiSetClkDivisor(dev, spiCalculateDivider(MPU9250_MAX_SPI_INIT_CLK_HZ)); //low speed
 142     mpu9250SpiWriteRegister(dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
 143
 144     uint8_t attemptsRemaining = 20;
 145     do {
 146         delay(150);
 147         const uint8_t in = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
 148         if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) {
 149             break;
 150         }
 151         if (!attemptsRemaining) {
 152             return MPU_NONE;
 153         }
 154     } while (attemptsRemaining--);
 155
 156     spiSetClkDivisor(dev, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ));
 157
 158     return MPU_9250_SPI;
 159 }
 160
 161 bool mpu9250SpiAccDetect(accDev_t *acc)
 162 {
 163     if (acc->mpuDetectionResult.sensor != MPU_9250_SPI) {
 164         return false;
 165     }
 166
 167     acc->initFn = mpu9250SpiAccInit;
 168     acc->readFn = mpuAccReadSPI;
 169
 170     return true;
 171 }
 172
 173 bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
 174 {
 175     if (gyro->mpuDetectionResult.sensor != MPU_9250_SPI) {
 176         return false;
 177     }
 178
 179     gyro->initFn = mpu9250SpiGyroInit;
 180     gyro->readFn = mpuGyroReadSPI;
 181
 182     gyro->scale = GYRO_SCALE_2000DPS;
 183
 184     return true;
 185 }