src/main/drivers/accgyro_spi_mpu6500.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 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdlib.h>
  21
  22 #include "platform.h"
  23
  24 #include "common/axis.h"
  25 #include "common/maths.h"
  26
  27 #include "system.h"
  28 #include "exti.h"
  29 #include "gpio.h"
  30 #include "bus_spi.h"
  31
  32 #include "sensor.h"
  33 #include "accgyro.h"
  34 #include "accgyro_mpu.h"
  35 #include "accgyro_mpu6500.h"
  36 #include "accgyro_spi_mpu6500.h"
  37
  38 #define DISABLE_MPU6500       GPIO_SetBits(MPU6500_CS_GPIO,   MPU6500_CS_PIN)
  39 #define ENABLE_MPU6500        GPIO_ResetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
  40
  41 bool mpu6500WriteRegister(uint8_t reg, uint8_t data)
  42 {
  43     ENABLE_MPU6500;
  44     spiTransferByte(MPU6500_SPI_INSTANCE, reg);
  45     spiTransferByte(MPU6500_SPI_INSTANCE, data);
  46     DISABLE_MPU6500;
  47
  48     return true;
  49 }
  50
  51 bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
  52 {
  53     ENABLE_MPU6500;
  54     spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
  55     spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
  56     DISABLE_MPU6500;
  57
  58     return true;
  59 }
  60
  61 static void mpu6500SpiInit(void)
  62 {
  63     static bool hardwareInitialised = false;
  64
  65     if (hardwareInitialised) {
  66         return;
  67     }
  68
  69 #ifdef STM32F303xC
  70     RCC_AHBPeriphClockCmd(MPU6500_CS_GPIO_CLK_PERIPHERAL, ENABLE);
  71
  72     GPIO_InitTypeDef GPIO_InitStructure;
  73     GPIO_InitStructure.GPIO_Pin = MPU6500_CS_PIN;
  74     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
  75     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  76     GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  77     GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
  78
  79     GPIO_Init(MPU6500_CS_GPIO, &GPIO_InitStructure);
  80 #endif
  81
  82 #ifdef STM32F10X
  83     RCC_APB2PeriphClockCmd(MPU6500_CS_GPIO_CLK_PERIPHERAL, ENABLE);
  84
  85     gpio_config_t gpio;
  86     // CS as output
  87     gpio.mode = Mode_Out_PP;
  88     gpio.pin = MPU6500_CS_PIN;
  89     gpio.speed = Speed_50MHz;
  90     gpioInit(MPU6500_CS_GPIO, &gpio);
  91 #endif
  92
  93     GPIO_SetBits(MPU6500_CS_GPIO,   MPU6500_CS_PIN);
  94
  95     spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_9MHZ_CLOCK_DIVIDER);
  96
  97     hardwareInitialised = true;
  98 }
  99
 100 bool mpu6500SpiDetect(void)
 101 {
 102     uint8_t sig;
 103
 104     mpu6500SpiInit();
 105
 106     mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &sig);
 107
 108     if (sig == MPU6500_WHO_AM_I_CONST || sig == MPU9250_WHO_AM_I_CONST) {
 109         return true;
 110     }
 111
 112     return false;
 113 }
 114
 115 bool mpu6500SpiAccDetect(acc_t *acc)
 116 {
 117     if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
 118         return false;
 119     }
 120
 121     acc->init = mpu6500AccInit;
 122     acc->read = mpuAccRead;
 123
 124     return true;
 125 }
 126
 127 bool mpu6500SpiGyroDetect(gyro_t *gyro)
 128 {
 129     if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
 130         return false;
 131     }
 132
 133     gyro->init = mpu6500GyroInit;
 134     gyro->read = mpuGyroRead;
 135     gyro->intStatus = checkMPUDataReady;
 136
 137     // 16.4 dps/lsb scalefactor
 138     gyro->scale = 1.0f / 16.4f;
 139
 140     return true;
 141 }