src/main/drivers/accgyro_spi_mpu9250.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  * Kalyn Doerr (RS2K) - Robust rewrite
  24 */
  25
  26 #include <stdbool.h>
  27 #include <stdint.h>
  28
  29 #include "platform.h"
  30
  31 #include "build/debug.h"
  32
  33 #include "common/axis.h"
  34 #include "common/maths.h"
  35
  36 #include "io.h"
  37
  38 #include "system.h"
  39 #include "exti.h"
  40 #include "bus_spi.h"
  41 #include "gyro_sync.h"
  42 #include "light_led.h"
  43
  44 #include "sensor.h"
  45 #include "accgyro.h"
  46 #include "accgyro_mpu.h"
  47 #include "accgyro_spi_mpu9250.h"
  48
  49 static void mpu9250AccAndGyroInit(gyroDev_t *gyro);
  50
  51 static bool mpuSpi9250InitDone = false;
  52
  53 #define DISABLE_MPU9250(spiCsnPin)       IOHi(spiCsnPin)
  54 #define ENABLE_MPU9250(spiCsnPin)        IOLo(spiCsnPin)
  55
  56 void mpu9250SpiResetGyro(void)
  57 {
  58     // Device Reset
  59 #ifdef MPU9250_CS_PIN
  60     busDevice_t bus = { .spi = { .csnPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN)) } };
  61     mpu9250SpiWriteRegister(&bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
  62     delay(150);
  63 #endif
  64 }
  65
  66 bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
  67 {
  68     ENABLE_MPU9250(bus->spi.csnPin);
  69     delayMicroseconds(1);
  70     spiTransferByte(MPU9250_SPI_INSTANCE, reg);
  71     spiTransferByte(MPU9250_SPI_INSTANCE, data);
  72     DISABLE_MPU9250(bus->spi.csnPin);
  73     delayMicroseconds(1);
  74
  75     return true;
  76 }
  77
  78 bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
  79 {
  80     ENABLE_MPU9250(bus->spi.csnPin);
  81     spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
  82     spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
  83     DISABLE_MPU9250(bus->spi.csnPin);
  84
  85     return true;
  86 }
  87
  88 bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
  89 {
  90     ENABLE_MPU9250(bus->spi.csnPin);
  91     delayMicroseconds(1);
  92     spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
  93     spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
  94     DISABLE_MPU9250(bus->spi.csnPin);
  95     delayMicroseconds(1);
  96
  97     return true;
  98 }
  99
 100 void mpu9250SpiGyroInit(gyroDev_t *gyro)
 101 {
 102     mpuGyroInit(gyro);
 103
 104     mpu9250AccAndGyroInit(gyro);
 105
 106     spiResetErrorCounter(MPU9250_SPI_INSTANCE);
 107
 108     spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers
 109
 110     mpuGyroRead(gyro);
 111
 112     if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) {
 113         spiResetErrorCounter(MPU9250_SPI_INSTANCE);
 114         failureMode(FAILURE_GYRO_INIT_FAILED);
 115     }
 116 }
 117
 118 void mpu9250SpiAccInit(accDev_t *acc)
 119 {
 120     acc->acc_1G = 512 * 8;
 121 }
 122
 123 bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
 124 {
 125     uint8_t in;
 126     uint8_t attemptsRemaining = 20;
 127
 128     mpu9250SpiWriteRegister(bus, reg, data);
 129     delayMicroseconds(100);
 130
 131     do {
 132         mpu9250SpiSlowReadRegister(bus, reg, 1, &in);
 133         if (in == data) {
 134             return true;
 135         } else {
 136             debug[3]++;
 137             mpu9250SpiWriteRegister(bus, reg, data);
 138             delayMicroseconds(100);
 139         }
 140     } while (attemptsRemaining--);
 141     return false;
 142 }
 143
 144 static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
 145
 146     if (mpuSpi9250InitDone) {
 147         return;
 148     }
 149
 150     spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
 151
 152     mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
 153     delay(50);
 154
 155     verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
 156
 157     //Fchoice_b defaults to 00 which makes fchoice 11
 158     const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
 159     verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
 160
 161     if (gyro->lpf == 4) {
 162         verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF
 163     } else if (gyro->lpf < 4) {
 164         verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
 165     } else if (gyro->lpf > 4) {
 166         verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF
 167     }
 168
 169     verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
 170
 171     verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
 172     verifympu9250SpiWriteRegister(&gyro->bus, 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
 173
 174 #if defined(USE_MPU_DATA_READY_SIGNAL)
 175     verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
 176 #endif
 177
 178     spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
 179
 180     mpuSpi9250InitDone = true; //init done
 181 }
 182
 183 bool mpu9250SpiDetect(const busDevice_t *bus)
 184 {
 185     uint8_t in;
 186     uint8_t attemptsRemaining = 20;
 187
 188     IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
 189     IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
 190
 191     spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
 192     mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
 193
 194     do {
 195         delay(150);
 196
 197         mpu9250SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
 198         if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) {
 199             break;
 200         }
 201         if (!attemptsRemaining) {
 202             return false;
 203         }
 204     } while (attemptsRemaining--);
 205
 206     spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
 207
 208     return true;
 209 }
 210
 211 bool mpu9250SpiAccDetect(accDev_t *acc)
 212 {
 213     if (acc->mpuDetectionResult.sensor != MPU_9250_SPI) {
 214         return false;
 215     }
 216
 217     acc->init = mpu9250SpiAccInit;
 218     acc->read = mpuAccRead;
 219
 220     return true;
 221 }
 222
 223 bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
 224 {
 225     if (gyro->mpuDetectionResult.sensor != MPU_9250_SPI) {
 226         return false;
 227     }
 228
 229     gyro->init = mpu9250SpiGyroInit;
 230     gyro->read = mpuGyroRead;
 231     gyro->intStatus = mpuCheckDataReady;
 232
 233     // 16.4 dps/lsb scalefactor
 234     gyro->scale = 1.0f / 16.4f;
 235
 236     return true;
 237 }