Skip to content

Commit

Permalink
Configurable acc/gyro
Browse files Browse the repository at this point in the history
  • Loading branch information
@ezshinoda
ezshinoda committed Sep 10, 2018
1 parent 5ef68ef commit fc6c24c
Show file tree
Hide file tree
Showing 133 changed files with 1,083 additions and 1,104 deletions.
4 changes: 0 additions & 4 deletions src/main/drivers/accgyro/accgyro.h
View file
Expand Up @@ -35,10 +35,6 @@
#pragma GCC diagnostic warning "-Wpadded"
#endif

#ifndef MPU_I2C_INSTANCE
#define MPU_I2C_INSTANCE I2C_DEVICE
#endif

typedef enum {
GYRO_NONE = 0,
GYRO_DEFAULT,
Expand Down
152 changes: 55 additions & 97 deletions src/main/drivers/accgyro/accgyro_mpu.c
View file
Expand Up @@ -54,17 +54,16 @@
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
#include "drivers/accgyro/accgyro_mpu.h"

#ifndef MPU_I2C_INSTANCE
#define MPU_I2C_INSTANCE I2C_DEVICE
#endif
#include "pg/pg.h"
#include "pg/gyrodev.h"

#ifndef MPU_ADDRESS
#define MPU_ADDRESS 0x68
#endif

#define MPU_INQUIRY_MASK 0x7E

#ifdef USE_I2C
#ifdef USE_I2C_GYRO
static void mpu6050FindRevision(gyroDev_t *gyro)
{
// There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code
Expand Down Expand Up @@ -103,7 +102,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
/*
* Gyro interrupt service routine
*/
#if defined(MPU_INT_EXTI)
#ifdef USE_GYRO_EXTI
static void mpuIntExtiHandler(extiCallbackRec_t *cb)
{
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
Expand Down Expand Up @@ -136,11 +135,11 @@ static void mpuIntExtiInit(gyroDev_t *gyro)
#endif

#if defined (STM32F7)
IOInit(mpuIntIO, OWNER_MPU_EXTI, 0);
IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0);
EXTIHandlerInit(&gyro->exti, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ?
#else
IOInit(mpuIntIO, OWNER_MPU_EXTI, 0);
IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0);
IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?

EXTIHandlerInit(&gyro->exti, mpuIntExtiHandler);
Expand Down Expand Up @@ -182,6 +181,7 @@ bool mpuGyroRead(gyroDev_t *gyro)
return true;
}

#ifdef USE_SPI_GYRO
bool mpuGyroReadSPI(gyroDev_t *gyro)
{
static const uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
Expand All @@ -199,120 +199,78 @@ bool mpuGyroReadSPI(gyroDev_t *gyro)
return true;
}

#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
{
UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI

uint8_t sensor = MPU_NONE;
UNUSED(sensor);

// note, when USE_DUAL_GYRO is enabled the gyro->bus must already be initialised.
typedef uint8_t (*gyroSpiDetectFn_t)(const busDevice_t *bus);

static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
#ifdef USE_GYRO_SPI_MPU6000
#ifndef USE_DUAL_GYRO
spiBusSetInstance(&gyro->bus, MPU6000_SPI_INSTANCE);
#endif
#ifdef MPU6000_CS_PIN
gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin;
mpu6000SpiDetect,
#endif
sensor = mpu6000SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
return true;
}
#endif

#ifdef USE_GYRO_SPI_MPU6500
#ifndef USE_DUAL_GYRO
spiBusSetInstance(&gyro->bus, MPU6500_SPI_INSTANCE);
mpu6500SpiDetect, // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
#endif
#ifdef MPU6500_CS_PIN
gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin;
#endif
sensor = mpu6500SpiDetect(&gyro->bus);
// some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
return true;
}
#endif

#ifdef USE_GYRO_SPI_MPU9250
#ifndef USE_DUAL_GYRO
spiBusSetInstance(&gyro->bus, MPU9250_SPI_INSTANCE);
#endif
#ifdef MPU9250_CS_PIN
gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin;
#endif
sensor = mpu9250SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
return true;
}
mpu9250SpiDetect,
#endif

#ifdef USE_GYRO_SPI_ICM20649
#ifdef ICM20649_SPI_INSTANCE
spiBusSetInstance(&gyro->bus, ICM20649_SPI_INSTANCE);
#endif
#ifdef ICM20649_CS_PIN
gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20649_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin;
icm20649SpiDetect,
#endif
sensor = icm20649SpiDetect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
return true;
}
#endif

#ifdef USE_GYRO_SPI_ICM20689
#ifndef USE_DUAL_GYRO
spiBusSetInstance(&gyro->bus, ICM20689_SPI_INSTANCE);
icm20689SpiDetect, // icm20689SpiDetect detects ICM20602 and ICM20689
#endif
#ifdef ICM20689_CS_PIN
gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin;
#endif
sensor = icm20689SpiDetect(&gyro->bus);
// icm20689SpiDetect detects ICM20602 and ICM20689
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
return true;
}
#endif

#ifdef USE_ACCGYRO_BMI160
#ifndef USE_DUAL_GYRO
spiBusSetInstance(&gyro->bus, BMI160_SPI_INSTANCE);
#endif
#ifdef BMI160_CS_PIN
gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI160_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin;
bmi160Detect,
#endif
sensor = bmi160Detect(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
return true;
NULL // Avoid an empty array
};

static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
{
gyro->bus.bustype = BUSTYPE_SPI;

spiBusSetInstance(&gyro->bus, spiInstanceByDevice(SPI_CFG_TO_DEV(config->spiBus)));
gyro->bus.busdev_u.spi.csnPin = IOGetByTag(config->csnTag);
IOInit(gyro->bus.busdev_u.spi.csnPin, OWNER_GYRO_CS, RESOURCE_INDEX(config->index));
IOConfigGPIO(gyro->bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(gyro->bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus.

uint8_t sensor = MPU_NONE;

// It is hard to use hardware to optimize the detection loop here,
// as hardware type and detection function name doesn't match.
// May need a bitmap of hardware to detection function to do it right?

for (size_t index = 0 ; gyroSpiDetectFnTable[index] ; index++) {
sensor = (gyroSpiDetectFnTable[index])(&gyro->bus);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
return true;
}
}
#endif

spiPreinitCsByTag(config->csnTag);

return false;
}
#endif

void mpuDetect(gyroDev_t *gyro)
void mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
{
// MPU datasheet specifies 30ms.
delay(35);

#ifdef USE_I2C
if (gyro->bus.bustype == BUSTYPE_NONE) {
// if no bustype is selected try I2C first.
if (config->bustype == BUSTYPE_NONE) {
return;
}

if (config->bustype == BUSTYPE_GYRO_AUTO) {
gyro->bus.bustype = BUSTYPE_I2C;
} else {
gyro->bus.bustype = config->bustype;
}

#ifdef USE_I2C_GYRO
if (gyro->bus.bustype == BUSTYPE_I2C) {
gyro->bus.busdev_u.i2c.device = MPU_I2C_INSTANCE;
gyro->bus.busdev_u.i2c.address = MPU_ADDRESS;
gyro->bus.busdev_u.i2c.address = config->i2cAddress ? config->i2cAddress : MPU_ADDRESS;

uint8_t sig = 0;
bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1);
Expand All @@ -339,15 +297,15 @@ void mpuDetect(gyroDev_t *gyro)
}
#endif

#ifdef USE_SPI
#ifdef USE_SPI_GYRO
gyro->bus.bustype = BUSTYPE_SPI;
detectSPISensorsAndUpdateDetectionResult(gyro);
detectSPISensorsAndUpdateDetectionResult(gyro, config);
#endif
}

void mpuGyroInit(gyroDev_t *gyro)
{
#ifdef MPU_INT_EXTI
#ifdef USE_GYRO_EXTI
mpuIntExtiInit(gyro);
#else
UNUSED(gyro);
Expand Down
3 changes: 2 additions & 1 deletion src/main/drivers/accgyro/accgyro_mpu.h
View file
Expand Up @@ -218,10 +218,11 @@ typedef struct mpuDetectionResult_s {
} mpuDetectionResult_t;

struct gyroDev_s;
struct gyroDeviceConfig_s;
void mpuGyroInit(struct gyroDev_s *gyro);
bool mpuGyroRead(struct gyroDev_s *gyro);
bool mpuGyroReadSPI(struct gyroDev_s *gyro);
void mpuDetect(struct gyroDev_s *gyro);
void mpuDetect(struct gyroDev_s *gyro, const struct gyroDeviceConfig_s *config);
uint8_t mpuGyroDLPF(struct gyroDev_s *gyro);
uint8_t mpuGyroFCHOICE(struct gyroDev_s *gyro);
uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg);
Expand Down
15 changes: 3 additions & 12 deletions src/main/drivers/accgyro/accgyro_spi_bmi160.c
View file
Expand Up @@ -97,11 +97,6 @@ uint8_t bmi160Detect(const busDevice_t *bus)
return BMI_160_SPI;
}

#ifndef USE_DUAL_GYRO
IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->busdev_u.spi.csnPin);
#endif

spiSetDivisor(bus->busdev_u.spi.instance, BMI160_SPI_DIVISOR);

Expand Down Expand Up @@ -250,22 +245,18 @@ void bmi160ExtiHandler(extiCallbackRec_t *cb)

static void bmi160IntExtiInit(gyroDev_t *gyro)
{
static bool bmi160ExtiInitDone = false;

if (bmi160ExtiInitDone) {
if (gyro->mpuIntExtiTag == IO_TAG_NONE) {
return;
}

IO_t mpuIntIO = IOGetByTag(IO_TAG(BMI160_INT_EXTI));
IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag);

IOInit(mpuIntIO, OWNER_MPU_EXTI, 0);
IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0);
IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?

EXTIHandlerInit(&gyro->exti, bmi160ExtiHandler);
EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
EXTIEnable(mpuIntIO, true);

bmi160ExtiInitDone = true;
}


Expand Down
5 changes: 0 additions & 5 deletions src/main/drivers/accgyro/accgyro_spi_icm20649.c
View file
Expand Up @@ -45,11 +45,6 @@ static void icm20649SpiInit(const busDevice_t *bus)
return;
}

#ifndef USE_DUAL_GYRO
IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->busdev_u.spi.csnPin);
#endif

// all registers can be read/written at full speed (7MHz +-10%)
// TODO verify that this works at 9MHz and 10MHz on non F7
Expand Down
5 changes: 0 additions & 5 deletions src/main/drivers/accgyro/accgyro_spi_icm20689.c
View file
Expand Up @@ -45,11 +45,6 @@ static void icm20689SpiInit(const busDevice_t *bus)
return;
}

#ifndef USE_DUAL_GYRO
IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->busdev_u.spi.csnPin);
#endif

spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD);

Expand Down
5 changes: 0 additions & 5 deletions src/main/drivers/accgyro/accgyro_spi_mpu6000.c
View file
Expand Up @@ -127,11 +127,6 @@ void mpu6000SpiAccInit(accDev_t *acc)

uint8_t mpu6000SpiDetect(const busDevice_t *bus)
{
#ifndef USE_DUAL_GYRO
IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->busdev_u.spi.csnPin);
#endif

spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON);

Expand Down
5 changes: 0 additions & 5 deletions src/main/drivers/accgyro/accgyro_spi_mpu6500.c
View file
Expand Up @@ -41,11 +41,6 @@

static void mpu6500SpiInit(const busDevice_t *bus)
{
#ifndef USE_DUAL_GYRO
IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->busdev_u.spi.csnPin);
#endif

spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_FAST);
}
Expand Down
5 changes: 0 additions & 5 deletions src/main/drivers/accgyro/accgyro_spi_mpu9250.c
View file
Expand Up @@ -151,11 +151,6 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {

uint8_t mpu9250SpiDetect(const busDevice_t *bus)
{
#ifndef USE_DUAL_GYRO
IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->busdev_u.spi.csnPin);
#endif

spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATON); //low speed
mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
Expand Down
6 changes: 0 additions & 6 deletions src/main/drivers/accgyro_legacy/accgyro_l3gd20.c
View file
Expand Up @@ -82,12 +82,6 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
UNUSED(SPIx); // FIXME

mpul3gd20CsPin = IOGetByTag(IO_TAG(L3GD20_CS_PIN));
#ifndef USE_DUAL_GYRO
IOInit(mpul3gd20CsPin, OWNER_MPU_CS, 0);
IOConfigGPIO(mpul3gd20CsPin, SPI_IO_CS_CFG);

DISABLE_L3GD20;
#endif

spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD);
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/drivers/accgyro_legacy/accgyro_mma845x.c
View file
Expand Up @@ -86,7 +86,7 @@ static uint8_t device_id;
static inline void mma8451ConfigureInterrupt(void)
{
#ifdef MMA8451_INT_PIN
IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_MPU_EXTI, 0);
IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_GYRO_EXTI, 0);
// TODO - maybe pullup / pulldown ?
IOConfigGPIO(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), IOCFG_IN_FLOATING);
#endif
Expand Down
3 changes: 2 additions & 1 deletion src/main/drivers/bus.h
View file
Expand Up @@ -29,7 +29,8 @@ typedef enum {
BUSTYPE_NONE = 0,
BUSTYPE_I2C,
BUSTYPE_SPI,
BUSTYPE_MPU_SLAVE // Slave I2C on SPI master
BUSTYPE_MPU_SLAVE, // Slave I2C on SPI master
BUSTYPE_GYRO_AUTO // Only used by acc/gyro bus auto detection code
} busType_e;

typedef struct busDevice_s {
Expand Down

0 comments on commit fc6c24c

Please sign in to comment.