From 5f16cfb72aba005e6d1766384d48f59ad29c3a51 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 17 Jul 2014 22:12:21 +0100 Subject: [PATCH] CC3D - Support MPU6000 Gyro/Acc and SPI 1/2. --- Makefile | 1 + src/main/drivers/accgyro_mma845x.c | 1 + src/main/drivers/accgyro_spi_mpu6000.c | 362 +++++++++++++++++++++++++ src/main/drivers/accgyro_spi_mpu6000.h | 18 ++ src/main/drivers/adc_stm32f10x.c | 5 + src/main/drivers/bus_spi.c | 246 ++++++++++++++--- src/main/drivers/bus_spi.h | 12 +- src/main/drivers/system.c | 6 +- src/main/io/serial_cli.c | 2 +- src/main/sensors/acceleration.h | 5 +- src/main/sensors/initialisation.c | 35 ++- src/main/target/CC3D/target.h | 2 +- 12 files changed, 647 insertions(+), 48 deletions(-) create mode 100644 src/main/drivers/accgyro_spi_mpu6000.c create mode 100644 src/main/drivers/accgyro_spi_mpu6000.h diff --git a/Makefile b/Makefile index 750cd7a91..9b66e4329 100644 --- a/Makefile +++ b/Makefile @@ -223,6 +223,7 @@ OLIMEXINO_SRC = startup_stm32f10x_md_gcc.S \ $(COMMON_SRC) CC3D_SRC = startup_stm32f10x_md_gcc.S \ + drivers/accgyro_spi_mpu6000.c \ drivers/adc.c \ drivers/adc_stm32f10x.c \ drivers/bus_i2c_stm32f10x.c \ diff --git a/src/main/drivers/accgyro_mma845x.c b/src/main/drivers/accgyro_mma845x.c index 98a67d1d5..1a4c15379 100644 --- a/src/main/drivers/accgyro_mma845x.c +++ b/src/main/drivers/accgyro_mma845x.c @@ -102,6 +102,7 @@ static inline void mma8451ConfigureInterrupt(void) { #ifdef NAZE // PA5 - ACC_INT2 output on NAZE rev3/4 hardware + // NAZE rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board // OLIMEXINO - The PA5 pin is wired up to LED1, if you need to use an mma8452 on an Olimexino use a different pin and provide support in code. gpio_config_t gpio; diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c new file mode 100644 index 000000000..512b5776b --- /dev/null +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -0,0 +1,362 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + + +/* + * Authors: + * Dominic Clifton - Cleanflight implementation + * John Ihlein - Initial FF32 code +*/ + +#include +#include +#include + +#include "platform.h" + +#include "common/axis.h" +#include "common/maths.h" + +#include "system.h" +#include "gpio.h" +#include "bus_spi.h" + +#include "accgyro.h" +#include "accgyro_spi_mpu6000.h" + +// Registers +#define MPU6000_PRODUCT_ID 0x0C +#define MPU6000_SMPLRT_DIV 0x19 +#define MPU6000_GYRO_CONFIG 0x1B +#define MPU6000_ACCEL_CONFIG 0x1C +#define MPU6000_FIFO_EN 0x23 +#define MPU6000_INT_PIN_CFG 0x37 +#define MPU6000_INT_ENABLE 0x38 +#define MPU6000_INT_STATUS 0x3A +#define MPU6000_ACCEL_XOUT_H 0x3B +#define MPU6000_ACCEL_XOUT_L 0x3C +#define MPU6000_ACCEL_YOUT_H 0x3D +#define MPU6000_ACCEL_YOUT_L 0x3E +#define MPU6000_ACCEL_ZOUT_H 0x3F +#define MPU6000_ACCEL_ZOUT_L 0x40 +#define MPU6000_TEMP_OUT_H 0x41 +#define MPU6000_TEMP_OUT_L 0x42 +#define MPU6000_GYRO_XOUT_H 0x43 +#define MPU6000_GYRO_XOUT_L 0x44 +#define MPU6000_GYRO_YOUT_H 0x45 +#define MPU6000_GYRO_YOUT_L 0x46 +#define MPU6000_GYRO_ZOUT_H 0x47 +#define MPU6000_GYRO_ZOUT_L 0x48 +#define MPU6000_USER_CTRL 0x6A +#define MPU6000_SIGNAL_PATH_RESET 0x68 +#define MPU6000_PWR_MGMT_1 0x6B +#define MPU6000_PWR_MGMT_2 0x6C +#define MPU6000_FIFO_COUNTH 0x72 +#define MPU6000_FIFO_COUNTL 0x73 +#define MPU6000_FIFO_R_W 0x74 +#define MPU6000_WHOAMI 0x75 + +// Bits +#define BIT_SLEEP 0x40 +#define BIT_H_RESET 0x80 +#define BITS_CLKSEL 0x07 +#define MPU_CLK_SEL_PLLGYROX 0x01 +#define MPU_CLK_SEL_PLLGYROZ 0x03 +#define MPU_EXT_SYNC_GYROX 0x02 +#define BITS_FS_250DPS 0x00 +#define BITS_FS_500DPS 0x08 +#define BITS_FS_1000DPS 0x10 +#define BITS_FS_2000DPS 0x18 +#define BITS_FS_2G 0x00 +#define BITS_FS_4G 0x08 +#define BITS_FS_8G 0x10 +#define BITS_FS_16G 0x18 +#define BITS_FS_MASK 0x18 +#define BITS_DLPF_CFG_256HZ 0x00 +#define BITS_DLPF_CFG_188HZ 0x01 +#define BITS_DLPF_CFG_98HZ 0x02 +#define BITS_DLPF_CFG_42HZ 0x03 +#define BITS_DLPF_CFG_20HZ 0x04 +#define BITS_DLPF_CFG_10HZ 0x05 +#define BITS_DLPF_CFG_5HZ 0x06 +#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 +#define BITS_DLPF_CFG_MASK 0x07 +#define BIT_INT_ANYRD_2CLEAR 0x10 +#define BIT_RAW_RDY_EN 0x01 +#define BIT_I2C_IF_DIS 0x10 +#define BIT_INT_STATUS_DATA 0x01 +#define BIT_GYRO 3 +#define BIT_ACC 2 +#define BIT_TEMP 1 + +// Product ID Description for MPU6000 +// high 4 bits low 4 bits +// Product Name Product Revision +#define MPU6000ES_REV_C4 0x14 +#define MPU6000ES_REV_C5 0x15 +#define MPU6000ES_REV_D6 0x16 +#define MPU6000ES_REV_D7 0x17 +#define MPU6000ES_REV_D8 0x18 +#define MPU6000_REV_C4 0x54 +#define MPU6000_REV_C5 0x55 +#define MPU6000_REV_D6 0x56 +#define MPU6000_REV_D7 0x57 +#define MPU6000_REV_D8 0x58 +#define MPU6000_REV_D9 0x59 +#define MPU6000_REV_D10 0x5A + +#ifdef CC3D + +#define MPU6000_CS_GPIO GPIOA +#define MPU6000_CS_PIN GPIO_Pin_4 + +#define MPU6000_SPI_INSTANCE SPI1 +#endif + +#define DISABLE_MPU6000 GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN) +#define ENABLE_MPU6000 GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN) + +void mpu6000SpiGyroRead(int16_t *gyroData); +void mpu6000SpiAccRead(int16_t *gyroData); + +void mpu6000SpiGyroInit(void) +{ +} + +void mpu6000SpiAccInit(void) +{ + acc_1G = 512 * 8; +} + +bool mpu6000SpiDetect(void) +{ + // FIXME this isn't working, not debugged yet. + return true; // just assume it's there for now +#if 0 + uint8_t product; + + spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); + + ENABLE_MPU6000; + spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_PRODUCT_ID); + spiTransfer(MPU6000_SPI_INSTANCE, &product, NULL, 1); + DISABLE_MPU6000; + + + + + /* look for a product ID we recognise */ + + // verify product revision + switch (product) { + case MPU6000ES_REV_C4: + case MPU6000ES_REV_C5: + case MPU6000_REV_C4: + case MPU6000_REV_C5: + case MPU6000ES_REV_D6: + case MPU6000ES_REV_D7: + case MPU6000ES_REV_D8: + case MPU6000_REV_D6: + case MPU6000_REV_D7: + case MPU6000_REV_D8: + case MPU6000_REV_D9: + case MPU6000_REV_D10: + return true; + } + + return false; +#endif +} + +static bool initDone = false; + +void mpu6000AccAndGyroInit() { + + if (initDone) { + return; + } + + spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); + + ENABLE_MPU6000; + spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_PWR_MGMT_1); // Device Reset + spiTransferByte(MPU6000_SPI_INSTANCE, BIT_H_RESET); + DISABLE_MPU6000; + + delay(150); + + ENABLE_MPU6000; + spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_SIGNAL_PATH_RESET); // Device Reset + spiTransferByte(MPU6000_SPI_INSTANCE, BIT_GYRO | BIT_ACC | BIT_TEMP); + DISABLE_MPU6000; + + delay(150); + + ENABLE_MPU6000; + spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_PWR_MGMT_1); // Clock Source PPL with Z axis gyro reference + spiTransferByte(MPU6000_SPI_INSTANCE, MPU_CLK_SEL_PLLGYROZ); + DISABLE_MPU6000; + + delayMicroseconds(1); + + ENABLE_MPU6000; + spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_USER_CTRL); // Disable Primary I2C Interface + spiTransferByte(MPU6000_SPI_INSTANCE, BIT_I2C_IF_DIS); + DISABLE_MPU6000; + + delayMicroseconds(1); + + ENABLE_MPU6000; + spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_PWR_MGMT_2); + spiTransferByte(MPU6000_SPI_INSTANCE, 0x00); + DISABLE_MPU6000; + + delayMicroseconds(1); + + ENABLE_MPU6000; + spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_SMPLRT_DIV); // Accel Sample Rate 1kHz + spiTransferByte(MPU6000_SPI_INSTANCE, 0x00); // Gyroscope Output Rate = 1kHz when the DLPF is enabled + DISABLE_MPU6000; + + delayMicroseconds(1); + + ENABLE_MPU6000; + spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_ACCEL_CONFIG); // Accel +/- 8 G Full Scale + spiTransferByte(MPU6000_SPI_INSTANCE, BITS_FS_8G); + DISABLE_MPU6000; + + delayMicroseconds(1); + + ENABLE_MPU6000; + spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_GYRO_CONFIG); // Gyro +/- 1000 DPS Full Scale + spiTransferByte(MPU6000_SPI_INSTANCE, BITS_FS_2000DPS); + DISABLE_MPU6000; + + initDone = true; +} + +bool mpu6000SpiAccDetect(acc_t *acc) +{ + if (!mpu6000SpiDetect()) { + return false; + } + + spiResetErrorCounter(MPU6000_SPI_INSTANCE); + + mpu6000AccAndGyroInit(); + + acc->init = mpu6000SpiAccInit; + acc->read = mpu6000SpiAccRead; + + delay(100); + return true; +} + +bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf) +{ + if (!mpu6000SpiDetect()) { + return false; + } + + spiResetErrorCounter(MPU6000_SPI_INSTANCE); + + mpu6000AccAndGyroInit(); + + uint8_t mpuLowPassFilter = BITS_DLPF_CFG_42HZ; + int16_t data[3]; + + // default lpf is 42Hz + switch (lpf) { + case 256: + mpuLowPassFilter = BITS_DLPF_CFG_256HZ; + break; + case 188: + mpuLowPassFilter = BITS_DLPF_CFG_188HZ; + break; + case 98: + mpuLowPassFilter = BITS_DLPF_CFG_98HZ; + break; + default: + case 42: + mpuLowPassFilter = BITS_DLPF_CFG_42HZ; + break; + case 20: + mpuLowPassFilter = BITS_DLPF_CFG_20HZ; + break; + case 10: + mpuLowPassFilter = BITS_DLPF_CFG_10HZ; + break; + case 5: + mpuLowPassFilter = BITS_DLPF_CFG_5HZ; + case 0: + mpuLowPassFilter = BITS_DLPF_CFG_2100HZ_NOLPF; + break; + } + + spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); + + ENABLE_MPU6000; + spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_CONFIG); // Accel and Gyro DLPF Setting + spiTransferByte(MPU6000_SPI_INSTANCE, mpuLowPassFilter); + DISABLE_MPU6000; + + delayMicroseconds(1); + + mpu6000SpiGyroRead(data); + + if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) { + spiResetErrorCounter(MPU6000_SPI_INSTANCE); + return false; + } + gyro->init = mpu6000SpiGyroInit; + gyro->read = mpu6000SpiGyroRead; + // 16.4 dps/lsb scalefactor + gyro->scale = 1.0f / 16.4f; + //gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f; + delay(100); + return true; +} + +void mpu6000SpiGyroRead(int16_t *gyroData) +{ + uint8_t buf[6]; + spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock + + ENABLE_MPU6000; + spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_GYRO_XOUT_H | 0x80); + spiTransfer(MPU6000_SPI_INSTANCE, buf, NULL, 6); + DISABLE_MPU6000; + + gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]); + gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]); + gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]); +} + +void mpu6000SpiAccRead(int16_t *gyroData) +{ + uint8_t buf[6]; + spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock + + ENABLE_MPU6000; + spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_ACCEL_XOUT_H | 0x80); + spiTransfer(MPU6000_SPI_INSTANCE, buf, NULL, 6); + DISABLE_MPU6000; + + gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]); + gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]); + gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]); +} diff --git a/src/main/drivers/accgyro_spi_mpu6000.h b/src/main/drivers/accgyro_spi_mpu6000.h new file mode 100644 index 000000000..519a28d6b --- /dev/null +++ b/src/main/drivers/accgyro_spi_mpu6000.h @@ -0,0 +1,18 @@ + +#pragma once + +#define MPU6000_CONFIG 0x1A + +#define BITS_DLPF_CFG_256HZ 0x00 +#define BITS_DLPF_CFG_188HZ 0x01 +#define BITS_DLPF_CFG_98HZ 0x02 +#define BITS_DLPF_CFG_42HZ 0x03 + +#define GYRO_SCALE_FACTOR 0.00053292f // (4/131) * pi/180 (32.75 LSB = 1 DPS) + + +bool mpu6000SpiAccDetect(acc_t *acc); +bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf); + +void mpu6000SpiGyroRead(int16_t *gyroData); +void mpu6000SpiAccRead(int16_t *gyroData); diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index 07094721d..77cb80816 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -39,6 +39,10 @@ extern adc_config_t adcConfig[ADC_CHANNEL_COUNT]; extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT]; +#ifdef CC3D +void adcInit(drv_adc_config_t *init) { +} +#else void adcInit(drv_adc_config_t *init) { ADC_InitTypeDef adc; @@ -148,3 +152,4 @@ void adcInit(drv_adc_config_t *init) ADC_SoftwareStartConvCmd(ADC1, ENABLE); } +#endif diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index b1c5696ae..564b26bfd 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -24,16 +24,70 @@ #include "bus_spi.h" -// SPI2 Driver -// PB15 28 SPI2_MOSI -// PB14 27 SPI2_MISO -// PB13 26 SPI2_SCK -// PB12 25 SPI2_NSS +static volatile uint16_t spi1ErrorCount = 0; +static volatile uint16_t spi2ErrorCount = 0; -static bool spiTest(void); - -bool spiInit(void) +void initSpi1(void) { +#ifdef CC3D + GPIO_PinRemapConfig(GPIO_PartialRemap_TIM3, ENABLE); +#endif + + // Specific to the STM32F103 + // SPI1 Driver + // PA7 17 SPI1_MOSI + // PA6 16 SPI1_MISO + // PA5 15 SPI1_SCK + // PA4 14 SPI1_NSS + + gpio_config_t gpio; + SPI_InitTypeDef spi; + + // Enable SPI1 clock + RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE); + RCC_APB2PeriphResetCmd(RCC_APB2Periph_SPI1, ENABLE); + + // MOSI + SCK as output + gpio.mode = Mode_AF_PP; + gpio.pin = Pin_7 | Pin_5; + gpio.speed = Speed_50MHz; + gpioInit(GPIOA, &gpio); + // MISO as input + gpio.pin = Pin_6; + gpio.mode = Mode_IN_FLOATING; + gpioInit(GPIOA, &gpio); + // NSS as gpio slave select + gpio.pin = Pin_4; + gpio.mode = Mode_Out_PP; + gpioInit(GPIOA, &gpio); + + // Init SPI2 hardware + SPI_I2S_DeInit(SPI1); + + spi.SPI_Mode = SPI_Mode_Master; + spi.SPI_Direction = SPI_Direction_2Lines_FullDuplex; + spi.SPI_DataSize = SPI_DataSize_8b; + spi.SPI_NSS = SPI_NSS_Soft; + spi.SPI_FirstBit = SPI_FirstBit_MSB; + spi.SPI_CRCPolynomial = 7; + spi.SPI_CPOL = SPI_CPOL_High; + spi.SPI_CPHA = SPI_CPHA_2Edge; + spi.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; + + SPI_Init(SPI1, &spi); + SPI_Cmd(SPI1, ENABLE); +} + +void initSpi2(void) +{ + + // Specific to the STM32F103 + // SPI2 Driver + // PB15 28 SPI2_MOSI + // PB14 27 SPI2_MISO + // PB13 26 SPI2_SCK + // PB12 25 SPI2_NSS + gpio_config_t gpio; SPI_InitTypeDef spi; @@ -57,6 +111,7 @@ bool spiInit(void) // Init SPI2 hardware SPI_I2S_DeInit(SPI2); + spi.SPI_Mode = SPI_Mode_Master; spi.SPI_Direction = SPI_Direction_2Lines_FullDuplex; spi.SPI_DataSize = SPI_DataSize_8b; @@ -66,52 +121,101 @@ bool spiInit(void) spi.SPI_CPOL = SPI_CPOL_High; spi.SPI_CPHA = SPI_CPHA_2Edge; spi.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; + SPI_Init(SPI2, &spi); SPI_Cmd(SPI2, ENABLE); - - return spiTest(); } -void spiSelect(bool select) +bool spiInit(SPI_TypeDef *instance) { - if (select) { - digitalLo(GPIOB, Pin_12); + if (instance == SPI1) { + initSpi1(); + } else if (instance == SPI2) { + initSpi2(); } else { - digitalHi(GPIOB, Pin_12); + return false; } + + return true; } -uint8_t spiTransferByte(uint8_t in) +uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance) +{ + if (instance == SPI1) { + spi1ErrorCount++; + } else if (instance == SPI2) { + spi2ErrorCount++; + } + return -1; +} + +// return uint8_t value or -1 when failure +uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t data) +{ + uint16_t spiTimeout = 1000; + + while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) + if ((spiTimeout--) == 0) + return spiTimeoutUserCallback(instance); + +#ifdef STM32F303xC + SPI_SendData8(instance, data); +#endif +#ifdef STM32F10X_MD + SPI_I2S_SendData(instance, data); +#endif + spiTimeout = 1000; + while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) + if ((spiTimeout--) == 0) + return spiTimeoutUserCallback(instance); + +#ifdef STM32F303xC + return ((uint8_t)SPI_ReceiveData8(instance)); +#endif +#ifdef STM32F10X_MD + return ((uint8_t)SPI_I2S_ReceiveData(instance)); +#endif + } +/* +uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in) { uint8_t rx; - SPI2->DR; - SPI2->DR = in; - while (!(SPI2->SR & SPI_I2S_FLAG_RXNE)); - rx = SPI2->DR; - while (!(SPI2->SR & SPI_I2S_FLAG_TXE)); - while (SPI2->SR & SPI_I2S_FLAG_BSY); + instance->DR; + instance->DR = in; + while (!(instance->SR & SPI_I2S_FLAG_RXNE)); + rx = instance->DR; + while (!(instance->SR & SPI_I2S_FLAG_TXE)); + while (instance->SR & SPI_I2S_FLAG_BSY); return rx; } - -bool spiTransfer(uint8_t *out, uint8_t *in, int len) +*/ +bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, uint8_t *in, int len) { + uint16_t spiTimeout = 1000; + uint8_t b; - SPI2->DR; + instance->DR; while (len--) { b = in ? *(in++) : 0xFF; - while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_TXE) == RESET); + while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) { + if ((spiTimeout--) == 0) + return spiTimeoutUserCallback(instance); + } #ifdef STM32F303xC - SPI_I2S_SendData16(SPI2, b); + SPI_I2S_SendData16(instance, b); #endif #ifdef STM32F10X_MD - SPI_I2S_SendData(SPI2, b); + SPI_I2S_SendData(instance, b); #endif - while (SPI_I2S_GetFlagStatus(SPI2, SPI_I2S_FLAG_RXNE) == RESET); + while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) { + if ((spiTimeout--) == 0) + return spiTimeoutUserCallback(instance); + } #ifdef STM32F303xC - b = SPI_I2S_ReceiveData16(SPI2); + b = SPI_I2S_ReceiveData16(instance); #endif #ifdef STM32F10X_MD - b = SPI_I2S_ReceiveData(SPI2); + b = SPI_I2S_ReceiveData(instance); #endif if (out) *(out++) = b; @@ -120,16 +224,80 @@ bool spiTransfer(uint8_t *out, uint8_t *in, int len) return true; } -static bool spiTest(void) + +void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor) { - uint8_t out[] = { 0x9F, 0, 0, 0 }; - uint8_t in[4]; +#define BR_CLEAR_MASK 0xFFC7 - spiSelect(true); - spiTransfer(in, out, sizeof(out)); - spiSelect(false); - if (in[1] == 0xEF) - return true; + uint16_t tempRegister; - return false; + SPI_Cmd(instance, DISABLE); + + tempRegister = instance->CR1; + + switch (divisor) { + case 2: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_2; + break; + + case 4: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_4; + break; + + case 8: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_8; + break; + + case 16: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_16; + break; + + case 32: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_32; + break; + + case 64: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_64; + break; + + case 128: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_128; + break; + + case 256: + tempRegister &= BR_CLEAR_MASK; + tempRegister |= SPI_BaudRatePrescaler_256; + break; + } + + instance->CR1 = tempRegister; + + SPI_Cmd(instance, ENABLE); } + +uint16_t spiGetErrorCounter(SPI_TypeDef *instance) +{ + if (instance == SPI1) { + return spi1ErrorCount; + } else if (instance == SPI2) { + return spi2ErrorCount; + } + return 0; +} + +void spiResetErrorCounter(SPI_TypeDef *instance) +{ + if (instance == SPI1) { + spi1ErrorCount = 0; + } else if (instance == SPI2) { + spi2ErrorCount = 0; + } +} + diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index bc3c80ab4..263bd6234 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -17,4 +17,14 @@ #pragma once -bool spiInit(void); +#define SPI_0_5625MHZ_CLOCK_DIVIDER 128 +#define SPI_18MHZ_CLOCK_DIVIDER 2 + +bool spiInit(SPI_TypeDef *instance); +void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor); +uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in); + +bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, uint8_t *in, int len); + +uint16_t spiGetErrorCounter(SPI_TypeDef *instance); +void spiResetErrorCounter(SPI_TypeDef *instance); diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c index c8c6f839e..3f342e1e3 100755 --- a/src/main/drivers/system.c +++ b/src/main/drivers/system.c @@ -121,7 +121,11 @@ void systemInit(bool overclock) // Configure the rest of the stuff i2cInit(I2C2); - spiInit(); + +#ifdef CC3D + spiInit(SPI1); + spiInit(SPI2); +#endif // sleep for 100ms delay(100); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3bced3fbc..3a5505eb6 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -112,7 +112,7 @@ static const char * const sensorNames[] = { }; static const char * const accNames[] = { - "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "FAKE", "None", NULL + "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "FAKE", "None", NULL }; typedef struct { diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index defb5f1a7..279250722 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -25,8 +25,9 @@ typedef enum AccelSensors { ACC_MMA8452 = 3, ACC_BMA280 = 4, ACC_LSM303DLHC = 5, - ACC_FAKE = 6, - ACC_NONE = 7 + ACC_SPI_MPU6000 = 6, + ACC_FAKE = 7, + ACC_NONE = 8 } AccelSensors; extern uint8_t accHardware; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index ea081c32c..4721bac29 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -33,6 +33,9 @@ #include "drivers/accgyro_l3gd20.h" #include "drivers/accgyro_lsm303dlhc.h" #endif +#ifdef CC3D +#include "drivers/accgyro_spi_mpu6000.h" +#endif #include "drivers/barometer.h" #include "drivers/barometer_bmp085.h" @@ -51,11 +54,13 @@ #include "sensors/compass.h" #include "sensors/sonar.h" + // Use these to help with porting to new boards //#define USE_FAKE_GYRO #define USE_GYRO_L3G4200D #define USE_GYRO_L3GD20 #define USE_GYRO_MPU6050 +#define USE_GYRO_SPI_MPU6000 #define USE_GYRO_MPU3050 //#define USE_FAKE_ACC #define USE_ACC_ADXL345 @@ -63,11 +68,14 @@ #define USE_ACC_MMA8452 #define USE_ACC_LSM303DLHC #define USE_ACC_MPU6050 +#define USE_ACC_SPI_MPU6000 #define USE_BARO_MS5611 #define USE_BARO_BMP085 #ifdef NAZE #undef USE_ACC_LSM303DLHC +#undef USE_ACC_SPI_MPU6000 +#undef USE_GYRO_SPI_MPU6000 #undef USE_GYRO_L3GD20 #endif @@ -80,20 +88,24 @@ #undef USE_ACC_MMA8452 #undef USE_ACC_LSM303DLHC #undef USE_ACC_MPU6050 +#undef USE_ACC_SPI_MPU6000 #undef USE_GYRO_L3G4200D #undef USE_GYRO_L3GD20 #undef USE_GYRO_MPU3050 #undef USE_GYRO_MPU6050 +#undef USE_GYRO_SPI_MPU6000 #endif #if defined(OLIMEXINO) #undef USE_GYRO_L3GD20 #undef USE_GYRO_L3G4200D #undef USE_GYRO_MPU3050 +#undef USE_GYRO_SPI_MPU6000 #undef USE_ACC_LSM303DLHC #undef USE_ACC_BMA280 #undef USE_ACC_MMA8452 #undef USE_ACC_ADXL345 +#undef USE_ACC_SPI_MPU6000 #undef USE_BARO_MS5611 #endif @@ -101,15 +113,15 @@ #undef USE_GYRO_L3G4200D #undef USE_GYRO_MPU6050 #undef USE_GYRO_MPU3050 +#undef USE_GYRO_SPI_MPU6000 #undef USE_ACC_ADXL345 #undef USE_ACC_BMA280 #undef USE_ACC_MPU6050 #undef USE_ACC_MMA8452 +#undef USE_ACC_SPI_MPU6000 #endif #ifdef CC3D -#define USE_FAKE_GYRO -#define USE_FAKE_ACC #undef USE_GYRO_L3GD20 #undef USE_GYRO_L3G4200D #undef USE_GYRO_MPU6050 @@ -197,6 +209,12 @@ bool detectGyro(uint16_t gyroLpf) return true; } #endif + +#ifdef USE_GYRO_SPI_MPU6000 + if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) { + return true; + } +#endif return false; } @@ -270,6 +288,7 @@ retry: if (accHardwareToUse == ACC_BMA280) break; } + ; // fallthrough #endif #ifdef USE_ACC_LSM303DLHC case ACC_LSM303DLHC: @@ -278,8 +297,18 @@ retry: if (accHardwareToUse == ACC_LSM303DLHC) break; } + ; // fallthrough #endif - ; +#ifdef USE_GYRO_SPI_MPU6000 + case ACC_SPI_MPU6000: + if (mpu6000SpiAccDetect(&acc)) { + accHardware = ACC_SPI_MPU6000; + if (accHardwareToUse == ACC_SPI_MPU6000) + break; + } + ; // fallthrough +#endif + ; // prevent compiler error } // Found anything? Check if user fucked up or ACC is really missing. diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index b6da43250..30fd5e3a6 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -23,4 +23,4 @@ #define ACC #define GYRO -#define SENSORS_SET (SENSOR_ACC | SENSOR_MAG) +#define SENSORS_SET (SENSOR_ACC)