CC3D - Support MPU6000 Gyro/Acc and SPI 1/2.
This commit is contained in:
Dominic Clifton
parent
1fb75890d5
commit
5f16cfb72a
1
Makefile
1
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 \
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
* Authors:
|
||||
* Dominic Clifton - Cleanflight implementation
|
||||
* John Ihlein - Initial FF32 code
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#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]);
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -23,4 +23,4 @@
|
|||
#define ACC
|
||||
#define GYRO
|
||||
|
||||
#define SENSORS_SET (SENSOR_ACC | SENSOR_MAG)
|
||||
#define SENSORS_SET (SENSOR_ACC)
|
||||
|
|
|
|||
Loading…
Reference in New Issue