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 extern uint16_t acc_1G;
  42
  43 bool mpu6500WriteRegister(uint8_t reg, uint8_t data)
  44 {
  45     ENABLE_MPU6500;
  46     spiTransferByte(MPU6500_SPI_INSTANCE, reg);
  47     spiTransferByte(MPU6500_SPI_INSTANCE, data);
  48     DISABLE_MPU6500;
  49
  50     return true;
  51 }
  52
  53 bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
  54 {
  55     ENABLE_MPU6500;
  56     spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
  57     spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
  58     DISABLE_MPU6500;
  59
  60     return true;
  61 }
  62
  63 static void mpu6500SpiInit(void)
  64 {
  65     static bool hardwareInitialised = false;
  66
  67     if (hardwareInitialised) {
  68         return;
  69     }
  70
  71 #ifdef STM32F303xC
  72     RCC_AHBPeriphClockCmd(MPU6500_CS_GPIO_CLK_PERIPHERAL, ENABLE);
  73
  74     GPIO_InitTypeDef GPIO_InitStructure;
  75     GPIO_InitStructure.GPIO_Pin = MPU6500_CS_PIN;
  76     GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
  77     GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  78     GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  79     GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
  80
  81     GPIO_Init(MPU6500_CS_GPIO, &GPIO_InitStructure);
  82 #endif
  83
  84 #ifdef STM32F10X
  85     RCC_APB2PeriphClockCmd(MPU6500_CS_GPIO_CLK_PERIPHERAL, ENABLE);
  86
  87     gpio_config_t gpio;
  88     // CS as output
  89     gpio.mode = Mode_Out_PP;
  90     gpio.pin = MPU6500_CS_PIN;
  91     gpio.speed = Speed_50MHz;
  92     gpioInit(MPU6500_CS_GPIO, &gpio);
  93 #endif
  94
  95     GPIO_SetBits(MPU6500_CS_GPIO,   MPU6500_CS_PIN);
  96
  97     spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_9MHZ_CLOCK_DIVIDER);
  98
  99     hardwareInitialised = true;
 100 }
 101
 102 bool mpu6500SpiDetect(void)
 103 {
 104     uint8_t tmp;
 105
 106     mpu6500SpiInit();
 107
 108     mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp);
 109
 110     if (tmp != MPU6500_WHO_AM_I_CONST)
 111         return false;
 112
 113     return true;
 114 }
 115
 116 bool mpu6500SpiAccDetect(acc_t *acc)
 117 {
 118     if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
 119         return false;
 120     }
 121
 122     acc->init = mpu6500AccInit;
 123     acc->read = mpuAccRead;
 124
 125     return true;
 126 }
 127
 128 bool mpu6500SpiGyroDetect(gyro_t *gyro)
 129 {
 130     if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
 131         return false;
 132     }
 133
 134     gyro->init = mpu6500GyroInit;
 135     gyro->read = mpuGyroRead;
 136
 137     // 16.4 dps/lsb scalefactor
 138     gyro->scale = 1.0f / 16.4f;
 139
 140     return true;
 141 }
 142