src/main/drivers/accgyro/accgyro_spi_mpu6000.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 */
  26
  27 #include <stdbool.h>
  28 #include <stdint.h>
  29
  30 #include "platform.h"
  31
  32 #if defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_SPI_MPU6000)
  33
  34 #include "common/axis.h"
  35 #include "common/maths.h"
  36
  37 #include "drivers/accgyro/accgyro.h"
  38 #include "drivers/accgyro/accgyro_mpu.h"
  39 #include "drivers/accgyro/accgyro_spi_mpu6000.h"
  40 #include "drivers/bus_spi.h"
  41 #include "drivers/exti.h"
  42 #include "drivers/io.h"
  43 #include "drivers/time.h"
  44 #include "drivers/sensor.h"
  45 #include "drivers/system.h"
  46
  47
  48 static void mpu6000AccAndGyroInit(gyroDev_t *gyro);
  49
  50 // 1 MHz max SPI frequency for initialisation
  51 #define MPU6000_MAX_SPI_INIT_CLK_HZ 1000000
  52 // 20 MHz max SPI frequency
  53 #define MPU6000_MAX_SPI_CLK_HZ 20000000
  54
  55 #define MPU6000_SHORT_THRESHOLD         82  // Any interrupt interval less than this will be recognised as the short interval of ~79us
  56
  57 // Bits
  58 #define BIT_SLEEP                   0x40
  59 #define BIT_H_RESET                 0x80
  60 #define BITS_CLKSEL                 0x07
  61 #define MPU_CLK_SEL_PLLGYROX        0x01
  62 #define MPU_CLK_SEL_PLLGYROZ        0x03
  63 #define MPU_EXT_SYNC_GYROX          0x02
  64 #define BITS_FS_250DPS              0x00
  65 #define BITS_FS_500DPS              0x08
  66 #define BITS_FS_1000DPS             0x10
  67 #define BITS_FS_2000DPS             0x18
  68 #define BITS_FS_2G                  0x00
  69 #define BITS_FS_4G                  0x08
  70 #define BITS_FS_8G                  0x10
  71 #define BITS_FS_16G                 0x18
  72 #define BITS_FS_MASK                0x18
  73 #define BITS_DLPF_CFG_256HZ         0x00
  74 #define BITS_DLPF_CFG_188HZ         0x01
  75 #define BITS_DLPF_CFG_98HZ          0x02
  76 #define BITS_DLPF_CFG_42HZ          0x03
  77 #define BITS_DLPF_CFG_20HZ          0x04
  78 #define BITS_DLPF_CFG_10HZ          0x05
  79 #define BITS_DLPF_CFG_5HZ           0x06
  80 #define BITS_DLPF_CFG_2100HZ_NOLPF  0x07
  81 #define BITS_DLPF_CFG_MASK          0x07
  82 #define BIT_INT_ANYRD_2CLEAR        0x10
  83 #define BIT_RAW_RDY_EN              0x01
  84 #define BIT_I2C_IF_DIS              0x10
  85 #define BIT_INT_STATUS_DATA         0x01
  86 #define BIT_GYRO                    0x04
  87 #define BIT_ACC                     0x02
  88 #define BIT_TEMP                    0x01
  89
  90 // Product ID Description for MPU6000
  91 // high 4 bits low 4 bits
  92 // Product Name Product Revision
  93 #define MPU6000ES_REV_C4 0x14
  94 #define MPU6000ES_REV_C5 0x15
  95 #define MPU6000ES_REV_D6 0x16
  96 #define MPU6000ES_REV_D7 0x17
  97 #define MPU6000ES_REV_D8 0x18
  98 #define MPU6000_REV_C4 0x54
  99 #define MPU6000_REV_C5 0x55
 100 #define MPU6000_REV_D6 0x56
 101 #define MPU6000_REV_D7 0x57
 102 #define MPU6000_REV_D8 0x58
 103 #define MPU6000_REV_D9 0x59
 104 #define MPU6000_REV_D10 0x5A
 105
 106 void mpu6000SpiGyroInit(gyroDev_t *gyro)
 107 {
 108     mpuGyroInit(gyro);
 109
 110     mpu6000AccAndGyroInit(gyro);
 111
 112     spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ));
 113
 114     // Accel and Gyro DLPF Setting
 115     spiWriteReg(&gyro->dev, MPU6000_CONFIG, mpuGyroDLPF(gyro));
 116     delayMicroseconds(1);
 117
 118     spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ));
 119
 120     mpuGyroRead(gyro);
 121
 122     if (((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) {
 123         failureMode(FAILURE_GYRO_INIT_FAILED);
 124     }
 125 }
 126
 127 void mpu6000SpiAccInit(accDev_t *acc)
 128 {
 129     acc->acc_1G = 512 * 4;
 130 }
 131
 132 uint8_t mpu6000SpiDetect(const extDevice_t *dev)
 133 {
 134
 135     spiSetClkDivisor(dev, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ));
 136
 137     // reset the device configuration
 138     spiWriteReg(dev, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
 139     delay(100);  // datasheet specifies a 100ms delay after reset
 140
 141     // reset the device signal paths
 142     spiWriteReg(dev, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
 143     delay(100);  // datasheet specifies a 100ms delay after signal path reset
 144
 145
 146     const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
 147     delayMicroseconds(1); // Ensure CS high time is met which is violated on H7 without this delay
 148     uint8_t detectedSensor = MPU_NONE;
 149
 150     if (whoAmI == MPU6000_WHO_AM_I_CONST) {
 151         const uint8_t productID = spiReadRegMsk(dev, MPU_RA_PRODUCT_ID);
 152
 153         /* look for a product ID we recognise */
 154
 155         // verify product revision
 156         switch (productID) {
 157         case MPU6000ES_REV_C4:
 158         case MPU6000ES_REV_C5:
 159         case MPU6000_REV_C4:
 160         case MPU6000_REV_C5:
 161         case MPU6000ES_REV_D6:
 162         case MPU6000ES_REV_D7:
 163         case MPU6000ES_REV_D8:
 164         case MPU6000_REV_D6:
 165         case MPU6000_REV_D7:
 166         case MPU6000_REV_D8:
 167         case MPU6000_REV_D9:
 168         case MPU6000_REV_D10:
 169             detectedSensor = MPU_60x0_SPI;
 170         }
 171     }
 172
 173     spiSetClkDivisor(dev, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ));
 174     return detectedSensor;
 175 }
 176
 177 static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
 178 {
 179     spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ));
 180
 181     // Device was already reset during detection so proceed with configuration
 182
 183     // Clock Source PPL with Z axis gyro reference
 184     spiWriteReg(&gyro->dev, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
 185     delayMicroseconds(15);
 186
 187     // Disable Primary I2C Interface
 188     spiWriteReg(&gyro->dev, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
 189     delayMicroseconds(15);
 190
 191     spiWriteReg(&gyro->dev, MPU_RA_PWR_MGMT_2, 0x00);
 192     delayMicroseconds(15);
 193
 194     // Accel Sample Rate 1kHz
 195     // Gyroscope Output Rate =  1kHz when the DLPF is enabled
 196     spiWriteReg(&gyro->dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops);
 197     delayMicroseconds(15);
 198
 199     // Gyro +/- 2000 DPS Full Scale
 200     spiWriteReg(&gyro->dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
 201     delayMicroseconds(15);
 202
 203     // Accel +/- 16 G Full Scale
 204     spiWriteReg(&gyro->dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
 205     delayMicroseconds(15);
 206
 207     spiWriteReg(&gyro->dev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR
 208     delayMicroseconds(15);
 209
 210 #ifdef USE_MPU_DATA_READY_SIGNAL
 211     spiWriteReg(&gyro->dev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
 212     delayMicroseconds(15);
 213 #endif
 214
 215     spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ));
 216     delayMicroseconds(1);
 217 }
 218
 219 bool mpu6000SpiAccDetect(accDev_t *acc)
 220 {
 221     if (acc->mpuDetectionResult.sensor != MPU_60x0_SPI) {
 222         return false;
 223     }
 224
 225     acc->initFn = mpu6000SpiAccInit;
 226     acc->readFn = mpuAccReadSPI;
 227
 228     return true;
 229 }
 230
 231 bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
 232 {
 233     if (gyro->mpuDetectionResult.sensor != MPU_60x0_SPI) {
 234         return false;
 235     }
 236
 237     gyro->initFn = mpu6000SpiGyroInit;
 238     gyro->readFn = mpuGyroReadSPI;
 239     gyro->scale = GYRO_SCALE_2000DPS;
 240 #ifdef USE_GYRO_EXTI
 241     gyro->gyroShortPeriod = clockMicrosToCycles(MPU6000_SHORT_THRESHOLD);
 242 #endif
 243     return true;
 244 }
 245
 246 #endif