mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 11:29:58 +03:00
Configurable acc/gyro
This commit is contained in:
parent
5ef68ef6a3
commit
fc6c24c38e
133 changed files with 1095 additions and 1116 deletions
|
@ -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,
|
||||
|
|
|
@ -54,9 +54,8 @@
|
|||
#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
|
||||
|
@ -64,7 +63,7 @@
|
|||
|
||||
#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
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
|
@ -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};
|
||||
|
@ -199,120 +199,78 @@ bool mpuGyroReadSPI(gyroDev_t *gyro)
|
|||
return true;
|
||||
}
|
||||
|
||||
#ifdef USE_SPI
|
||||
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
||||
typedef uint8_t (*gyroSpiDetectFn_t)(const busDevice_t *bus);
|
||||
|
||||
static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
|
||||
#ifdef USE_GYRO_SPI_MPU6000
|
||||
mpu6000SpiDetect,
|
||||
#endif
|
||||
#ifdef USE_GYRO_SPI_MPU6500
|
||||
mpu6500SpiDetect, // some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
|
||||
#endif
|
||||
#ifdef USE_GYRO_SPI_MPU9250
|
||||
mpu9250SpiDetect,
|
||||
#endif
|
||||
#ifdef USE_GYRO_SPI_ICM20649
|
||||
icm20649SpiDetect,
|
||||
#endif
|
||||
#ifdef USE_GYRO_SPI_ICM20689
|
||||
icm20689SpiDetect, // icm20689SpiDetect detects ICM20602 and ICM20689
|
||||
#endif
|
||||
#ifdef USE_ACCGYRO_BMI160
|
||||
bmi160Detect,
|
||||
#endif
|
||||
NULL // Avoid an empty array
|
||||
};
|
||||
|
||||
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
|
||||
{
|
||||
UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI
|
||||
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;
|
||||
UNUSED(sensor);
|
||||
|
||||
// note, when USE_DUAL_GYRO is enabled the gyro->bus must already be initialised.
|
||||
// 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?
|
||||
|
||||
#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;
|
||||
#endif
|
||||
sensor = mpu6000SpiDetect(&gyro->bus);
|
||||
for (size_t index = 0 ; gyroSpiDetectFnTable[index] ; index++) {
|
||||
sensor = (gyroSpiDetectFnTable[index])(&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);
|
||||
#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;
|
||||
}
|
||||
#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;
|
||||
#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);
|
||||
#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;
|
||||
#endif
|
||||
sensor = bmi160Detect(&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.
|
||||
gyro->bus.bustype = BUSTYPE_I2C;
|
||||
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);
|
||||
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -49,11 +49,11 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
|
|||
"SDCARD_DETECT",
|
||||
"FLASH_CS",
|
||||
"BARO_CS",
|
||||
"MPU_CS",
|
||||
"GYRO_CS",
|
||||
"OSD_CS",
|
||||
"RX_SPI_CS",
|
||||
"SPI_CS",
|
||||
"MPU_EXTI",
|
||||
"GYRO_EXTI",
|
||||
"BARO_EXTI",
|
||||
"COMPASS_EXTI",
|
||||
"USB",
|
||||
|
@ -76,4 +76,6 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
|
|||
"USB_MSC_PIN",
|
||||
"SPI_PREINIT_IPU",
|
||||
"SPI_PREINIT_OPU",
|
||||
"GYRO1_CS",
|
||||
"GYRO2_CS",
|
||||
};
|
||||
|
|
|
@ -49,11 +49,11 @@ typedef enum {
|
|||
OWNER_SDCARD_DETECT,
|
||||
OWNER_FLASH_CS,
|
||||
OWNER_BARO_CS,
|
||||
OWNER_MPU_CS,
|
||||
OWNER_GYRO_CS,
|
||||
OWNER_OSD_CS,
|
||||
OWNER_RX_SPI_CS,
|
||||
OWNER_SPI_CS,
|
||||
OWNER_MPU_EXTI,
|
||||
OWNER_GYRO_EXTI,
|
||||
OWNER_BARO_EXTI,
|
||||
OWNER_COMPASS_EXTI,
|
||||
OWNER_USB,
|
||||
|
@ -76,6 +76,8 @@ typedef enum {
|
|||
OWNER_USB_MSC_PIN,
|
||||
OWNER_SPI_PREINIT_IPU,
|
||||
OWNER_SPI_PREINIT_OPU,
|
||||
OWNER_GYRO1_CS,
|
||||
OWNER_GYRO2_CS,
|
||||
OWNER_TOTAL_COUNT
|
||||
} resourceOwner_e;
|
||||
|
||||
|
|
|
@ -126,6 +126,7 @@ extern uint8_t __config_end;
|
|||
#include "pg/board.h"
|
||||
#include "pg/bus_i2c.h"
|
||||
#include "pg/bus_spi.h"
|
||||
#include "pg/gyrodev.h"
|
||||
#include "pg/max7456.h"
|
||||
#include "pg/pinio.h"
|
||||
#include "pg/pg.h"
|
||||
|
@ -2701,7 +2702,7 @@ static void cliPrintGyroRegisters(uint8_t whichSensor)
|
|||
|
||||
static void cliDumpGyroRegisters(char *cmdline)
|
||||
{
|
||||
#ifdef USE_DUAL_GYRO
|
||||
#ifdef USE_MULTI_GYRO
|
||||
if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) {
|
||||
cliPrintLinef("\r\n# Gyro 1");
|
||||
cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1);
|
||||
|
@ -2712,7 +2713,7 @@ static void cliDumpGyroRegisters(char *cmdline)
|
|||
}
|
||||
#else
|
||||
cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1);
|
||||
#endif // USE_DUAL_GYRO
|
||||
#endif
|
||||
UNUSED(cmdline);
|
||||
}
|
||||
#endif
|
||||
|
@ -3823,6 +3824,8 @@ const cliResourceValue_t resourceTable[] = {
|
|||
#ifdef USE_RX_SPI
|
||||
DEFS( OWNER_RX_SPI_CS, PG_RX_SPI_CONFIG, rxSpiConfig_t, csnTag ),
|
||||
#endif
|
||||
#define PG_ARRAY_OFFSET(type, index, member) (index * sizeof(type) + offsetof(type, member))
|
||||
DEFW( OWNER_GYRO_CS, PG_GYRO_DEVICE_CONFIG, gyroDeviceConfig_t, csnTag, 2 ),
|
||||
};
|
||||
|
||||
#undef DEFS
|
||||
|
|
|
@ -70,6 +70,7 @@
|
|||
#include "pg/beeper_dev.h"
|
||||
#include "pg/dashboard.h"
|
||||
#include "pg/flash.h"
|
||||
#include "pg/gyrodev.h"
|
||||
#include "pg/max7456.h"
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
@ -158,7 +159,7 @@ static const char * const lookupTableAlignment[] = {
|
|||
"CW270FLIP"
|
||||
};
|
||||
|
||||
#ifdef USE_DUAL_GYRO
|
||||
#ifdef USE_MULTI_GYRO
|
||||
static const char * const lookupTableGyro[] = {
|
||||
"FIRST", "SECOND", "BOTH"
|
||||
};
|
||||
|
@ -289,7 +290,10 @@ static const char * const lookupTableFailsafeSwitchMode[] = {
|
|||
};
|
||||
|
||||
static const char * const lookupTableBusType[] = {
|
||||
"NONE", "I2C", "SPI", "SLAVE"
|
||||
"NONE", "I2C", "SPI", "SLAVE",
|
||||
#if defined(USE_SPI_GYRO) && defined(USE_I2C_GYRO)
|
||||
"GYROAUTO"
|
||||
#endif
|
||||
};
|
||||
|
||||
#ifdef USE_MAX7456
|
||||
|
@ -458,7 +462,7 @@ const lookupTableEntry_t lookupTables[] = {
|
|||
#ifdef USE_LED_STRIP
|
||||
LOOKUP_TABLE_ENTRY(lookupLedStripFormatRGB),
|
||||
#endif
|
||||
#ifdef USE_DUAL_GYRO
|
||||
#ifdef USE_MULTI_GYRO
|
||||
LOOKUP_TABLE_ENTRY(lookupTableGyro),
|
||||
#endif
|
||||
LOOKUP_TABLE_ENTRY(lookupTableThrottleLimitType),
|
||||
|
@ -485,6 +489,7 @@ const lookupTableEntry_t lookupTables[] = {
|
|||
#ifdef USE_VTX_COMMON
|
||||
LOOKUP_TABLE_ENTRY(lookupTableVtxLowPowerDisarm),
|
||||
#endif
|
||||
LOOKUP_TABLE_ENTRY(lookupTableGyroHardware),
|
||||
};
|
||||
|
||||
#undef LOOKUP_TABLE_ENTRY
|
||||
|
@ -526,7 +531,8 @@ const clivalue_t valueTable[] = {
|
|||
#if defined(GYRO_USES_SPI) && defined(USE_32K_CAPABLE_GYRO)
|
||||
{ "gyro_use_32khz", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_use_32khz) },
|
||||
#endif
|
||||
#ifdef USE_DUAL_GYRO
|
||||
#endif
|
||||
#ifdef USE_MULTI_GYRO
|
||||
{ "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
|
||||
#endif
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
|
@ -1153,6 +1159,20 @@ const clivalue_t valueTable[] = {
|
|||
{ "rcdevice_init_dev_attempts", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_RCDEVICE_CONFIG, offsetof(rcdeviceConfig_t, initDeviceAttempts) },
|
||||
{ "rcdevice_init_dev_attempt_interval", VAR_UINT32 | MASTER_VALUE, .config.minmax = { 500, 5000 }, PG_RCDEVICE_CONFIG, offsetof(rcdeviceConfig_t, initDeviceAttemptInterval) }
|
||||
#endif
|
||||
|
||||
#define PG_ARRAY_OFFSET(type, index, member) (index * sizeof(type) + offsetof(type, member))
|
||||
|
||||
// PG_GYRO_DEVICE_CONFIG
|
||||
{ "gyro_1_bustype", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_OFFSET(gyroDeviceConfig_t, 0, bustype) },
|
||||
{ "gyro_1_spibus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_OFFSET(gyroDeviceConfig_t, 0, spiBus) },
|
||||
{ "gyro_1_i2cBus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_OFFSET(gyroDeviceConfig_t, 0, i2cBus) },
|
||||
{ "gyro_1_i2c_address", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_OFFSET(gyroDeviceConfig_t, 0, i2cAddress) },
|
||||
#ifdef USE_MULTI_GYRO
|
||||
{ "gyro_2_bustype", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_OFFSET(gyroDeviceConfig_t, 1, bustype) },
|
||||
{ "gyro_2_spibus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_OFFSET(gyroDeviceConfig_t, 1, spiBus) },
|
||||
{ "gyro_2_i2cBus", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_OFFSET(gyroDeviceConfig_t, 1, i2cBus) },
|
||||
{ "gyro_2_i2c_address", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_OFFSET(gyroDeviceConfig_t, 1, i2cAddress) },
|
||||
#endif
|
||||
};
|
||||
|
||||
const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);
|
||||
|
|
|
@ -92,7 +92,7 @@ typedef enum {
|
|||
#ifdef USE_LED_STRIP
|
||||
TABLE_RGB_GRB,
|
||||
#endif
|
||||
#ifdef USE_DUAL_GYRO
|
||||
#ifdef USE_MULTI_GYRO
|
||||
TABLE_GYRO,
|
||||
#endif
|
||||
TABLE_THROTTLE_LIMIT_TYPE,
|
||||
|
@ -119,6 +119,7 @@ typedef enum {
|
|||
#ifdef USE_VTX_COMMON
|
||||
TABLE_VTX_LOW_POWER_DISARM,
|
||||
#endif
|
||||
TABLE_GYRO_HARDWARE,
|
||||
LOOKUP_TABLE_COUNT
|
||||
} lookupTableIndex_e;
|
||||
|
||||
|
|
89
src/main/pg/gyrodev.c
Normal file
89
src/main/pg/gyrodev.c
Normal file
|
@ -0,0 +1,89 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
|
||||
* 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 this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/bus_spi.h"
|
||||
|
||||
#include "pg/gyrodev.h"
|
||||
|
||||
ioTag_t selectMPUIntExtiConfigByHardwareRevision(void); // XXX Should be gone
|
||||
|
||||
#if defined(USE_SPI_GYRO) || defined(USE_I2C_GYRO)
|
||||
static void gyroResetCommonDeviceConfig(gyroDeviceConfig_t *devconf, ioTag_t extiTag, sensor_align_e align)
|
||||
{
|
||||
devconf->extiTag = extiTag;
|
||||
devconf->align = align;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_SPI_GYRO
|
||||
static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *instance, ioTag_t csnTag, ioTag_t extiTag, sensor_align_e align)
|
||||
{
|
||||
devconf->bustype = BUSTYPE_SPI;
|
||||
devconf->spiBus = SPI_DEV_TO_CFG(spiDeviceByInstance(instance));
|
||||
devconf->csnTag = csnTag;
|
||||
gyroResetCommonDeviceConfig(devconf, extiTag, align);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_I2C_GYRO
|
||||
static void gyroResetI2cDeviceConfig(gyroDeviceConfig_t *devconf, I2CDevice i2cbus, ioTag_t extiTag, sensor_align_e align)
|
||||
{
|
||||
devconf->bustype = BUSTYPE_I2C;
|
||||
devconf->i2cBus = I2C_DEV_TO_CFG(i2cbus);
|
||||
devconf->i2cAddress = GYRO_I2C_ADDRESS;
|
||||
gyroResetCommonDeviceConfig(devconf, extiTag, align);
|
||||
}
|
||||
#endif
|
||||
|
||||
PG_REGISTER_ARRAY_WITH_RESET_FN(gyroDeviceConfig_t, MAX_GYRODEV_COUNT, gyroDeviceConfig, PG_GYRO_DEVICE_CONFIG, 0);
|
||||
|
||||
void pgResetFn_gyroDeviceConfig(gyroDeviceConfig_t *devconf)
|
||||
{
|
||||
devconf[0].index = 0;
|
||||
|
||||
// All multi-gyro boards use SPI based gyros.
|
||||
#ifdef USE_SPI_GYRO
|
||||
gyroResetSpiDeviceConfig(&devconf[0], GYRO_1_SPI_INSTANCE, IO_TAG(GYRO_1_CS_PIN), IO_TAG(GYRO_1_EXTI_PIN), GYRO_1_ALIGN);
|
||||
#ifdef USE_MULTI_GYRO
|
||||
gyroResetSpiDeviceConfig(&devconf[1], GYRO_2_SPI_INSTANCE, IO_TAG(GYRO_2_CS_PIN), IO_TAG(GYRO_2_EXTI_PIN), GYRO_2_ALIGN);
|
||||
devconf[1].index = 1;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// I2C gyros appear as a sole gyro in single gyro boards.
|
||||
#if defined(USE_I2C_GYRO) && !defined(USE_MULTI_GYRO)
|
||||
devconf[0].i2cBus = I2C_DEV_TO_CFG(I2CINVALID); // XXX Not required?
|
||||
gyroResetI2cDeviceConfig(&devconf[0], I2C_DEVICE, IO_TAG(GYRO_1_EXTI_PIN), GYRO_1_ALIGN);
|
||||
#endif
|
||||
|
||||
// Special treatment for very rare F3 targets with variants having either I2C or SPI acc/gyro chip; mark it for run time detection.
|
||||
#if defined(USE_SPI_GYRO) && defined(USE_I2C_GYRO)
|
||||
devconf[0].bustype = BUSTYPE_GYRO_AUTO;
|
||||
#endif
|
||||
}
|
49
src/main/pg/gyrodev.h
Normal file
49
src/main/pg/gyrodev.h
Normal file
|
@ -0,0 +1,49 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
|
||||
* 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 this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <stdint.h>
|
||||
//#include <stdbool.h>
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "drivers/io_types.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
typedef struct gyroDeviceConfig_s {
|
||||
int8_t index;
|
||||
uint8_t bustype;
|
||||
uint8_t spiBus;
|
||||
ioTag_t csnTag;
|
||||
uint8_t i2cBus;
|
||||
uint8_t i2cAddress;
|
||||
ioTag_t extiTag;
|
||||
sensor_align_e align;
|
||||
} gyroDeviceConfig_t;
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
#define MAX_GYRODEV_COUNT 2
|
||||
#else
|
||||
#define MAX_GYRODEV_COUNT 1
|
||||
#endif
|
||||
|
||||
PG_DECLARE_ARRAY(gyroDeviceConfig_t, MAX_GYRODEV_COUNT, gyroDeviceConfig);
|
|
@ -134,7 +134,8 @@
|
|||
#define PG_RX_SPI_CONFIG 537
|
||||
#define PG_BOARD_CONFIG 538
|
||||
#define PG_RCDEVICE_CONFIG 539
|
||||
#define PG_BETAFLIGHT_END 539
|
||||
#define PG_GYRO_DEVICE_CONFIG 540
|
||||
#define PG_BETAFLIGHT_END 540
|
||||
|
||||
|
||||
// OSD configuration (subject to change)
|
||||
|
|
|
@ -347,14 +347,11 @@ bool accInit(uint32_t gyroSamplingInverval)
|
|||
acc.dev.mpuDetectionResult = *gyroMpuDetectionResult();
|
||||
acc.dev.acc_high_fsr = accelerometerConfig()->acc_high_fsr;
|
||||
|
||||
#ifdef USE_DUAL_GYRO
|
||||
acc.dev.accAlign = ACC_1_ALIGN;
|
||||
#ifdef ACC_2_ALIGN
|
||||
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) {
|
||||
acc.dev.accAlign = ACC_2_ALIGN;
|
||||
} else {
|
||||
acc.dev.accAlign = ACC_1_ALIGN;
|
||||
}
|
||||
#else
|
||||
acc.dev.accAlign = ALIGN_DEFAULT;
|
||||
#endif
|
||||
|
||||
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
|
||||
|
|
|
@ -36,6 +36,7 @@
|
|||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
#include "pg/gyrodev.h"
|
||||
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "drivers/accgyro/accgyro_fake.h"
|
||||
|
@ -77,10 +78,6 @@
|
|||
#endif
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
#include "hardware_revision.h"
|
||||
#endif
|
||||
|
||||
#if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6500)))
|
||||
#define USE_GYRO_SLEW_LIMITER
|
||||
#endif
|
||||
|
@ -152,7 +149,7 @@ typedef struct gyroSensor_s {
|
|||
} gyroSensor_t;
|
||||
|
||||
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
|
||||
#ifdef USE_DUAL_GYRO
|
||||
#ifdef USE_MULTI_GYRO
|
||||
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor2;
|
||||
#endif
|
||||
|
||||
|
@ -211,47 +208,34 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
|
|||
.dyn_filter_range = DYN_FILTER_RANGE_MEDIUM,
|
||||
);
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
#define ACTIVE_GYRO ((gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyroSensor2 : &gyroSensor1)
|
||||
#else
|
||||
#define ACTIVE_GYRO (&gyroSensor1)
|
||||
#endif
|
||||
|
||||
const busDevice_t *gyroSensorBus(void)
|
||||
{
|
||||
#ifdef USE_DUAL_GYRO
|
||||
if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
|
||||
return &gyroSensor2.gyroDev.bus;
|
||||
} else {
|
||||
return &gyroSensor1.gyroDev.bus;
|
||||
}
|
||||
#else
|
||||
return &gyroSensor1.gyroDev.bus;
|
||||
#endif
|
||||
return &ACTIVE_GYRO->gyroDev.bus;
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_REGISTER_DUMP
|
||||
const busDevice_t *gyroSensorBusByDevice(uint8_t whichSensor)
|
||||
{
|
||||
#ifdef USE_DUAL_GYRO
|
||||
#ifdef USE_MULTI_GYRO
|
||||
if (whichSensor == GYRO_CONFIG_USE_GYRO_2) {
|
||||
return &gyroSensor2.gyroDev.bus;
|
||||
} else {
|
||||
return &gyroSensor1.gyroDev.bus;
|
||||
}
|
||||
#else
|
||||
UNUSED(whichSensor);
|
||||
return &gyroSensor1.gyroDev.bus;
|
||||
#endif
|
||||
return &gyroSensor1.gyroDev.bus;
|
||||
}
|
||||
#endif // USE_GYRO_REGISTER_DUMP
|
||||
|
||||
const mpuDetectionResult_t *gyroMpuDetectionResult(void)
|
||||
{
|
||||
#ifdef USE_DUAL_GYRO
|
||||
if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
|
||||
return &gyroSensor2.gyroDev.mpuDetectionResult;
|
||||
} else {
|
||||
return &gyroSensor1.gyroDev.mpuDetectionResult;
|
||||
}
|
||||
#else
|
||||
return &gyroSensor1.gyroDev.mpuDetectionResult;
|
||||
#endif
|
||||
return &ACTIVE_GYRO->gyroDev.mpuDetectionResult;
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
||||
|
@ -266,9 +250,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
|||
case GYRO_MPU6050:
|
||||
if (mpu6050GyroDetect(dev)) {
|
||||
gyroHardware = GYRO_MPU6050;
|
||||
#ifdef GYRO_MPU6050_ALIGN
|
||||
dev->gyroAlign = GYRO_MPU6050_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
|
@ -278,9 +259,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
|||
case GYRO_L3G4200D:
|
||||
if (l3g4200dDetect(dev)) {
|
||||
gyroHardware = GYRO_L3G4200D;
|
||||
#ifdef GYRO_L3G4200D_ALIGN
|
||||
dev->gyroAlign = GYRO_L3G4200D_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
|
@ -290,9 +268,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
|||
case GYRO_MPU3050:
|
||||
if (mpu3050Detect(dev)) {
|
||||
gyroHardware = GYRO_MPU3050;
|
||||
#ifdef GYRO_MPU3050_ALIGN
|
||||
dev->gyroAlign = GYRO_MPU3050_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
|
@ -302,9 +277,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
|||
case GYRO_L3GD20:
|
||||
if (l3gd20Detect(dev)) {
|
||||
gyroHardware = GYRO_L3GD20;
|
||||
#ifdef GYRO_L3GD20_ALIGN
|
||||
dev->gyroAlign = GYRO_L3GD20_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
|
@ -314,9 +286,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
|||
case GYRO_MPU6000:
|
||||
if (mpu6000SpiGyroDetect(dev)) {
|
||||
gyroHardware = GYRO_MPU6000;
|
||||
#ifdef GYRO_MPU6000_ALIGN
|
||||
dev->gyroAlign = GYRO_MPU6000_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
|
@ -348,9 +317,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
|||
default:
|
||||
gyroHardware = GYRO_MPU6500;
|
||||
}
|
||||
#ifdef GYRO_MPU6500_ALIGN
|
||||
dev->gyroAlign = GYRO_MPU6500_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
|
@ -360,9 +326,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
|||
case GYRO_MPU9250:
|
||||
if (mpu9250SpiGyroDetect(dev)) {
|
||||
gyroHardware = GYRO_MPU9250;
|
||||
#ifdef GYRO_MPU9250_ALIGN
|
||||
dev->gyroAlign = GYRO_MPU9250_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
|
@ -372,9 +335,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
|||
case GYRO_ICM20649:
|
||||
if (icm20649SpiGyroDetect(dev)) {
|
||||
gyroHardware = GYRO_ICM20649;
|
||||
#ifdef GYRO_ICM20649_ALIGN
|
||||
dev->gyroAlign = GYRO_ICM20649_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
|
@ -384,9 +344,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
|||
case GYRO_ICM20689:
|
||||
if (icm20689SpiGyroDetect(dev)) {
|
||||
gyroHardware = GYRO_ICM20689;
|
||||
#ifdef GYRO_ICM20689_ALIGN
|
||||
dev->gyroAlign = GYRO_ICM20689_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
|
@ -396,9 +353,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
|||
case GYRO_BMI160:
|
||||
if (bmi160SpiGyroDetect(dev)) {
|
||||
gyroHardware = GYRO_BMI160;
|
||||
#ifdef GYRO_BMI160_ALIGN
|
||||
dev->gyroAlign = GYRO_BMI160_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
FALLTHROUGH;
|
||||
|
@ -426,14 +380,16 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
|
|||
return gyroHardware;
|
||||
}
|
||||
|
||||
static bool gyroInitSensor(gyroSensor_t *gyroSensor)
|
||||
static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
|
||||
{
|
||||
gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
|
||||
gyroSensor->gyroDev.gyroAlign = config->align;
|
||||
gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;
|
||||
|
||||
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
|
||||
|| defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689)
|
||||
|
||||
mpuDetect(&gyroSensor->gyroDev);
|
||||
mpuDetect(&gyroSensor->gyroDev, config);
|
||||
#endif
|
||||
|
||||
const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
|
||||
|
@ -536,79 +492,23 @@ bool gyroInit(void)
|
|||
bool ret = false;
|
||||
gyroToUse = gyroConfig()->gyro_to_use;
|
||||
|
||||
#if defined(USE_DUAL_GYRO) && defined(GYRO_1_CS_PIN)
|
||||
if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
|
||||
gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin = IOGetByTag(IO_TAG(GYRO_1_CS_PIN));
|
||||
IOInit(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, RESOURCE_INDEX(0));
|
||||
IOHi(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus.
|
||||
IOConfigGPIO(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_DUAL_GYRO) && defined(GYRO_2_CS_PIN)
|
||||
if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
|
||||
gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin = IOGetByTag(IO_TAG(GYRO_2_CS_PIN));
|
||||
IOInit(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, RESOURCE_INDEX(1));
|
||||
IOHi(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus.
|
||||
IOConfigGPIO(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG);
|
||||
}
|
||||
#endif
|
||||
|
||||
gyroSensor1.gyroDev.gyroAlign = ALIGN_DEFAULT;
|
||||
|
||||
#if defined(GYRO_1_EXTI_PIN)
|
||||
gyroSensor1.gyroDev.mpuIntExtiTag = IO_TAG(GYRO_1_EXTI_PIN);
|
||||
#elif defined(MPU_INT_EXTI)
|
||||
gyroSensor1.gyroDev.mpuIntExtiTag = IO_TAG(MPU_INT_EXTI);
|
||||
#elif defined(USE_HARDWARE_REVISION_DETECTION)
|
||||
gyroSensor1.gyroDev.mpuIntExtiTag = selectMPUIntExtiConfigByHardwareRevision();
|
||||
#else
|
||||
gyroSensor1.gyroDev.mpuIntExtiTag = IO_TAG_NONE;
|
||||
#endif // GYRO_1_EXTI_PIN
|
||||
#ifdef USE_DUAL_GYRO
|
||||
#ifdef GYRO_1_ALIGN
|
||||
gyroSensor1.gyroDev.gyroAlign = GYRO_1_ALIGN;
|
||||
#endif
|
||||
gyroSensor1.gyroDev.bus.bustype = BUSTYPE_SPI;
|
||||
spiBusSetInstance(&gyroSensor1.gyroDev.bus, GYRO_1_SPI_INSTANCE);
|
||||
if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
|
||||
ret = gyroInitSensor(&gyroSensor1);
|
||||
ret = gyroInitSensor(&gyroSensor1, gyroDeviceConfig(0));
|
||||
if (!ret) {
|
||||
return false; // TODO handle failure of first gyro detection better. - Perhaps update the config to use second gyro then indicate a new failure mode and reboot.
|
||||
}
|
||||
gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection;
|
||||
}
|
||||
#else // USE_DUAL_GYRO
|
||||
ret = gyroInitSensor(&gyroSensor1);
|
||||
gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection;
|
||||
#endif // USE_DUAL_GYRO
|
||||
|
||||
#ifdef USE_DUAL_GYRO
|
||||
|
||||
gyroSensor2.gyroDev.gyroAlign = ALIGN_DEFAULT;
|
||||
|
||||
#if defined(GYRO_2_EXTI_PIN)
|
||||
gyroSensor2.gyroDev.mpuIntExtiTag = IO_TAG(GYRO_2_EXTI_PIN);
|
||||
#elif defined(USE_HARDWARE_REVISION_DETECTION)
|
||||
gyroSensor2.gyroDev.mpuIntExtiTag = selectMPUIntExtiConfigByHardwareRevision();
|
||||
#else
|
||||
gyroSensor2.gyroDev.mpuIntExtiTag = IO_TAG_NONE;
|
||||
#endif // GYRO_2_EXTI_PIN
|
||||
#ifdef GYRO_2_ALIGN
|
||||
gyroSensor2.gyroDev.gyroAlign = GYRO_2_ALIGN;
|
||||
#endif
|
||||
gyroSensor2.gyroDev.bus.bustype = BUSTYPE_SPI;
|
||||
spiBusSetInstance(&gyroSensor2.gyroDev.bus, GYRO_2_SPI_INSTANCE);
|
||||
#ifdef USE_MULTI_GYRO
|
||||
if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
|
||||
ret = gyroInitSensor(&gyroSensor2);
|
||||
ret = gyroInitSensor(&gyroSensor2, gyroDeviceConfig(1));
|
||||
if (!ret) {
|
||||
return false; // TODO handle failure of second gyro detection better. - Perhaps update the config to use first gyro then indicate a new failure mode and reboot.
|
||||
}
|
||||
gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection;
|
||||
}
|
||||
#endif // USE_DUAL_GYRO
|
||||
|
||||
#ifdef USE_DUAL_GYRO
|
||||
// Only allow using both gyros simultaneously if they are the same hardware type.
|
||||
// If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
|
||||
if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
|
||||
|
@ -620,7 +520,8 @@ bool gyroInit(void)
|
|||
|
||||
}
|
||||
}
|
||||
#endif // USE_DUAL_GYRO
|
||||
#endif
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
@ -778,7 +679,7 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
|
|||
void gyroInitFilters(void)
|
||||
{
|
||||
gyroInitSensorFilters(&gyroSensor1);
|
||||
#ifdef USE_DUAL_GYRO
|
||||
#ifdef USE_MULTI_GYRO
|
||||
gyroInitSensorFilters(&gyroSensor2);
|
||||
#endif
|
||||
}
|
||||
|
@ -790,23 +691,21 @@ FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
|
|||
|
||||
FAST_CODE bool isGyroCalibrationComplete(void)
|
||||
{
|
||||
#ifdef USE_DUAL_GYRO
|
||||
switch (gyroToUse) {
|
||||
default:
|
||||
case GYRO_CONFIG_USE_GYRO_1: {
|
||||
return isGyroSensorCalibrationComplete(&gyroSensor1);
|
||||
}
|
||||
#ifdef USE_MULTI_GYRO
|
||||
case GYRO_CONFIG_USE_GYRO_2: {
|
||||
return isGyroSensorCalibrationComplete(&gyroSensor2);
|
||||
}
|
||||
case GYRO_CONFIG_USE_GYRO_BOTH: {
|
||||
return isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2);
|
||||
}
|
||||
}
|
||||
#else
|
||||
return isGyroSensorCalibrationComplete(&gyroSensor1);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
|
||||
{
|
||||
|
@ -832,7 +731,7 @@ void gyroStartCalibration(bool isFirstArmingCalibration)
|
|||
{
|
||||
if (!(isFirstArmingCalibration && firstArmingCalibrationWasStarted)) {
|
||||
gyroSetCalibrationCycles(&gyroSensor1);
|
||||
#ifdef USE_DUAL_GYRO
|
||||
#ifdef USE_MULTI_GYRO
|
||||
gyroSetCalibrationCycles(&gyroSensor2);
|
||||
#endif
|
||||
|
||||
|
@ -1073,7 +972,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
|
|||
|
||||
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
|
||||
{
|
||||
#ifdef USE_DUAL_GYRO
|
||||
switch (gyroToUse) {
|
||||
case GYRO_CONFIG_USE_GYRO_1:
|
||||
gyroUpdateSensor(&gyroSensor1, currentTimeUs);
|
||||
|
@ -1089,6 +987,7 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
|
|||
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 0, lrintf(gyro.gyroADCf[X]));
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[Y]));
|
||||
break;
|
||||
#ifdef USE_MULTI_GYRO
|
||||
case GYRO_CONFIG_USE_GYRO_2:
|
||||
gyroUpdateSensor(&gyroSensor2, currentTimeUs);
|
||||
if (isGyroSensorCalibrationComplete(&gyroSensor2)) {
|
||||
|
@ -1125,14 +1024,9 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
|
|||
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y] - gyroSensor2.gyroDev.gyroADCf[Y]));
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf(gyroSensor1.gyroDev.gyroADCf[Z] - gyroSensor2.gyroDev.gyroADCf[Z]));
|
||||
break;
|
||||
}
|
||||
#else
|
||||
gyroUpdateSensor(&gyroSensor1, currentTimeUs);
|
||||
gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X];
|
||||
gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y];
|
||||
gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z];
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
bool gyroGetAccumulationAverage(float *accumulationAverage)
|
||||
{
|
||||
|
@ -1166,15 +1060,7 @@ int16_t gyroGetTemperature(void)
|
|||
|
||||
int16_t gyroRateDps(int axis)
|
||||
{
|
||||
#ifdef USE_DUAL_GYRO
|
||||
if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
|
||||
return lrintf(gyro.gyroADCf[axis] / gyroSensor2.gyroDev.scale);
|
||||
} else {
|
||||
return lrintf(gyro.gyroADCf[axis] / gyroSensor1.gyroDev.scale);
|
||||
}
|
||||
#else
|
||||
return lrintf(gyro.gyroADCf[axis] / gyroSensor1.gyroDev.scale);
|
||||
#endif
|
||||
return lrintf(gyro.gyroADCf[axis] / ACTIVE_GYRO->gyroDev.scale);
|
||||
}
|
||||
|
||||
bool gyroOverflowDetected(void)
|
||||
|
|
|
@ -42,21 +42,28 @@
|
|||
#define BEEPER_INVERTED
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_GYRO
|
||||
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define ACC_MPU6000_ALIGN CW0_DEG_FLIP
|
||||
#define GYRO_MPU6000_ALIGN CW0_DEG_FLIP
|
||||
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define MPU6500_CS_PIN MPU6000_CS_PIN
|
||||
#define MPU6500_SPI_INSTANCE MPU6000_SPI_INSTANCE
|
||||
#define ACC_MPU6500_ALIGN CW0_DEG_FLIP
|
||||
#define GYRO_MPU6500_ALIGN CW0_DEG_FLIP
|
||||
|
||||
#define USE_MULTI_GYRO
|
||||
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_EXTI_PIN NONE
|
||||
|
||||
#define ACC_1_ALIGN CW0_DEG_FLIP
|
||||
#define GYRO_1_ALIGN CW0_DEG_FLIP
|
||||
|
||||
#define GYRO_2_SPI_INSTANCE SPI1
|
||||
#define GYRO_2_CS_PIN PC15
|
||||
#define GYRO_2_EXTI_PIN NONE
|
||||
|
||||
#define ACC_2_ALIGN CW0_DEG_FLIP
|
||||
#define GYRO_2_ALIGN CW0_DEG_FLIP
|
||||
|
||||
#define USE_MAG
|
||||
#define USE_MAG_HMC5883
|
||||
|
|
|
@ -33,24 +33,22 @@
|
|||
#define INVERTER_PIN_UART1 PC0
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define MPU6000_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_1_CS_PIN SPI1_NSS_PIN
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW0_DEG
|
||||
#define ACC_MPU6000_ALIGN CW0_DEG
|
||||
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define MPU6500_CS_PIN MPU6000_CS_PIN
|
||||
#define MPU6500_SPI_INSTANCE MPU6000_SPI_INSTANCE
|
||||
#define GYRO_MPU6500_ALIGN GYRO_MPU6000_ALIGN
|
||||
#define ACC_MPU6500_ALIGN ACC_MPU6000_ALIGN
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_BARO
|
||||
#define USE_BARO_BMP280
|
||||
|
|
|
@ -30,26 +30,27 @@
|
|||
|
||||
// MPU6050 interrupts
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA15
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA15
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_ACC
|
||||
|
||||
#define USE_GYRO_MPU6050
|
||||
#define GYRO_MPU6050_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_MPU6050
|
||||
#define ACC_MPU6050_ALIGN CW180_DEG
|
||||
// MPU6050 support has been dropped according to board wiki
|
||||
// https://github.com/betaflight/betaflight/wiki/Board---AIR32
|
||||
//#define USE_GYRO_MPU6050
|
||||
//#define USE_ACC_MPU6050
|
||||
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
#define MPU6000_CS_PIN PB12
|
||||
#define MPU6000_SPI_INSTANCE SPI2
|
||||
#define GYRO_1_CS_PIN PB12
|
||||
#define GYRO_1_SPI_INSTANCE SPI2
|
||||
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
|
||||
//#define USE_BARO
|
||||
//#define USE_BARO_MS5611
|
||||
|
|
|
@ -32,23 +32,24 @@
|
|||
#define BEEPER_INVERTED
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC13
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC13
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define USE_SPI
|
||||
#define USE_SPI_DEVICE_2
|
||||
|
||||
#define MPU6500_CS_PIN PB12
|
||||
#define MPU6500_SPI_INSTANCE SPI2
|
||||
#define GYRO_1_CS_PIN PB12
|
||||
#define GYRO_1_SPI_INSTANCE SPI2
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW0_DEG
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW0_DEG
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_BARO
|
||||
#define USE_BARO_SPI_BMP280
|
||||
|
|
|
@ -37,12 +37,12 @@
|
|||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6050
|
||||
|
||||
#define GYRO_MPU6050_ALIGN CW0_DEG
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6050
|
||||
|
||||
#define ACC_MPU6050_ALIGN CW0_DEG
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_UART1
|
||||
#define USE_UART2
|
||||
|
|
|
@ -37,6 +37,7 @@
|
|||
#include "flight/pid.h"
|
||||
|
||||
#include "pg/beeper_dev.h"
|
||||
#include "pg/gyrodev.h"
|
||||
#include "pg/rx.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
@ -60,6 +61,8 @@
|
|||
// alternative defaults settings for AlienFlight targets
|
||||
void targetConfiguration(void)
|
||||
{
|
||||
gyroDeviceConfigMutable(0)->extiTag = selectMPUIntExtiConfigByHardwareRevision();
|
||||
|
||||
/* depending on revision ... depends on the LEDs to be utilised. */
|
||||
if (hardwareRevision == AFF3_REV_2) {
|
||||
statusLedConfigMutable()->inversion = 0
|
||||
|
|
|
@ -49,15 +49,15 @@
|
|||
#define USE_GYRO_MPU6050
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
|
||||
#define GYRO_MPU6050_ALIGN CW270_DEG
|
||||
#define GYRO_MPU6500_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6050
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
|
||||
#define ACC_MPU6050_ALIGN CW270_DEG
|
||||
#define ACC_MPU6500_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
// No baro support.
|
||||
//#define USE_BARO
|
||||
|
@ -106,8 +106,8 @@
|
|||
#define SPI3_MISO_PIN PB4
|
||||
#define SPI3_MOSI_PIN PB5
|
||||
|
||||
#define MPU6500_CS_PIN SPI3_NSS_PIN
|
||||
#define MPU6500_SPI_INSTANCE SPI3
|
||||
#define GYRO_1_CS_PIN SPI3_NSS_PIN
|
||||
#define GYRO_1_SPI_INSTANCE SPI3
|
||||
|
||||
#define USE_ADC
|
||||
|
||||
|
|
|
@ -38,21 +38,22 @@
|
|||
|
||||
// MPU interrupt
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC14
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC14
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define MPU6500_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN SPI1_NSS_PIN
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_MAG
|
||||
#define USE_MAG_HMC5883
|
||||
|
|
|
@ -36,21 +36,22 @@
|
|||
|
||||
// MPU interrupt
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC14
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC14
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define MPU6500_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN SPI1_NSS_PIN
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_MAG
|
||||
#define USE_MAG_HMC5883
|
||||
|
|
|
@ -121,10 +121,11 @@
|
|||
*/
|
||||
// Interrupt
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC14
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC14
|
||||
// MPU
|
||||
#define MPU6500_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN SPI1_NSS_PIN
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
// MAG
|
||||
|
@ -136,11 +137,11 @@
|
|||
// GYRO
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW0_DEG
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
// ACC
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW0_DEG
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
|
||||
/* Optional Digital Pressure Sensor (barometer) - Bosch BMP280
|
||||
* TODO: not implemented on V1 or V2 pcb
|
||||
|
|
|
@ -31,20 +31,22 @@
|
|||
#define BEEPER_PIN PB2 // Unused pin, can be mapped to elsewhere
|
||||
#define BEEPER_INVERTED
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
// MPU6000 interrupts
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PINPC4
|
||||
#define USE_EXTI
|
||||
|
||||
#define USE_MAG
|
||||
|
|
|
@ -31,20 +31,22 @@
|
|||
#define BEEPER_PIN PB2 // Unused pin, can be mapped to elsewhere
|
||||
#define BEEPER_INVERTED
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
// MPU6000 interrupts
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PINPC4
|
||||
#define USE_EXTI
|
||||
|
||||
#define USE_MAG
|
||||
|
|
|
@ -32,23 +32,24 @@
|
|||
#define LED1_PIN PB2
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PB6
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PB6
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#if defined(BEESTORM)
|
||||
#define GYRO_MPU6500_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
#else
|
||||
#define GYRO_MPU6500_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
#endif
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#if defined(BEESTORM)
|
||||
#define ACC_MPU6500_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
#else
|
||||
#define ACC_MPU6500_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
#endif
|
||||
|
||||
#define SERIAL_PORT_COUNT 4
|
||||
|
@ -85,8 +86,8 @@
|
|||
#define SPI3_MISO_PIN PB4
|
||||
#define SPI3_MOSI_PIN PB5
|
||||
|
||||
#define MPU6500_CS_PIN PA15
|
||||
#define MPU6500_SPI_INSTANCE SPI3
|
||||
#define GYRO_1_CS_PIN PA15
|
||||
#define GYRO_1_SPI_INSTANCE SPI3
|
||||
|
||||
#define USE_MAX7456
|
||||
#define MAX7456_SPI_INSTANCE SPI1
|
||||
|
|
|
@ -33,21 +33,22 @@
|
|||
|
||||
// ICM20689 interrupt
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA8
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA8
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define ICM20689_CS_PIN SPI1_NSS_PIN
|
||||
#define ICM20689_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN SPI1_NSS_PIN
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_ICM20689_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_ICM20689
|
||||
#define GYRO_ICM20689_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_BARO
|
||||
#define USE_BARO_BMP280
|
||||
|
|
|
@ -51,20 +51,22 @@
|
|||
|
||||
#define USABLE_TIMER_CHANNEL_COUNT 10
|
||||
|
||||
#define MPU6000_CS_PIN PA15
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA15
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
|
||||
// MPU6000 interrupts
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define MPU_INT_EXTI PC13
|
||||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC13
|
||||
#define USE_EXTI
|
||||
|
||||
#define REMAP_TIM16_DMA
|
||||
|
|
|
@ -32,22 +32,23 @@
|
|||
// PC13 used as inverter select GPIO for UART2
|
||||
#define INVERTER_PIN_UART2 PC13
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
|
||||
|
||||
|
||||
// MPU6000 interrupts
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_BARO
|
||||
|
|
|
@ -47,22 +47,21 @@
|
|||
|
||||
// MPU6500 interrupt
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC5
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC5
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
||||
#define MPU6500_CS_PIN PC4
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PC4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW0_DEG
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW0_DEG
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_MAG
|
||||
#define USE_MAG_HMC5883
|
||||
|
|
|
@ -31,7 +31,8 @@
|
|||
#define BEEPER_OPT PA2
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA3
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA3
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
||||
|
@ -39,8 +40,8 @@
|
|||
#define USE_SPI_DEVICE_1
|
||||
#define USE_SPI_DEVICE_2
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define FLASH_CS_PIN PB12
|
||||
#define FLASH_SPI_INSTANCE SPI2
|
||||
|
@ -50,11 +51,11 @@
|
|||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
// MPU6000 interrupts
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
|
|
@ -69,7 +69,7 @@
|
|||
//#define L3GD20_CS_PIN PE3
|
||||
|
||||
//#define GYRO_L3GD20_ALIGN CW270_DEG
|
||||
#define GYRO_MPU6050_ALIGN CW0_DEG
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6050
|
||||
|
@ -81,7 +81,7 @@
|
|||
//#define LSM303DLHC_I2C_INT1_PIN PE4
|
||||
//#define LSM303DLHC_I2C_INT2_PIN PE5
|
||||
|
||||
#define ACC_MPU6050_ALIGN CW0_DEG
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_BARO
|
||||
#define USE_BARO_MS5611
|
||||
|
|
|
@ -6,12 +6,6 @@ TARGET_SRC = \
|
|||
drivers/accgyro/accgyro_mpu.c \
|
||||
drivers/accgyro/accgyro_mpu3050.c \
|
||||
drivers/accgyro/accgyro_mpu6050.c \
|
||||
drivers/accgyro_legacy/accgyro_l3gd20.c \
|
||||
drivers/accgyro_legacy/accgyro_lsm303dlhc.c \
|
||||
drivers/accgyro_legacy/accgyro_adxl345.c \
|
||||
drivers/accgyro_legacy/accgyro_bma280.c \
|
||||
drivers/accgyro_legacy/accgyro_mma845x.c \
|
||||
drivers/accgyro_legacy/accgyro_l3g4200d.c \
|
||||
drivers/barometer/barometer_ms5611.c \
|
||||
drivers/barometer/barometer_bmp280.c \
|
||||
drivers/compass/compass_ak8975.c
|
||||
|
|
38
src/main/target/CJMCU/config.c
Normal file
38
src/main/target/CJMCU/config.c
Normal file
|
@ -0,0 +1,38 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
|
||||
* 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 this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_TARGET_CONFIG
|
||||
|
||||
#include "drivers/io_types.h"
|
||||
#include "hardware_revision.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/gyrodev.h"
|
||||
|
||||
void targetConfiguration(void)
|
||||
{
|
||||
gyroDeviceConfigMutable(0)->extiTag = selectMPUIntExtiConfigByHardwareRevision();
|
||||
}
|
||||
#endif
|
|
@ -22,6 +22,7 @@
|
|||
|
||||
#define TARGET_BOARD_IDENTIFIER "CJM1" // CJMCU
|
||||
#define USE_HARDWARE_REVISION_DETECTION
|
||||
#define USE_TARGET_CONFIG
|
||||
#define TARGET_BUS_INIT
|
||||
|
||||
#define LED0_PIN PC14
|
||||
|
|
|
@ -38,32 +38,29 @@
|
|||
|
||||
#define INVERTER_PIN_UART1 PC0 // PC0 used as inverter select GPIO
|
||||
|
||||
#define CAMERA_CONTROL_PIN PB9 // define dedicated camera_osd_control pin
|
||||
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
// MPU 6000
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define USE_GYRO
|
||||
|
||||
// MPU 6000
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW0_DEG
|
||||
#define ACC_MPU6000_ALIGN CW0_DEG
|
||||
|
||||
// ICM-20602
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
|
||||
#define ACC_MPU6500_ALIGN CW0_DEG
|
||||
#define GYRO_MPU6500_ALIGN CW0_DEG
|
||||
#define MPU6500_CS_PIN PA4
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_MAX7456
|
||||
#define MAX7456_SPI_INSTANCE SPI3
|
||||
|
|
|
@ -30,29 +30,27 @@
|
|||
#define BEEPER_INVERTED
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//MPU-6000
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define USE_GYRO
|
||||
|
||||
//MPU-6000
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
|
||||
#define GYRO_MPU6000_ALIGN CW0_DEG
|
||||
#define ACC_MPU6000_ALIGN CW0_DEG
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
|
||||
// ICM-20602
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
|
||||
#define ACC_MPU6500_ALIGN CW0_DEG
|
||||
#define GYRO_MPU6500_ALIGN CW0_DEG
|
||||
#define MPU6500_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
|
||||
|
||||
#define USE_MAX7456
|
||||
#define MAX7456_SPI_INSTANCE SPI3
|
||||
|
|
|
@ -37,20 +37,21 @@
|
|||
|
||||
#define INVERTER_PIN_UART2 PB2 // PB2 used as inverter select GPIO
|
||||
|
||||
#define MPU6000_CS_PIN PC4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PC4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW270_DEG_FLIP
|
||||
#define ACC_1_ALIGN CW270_DEG_FLIP
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW270_DEG_FLIP
|
||||
#define GYRO_1_ALIGN CW270_DEG_FLIP
|
||||
|
||||
// MPU6000 interrupts
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC0
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC0
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_MAG
|
||||
|
|
|
@ -40,7 +40,8 @@
|
|||
|
||||
// MPU6500 interrupt
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA5
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA5
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
|
@ -52,25 +53,18 @@
|
|||
#define SPI1_MOSI_PIN PB5
|
||||
#define SPI1_NSS_PIN PA4
|
||||
|
||||
#define MPU6500_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
|
||||
#define MPU6000_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN SPI1_NSS_PIN
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW270_DEG
|
||||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW270_DEG
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_BARO
|
||||
#define USE_BARO_MS5611
|
||||
|
|
|
@ -63,16 +63,17 @@
|
|||
#define BEEPER_INVERTED
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC13
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PINPC13
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_VCP
|
||||
#define USE_UART3
|
||||
|
|
|
@ -82,17 +82,20 @@
|
|||
#define USE_I2C_DEVICE_3
|
||||
#define I2C_DEVICE (I2CDEV_3)
|
||||
|
||||
// This board only uses I2C acc/gyro
|
||||
#undef USE_MULTI_GYRO
|
||||
|
||||
// MPU9250 has the AD0 pin held high so the
|
||||
// address is 0x69 instead of the default 0x68
|
||||
#define MPU_ADDRESS 0x69
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_MAG
|
||||
#define USE_MPU9250_MAG // Enables bypass configuration on the MPU9250 I2C bus
|
||||
|
@ -100,7 +103,8 @@
|
|||
#define MAG_AK8963_ALIGN CW270_DEG
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC13
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC13
|
||||
|
||||
#define USE_SERIALRX_TARGET_CUSTOM
|
||||
#define SERIALRX_UART SERIAL_PORT_USART6
|
||||
|
|
|
@ -40,7 +40,8 @@
|
|||
#define SPI1_MOSI_PIN PA7
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_GYRO
|
||||
|
@ -55,14 +56,14 @@
|
|||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_ICM20689_ALIGN CW90_DEG
|
||||
//------MPU6000
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
|
||||
//Baro & MAG-------------------------------
|
||||
#define USE_I2C
|
||||
|
|
|
@ -57,10 +57,8 @@
|
|||
#define SPI2_NSS_PIN PB12
|
||||
|
||||
// tqfp48 pin 3
|
||||
#define MPU6500_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define MPU6000_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN SPI1_NSS_PIN
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
// tqfp48 pin 25
|
||||
#define BMP280_CS_PIN SPI2_NSS_PIN
|
||||
|
@ -73,19 +71,14 @@
|
|||
#define FLASH_SPI_INSTANCE SPI2
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW270_DEG
|
||||
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW270_DEG
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_BARO
|
||||
#define USE_BARO_BMP280
|
||||
|
@ -121,7 +114,8 @@
|
|||
|
||||
// mpu_int definition in sensors/initialisation.c
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC13
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC13
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
|
|
@ -26,7 +26,8 @@
|
|||
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA15
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA15
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
|
||||
|
@ -35,20 +36,20 @@
|
|||
#define USE_SPI_DEVICE_2
|
||||
|
||||
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
#define SPI1_SCK_PIN PB3
|
||||
#define SPI1_MISO_PIN PB4
|
||||
#define SPI1_MOSI_PIN PB5
|
||||
#define MPU6500_CS_PIN PA5
|
||||
#define GYRO_1_CS_PIN PA5
|
||||
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
|
||||
|
||||
#define USE_RX_SPI
|
||||
|
|
|
@ -31,22 +31,23 @@
|
|||
|
||||
// MPU9250 interrupt
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PB5
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PB5
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define MPU6500_CS_PIN PB12
|
||||
#define MPU6500_SPI_INSTANCE SPI2
|
||||
#define GYRO_1_CS_PIN PB12
|
||||
#define GYRO_1_SPI_INSTANCE SPI2
|
||||
|
||||
// Using MPU6050 for the moment.
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
//#define USE_BARO
|
||||
//#define USE_BARO_MS5611
|
||||
|
|
|
@ -36,19 +36,20 @@
|
|||
|
||||
// MPU6000 interrupts
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PB0
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PB0
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_MAG
|
||||
#define USE_MAG_HMC5883
|
||||
|
|
|
@ -52,44 +52,32 @@
|
|||
/*------------SENSORS--------------*/
|
||||
// MPU interrupt
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_ACC
|
||||
|
||||
#if !defined(FF_FORTINIF4_REV03)
|
||||
#define MPU6000_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
#define ICM20689_CS_PIN SPI1_NSS_PIN
|
||||
#define ICM20689_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define USE_GYRO_SPI_ICM20689
|
||||
#define GYRO_ICM20689_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_ICM20689_ALIGN CW180_DEG
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#if defined(FF_FORTINIF4_REV03)
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#else
|
||||
#define GYRO_1_CS_PIN PA8
|
||||
#endif
|
||||
|
||||
#define MPU6500_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW180_DEG
|
||||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
|
||||
/*---------------------------------*/
|
||||
|
||||
#if !defined(FF_FORTINIF4_REV03)
|
||||
|
|
|
@ -45,20 +45,21 @@
|
|||
|
||||
// MPU6000 interrupts
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA15
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA15
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
|
||||
#define MPU6000_CS_GPIO GPIOB
|
||||
#define MPU6000_CS_PIN PB12
|
||||
#define MPU6000_SPI_INSTANCE SPI2
|
||||
#define GYRO_1_CS_PIN PB12
|
||||
#define GYRO_1_SPI_INSTANCE SPI2
|
||||
|
||||
#define USE_VCP
|
||||
#define USE_UART1
|
||||
|
|
|
@ -46,38 +46,29 @@
|
|||
/*------------SENSORS--------------*/
|
||||
// MPU interrupt
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#if defined(FF_PIKOF4OSD)
|
||||
#define MPU6000_CS_PIN PA15
|
||||
#define MPU6000_SPI_INSTANCE SPI3
|
||||
|
||||
#define MPU6500_CS_PIN PA15
|
||||
#define MPU6500_SPI_INSTANCE SPI3
|
||||
#define GYRO_1_CS_PIN PA15
|
||||
#define GYRO_1_SPI_INSTANCE SPI3
|
||||
#else
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
|
||||
#define MPU6500_CS_PIN PA4
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
#endif
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
/*---------------------------------*/
|
||||
|
||||
#if defined(FF_PIKOF4OSD)
|
||||
|
|
|
@ -38,23 +38,24 @@
|
|||
#define INVERTER_PIN_UART6 PC8
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
// *************** Gyro & ACC **********************
|
||||
#define USE_SPI
|
||||
#define USE_SPI_DEVICE_1
|
||||
|
||||
#define MPU6500_CS_PIN PA4
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
|
||||
// *************** UART *****************************
|
||||
#define USE_VCP
|
||||
|
|
|
@ -40,7 +40,8 @@
|
|||
#define SPI1_MOSI_PIN PA7
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_GYRO
|
||||
|
@ -55,14 +56,14 @@
|
|||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_ICM20689_ALIGN CW90_DEG
|
||||
//------MPU6000
|
||||
#define MPU6000_CS_PIN PB2
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PB2
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
|
||||
//Baro & MAG-------------------------------
|
||||
#define USE_I2C
|
||||
|
|
|
@ -53,29 +53,30 @@
|
|||
#define BEEPER_INVERTED
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC13
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PINPC13
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU_INT, SDCardDetect
|
||||
#define MPU_ADDRESS 0x69
|
||||
|
||||
#ifdef MYMPU6000
|
||||
#define MPU6000_SPI_INSTANCE SPI2
|
||||
#define MPU6000_CS_PIN PB12
|
||||
#define GYRO_1_SPI_INSTANCE SPI2
|
||||
#define GYRO_1_CS_PIN PB12
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
#else
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6050
|
||||
#define GYRO_MPU6050_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6050
|
||||
#define ACC_MPU6050_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
#endif
|
||||
|
||||
#define USE_VCP
|
||||
|
|
|
@ -31,8 +31,8 @@
|
|||
|
||||
#define INVERTER_PIN_UART6 PC8
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
|
@ -40,11 +40,12 @@
|
|||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
|
||||
#define GYRO_MPU6000_ALIGN CW270_DEG
|
||||
#define ACC_MPU6000_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_BARO
|
||||
|
|
|
@ -64,40 +64,28 @@
|
|||
#define BEEPER_INVERTED
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA3
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA3
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define ICM20689_CS_PIN PA4
|
||||
#define ICM20689_SPI_INSTANCE SPI1
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
|
||||
#define MPU6500_CS_PIN PA4
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_ICM20689
|
||||
#define GYRO_ICM20689_ALIGN CW180_DEG
|
||||
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW90_DEG
|
||||
#define USE_GYRO_SPI_ICM20689
|
||||
// MPU6000 and ICM20689 are CW180 aligned.
|
||||
// MPU6500 is CW90 aligned, but can't handle this.
|
||||
// Must be configured after flashing.
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_ICM20689_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW90_DEG
|
||||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
|
||||
#define USE_SPI
|
||||
#define USE_SPI_DEVICE_1
|
||||
|
|
|
@ -39,41 +39,24 @@
|
|||
|
||||
// MPU6000 interrupts
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define ICM20689_CS_PIN PA4
|
||||
#define ICM20689_SPI_INSTANCE SPI1
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
|
||||
#define MPU6500_CS_PIN PA4
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_ICM20689
|
||||
#define GYRO_ICM20689_ALIGN CW180_DEG
|
||||
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW180_DEG
|
||||
#define USE_GYRO_SPI_ICM20689
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_ICM20689_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW180_DEG
|
||||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
|
||||
#ifdef FURYF4OSD
|
||||
#define USE_MAX7456
|
||||
|
|
|
@ -32,19 +32,20 @@
|
|||
#define BEEPER_INVERTED
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define ICM20689_CS_PIN PA4
|
||||
#define ICM20689_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_ICM20689
|
||||
#define GYRO_ICM20689_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_ICM20689_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
|
||||
//#define USE_BARO
|
||||
//#define USE_BARO_MS5611
|
||||
|
|
|
@ -50,17 +50,18 @@
|
|||
#define BEEPER_PIN PC15
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC13
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC13
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW0_DEG
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW0_DEG
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_FLASHFS
|
||||
#define USE_FLASH_M25P16
|
||||
|
@ -93,8 +94,8 @@
|
|||
#define FLASH_CS_PIN PB12
|
||||
#define FLASH_SPI_INSTANCE SPI2
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_MAX7456
|
||||
#define MAX7456_SPI_INSTANCE SPI1
|
||||
|
|
|
@ -26,17 +26,18 @@
|
|||
#define LED0_PIN PB3
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC13
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC13
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6050
|
||||
#define GYRO_MPU6050_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6050
|
||||
#define ACC_MPU6050_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_BARO
|
||||
#define USE_BARO_BMP085
|
||||
|
|
|
@ -31,7 +31,8 @@
|
|||
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC13
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC13
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
|
@ -40,11 +41,9 @@
|
|||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6500
|
||||
//#define USE_GYRO_SPI_MPU6500
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6500
|
||||
//#define USE_ACC_SPI_MPU6500
|
||||
|
||||
#define USE_BARO
|
||||
#define USE_BARO_BMP280
|
||||
|
|
|
@ -56,21 +56,22 @@
|
|||
|
||||
// ICM20689 interrupt
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC5
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC5
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define ICM20689_CS_PIN PC4
|
||||
#define ICM20689_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PC4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_ICM20689_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_ICM20689
|
||||
#define GYRO_ICM20689_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#if defined(FLYWOOF405)
|
||||
//------MPU6000
|
||||
|
|
|
@ -31,26 +31,24 @@
|
|||
#define BEEPER_PIN PD15
|
||||
#define BEEPER_INVERTED
|
||||
|
||||
//define camera control
|
||||
#define CAMERA_CONTROL_PIN PE13
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_GYRO
|
||||
#define USE_EXTI
|
||||
|
||||
// ICM-20689
|
||||
#define USE_ACC_SPI_ICM20689
|
||||
#define USE_GYRO_SPI_ICM20689
|
||||
#define GYRO_ICM20689_ALIGN CW270_DEG
|
||||
#define ACC_ICM20689_ALIGN CW270_DEG
|
||||
#define MPU_INT_EXTI PE1
|
||||
|
||||
#define ICM20689_CS_PIN SPI4_NSS_PIN
|
||||
#define ICM20689_SPI_INSTANCE SPI4
|
||||
#define GYRO_1_CS_PIN ICM20689_CS_PIN
|
||||
#define GYRO_1_SPI_INSTANCE ICM20689_SPI_INSTANCE
|
||||
|
||||
#define ACC_1_ALIGN ACC_ICM20689_ALIGN
|
||||
#define GYRO_1_ALIGN GYRO_ICM20689_ALIGN
|
||||
#define GYRO_1_CS_PIN SPI4_NSS_PIN
|
||||
#define GYRO_1_SPI_INSTANCE SPI4
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PE1
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define USE_EXTI
|
||||
|
||||
#define USE_VCP
|
||||
#define USE_USB_DETECT
|
||||
|
@ -173,4 +171,3 @@
|
|||
#define USABLE_TIMER_CHANNEL_COUNT 8
|
||||
|
||||
#define USED_TIMERS ( TIM_N(1) | TIM_N(5) | TIM_N(3) | TIM_N(4) | TIM_N(8) )
|
||||
|
||||
|
|
|
@ -36,19 +36,21 @@
|
|||
#define BEEPER_INVERTED
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PB2
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PB2
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#ifdef KISSCC
|
||||
#define USE_TARGET_CONFIG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6050
|
||||
#define GYRO_MPU6050_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6050
|
||||
#define ACC_MPU6050_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
|
||||
#define TARGET_DEFAULT_MIXER MIXER_QUADX_1234
|
||||
|
||||
|
@ -56,11 +58,11 @@
|
|||
#else
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6050
|
||||
#define GYRO_MPU6050_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6050
|
||||
#define ACC_MPU6050_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
#endif
|
||||
|
||||
#define USE_VCP
|
||||
|
|
|
@ -34,16 +34,16 @@
|
|||
#define BEEPER_PIN PC9
|
||||
#define BEEPER_INVERTED
|
||||
|
||||
#define MPU6000_CS_PIN PB12
|
||||
#define MPU6000_SPI_INSTANCE SPI2
|
||||
#define GYRO_1_CS_PIN PB12
|
||||
#define GYRO_1_SPI_INSTANCE SPI2
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
|
||||
|
||||
#define USE_SPI
|
||||
|
|
|
@ -49,21 +49,19 @@
|
|||
|
||||
// MPU6000 interrupts
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_GYRO
|
||||
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
|
||||
#if defined(KIWIF4) || defined(KIWIF4V2)
|
||||
#define USE_MAX7456
|
||||
|
|
|
@ -39,21 +39,22 @@
|
|||
#define INVERTER_PIN_UART1 PB13
|
||||
#define INVERTER_PIN_UART6 PB12
|
||||
|
||||
#define MPU6000_CS_PIN PB2
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PB2
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
// MPU6000 interrupts
|
||||
#define USE_EXTI
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define MPU_INT_EXTI PA4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA4
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_MAG
|
||||
#define USE_MAG_HMC5883
|
||||
|
|
|
@ -27,15 +27,16 @@
|
|||
#define BEEPER_PIN PC15
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA3
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA3
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_SPI
|
||||
#define USE_SPI_DEVICE_1
|
||||
#define USE_SPI_DEVICE_2
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define FLASH_CS_PIN PB12
|
||||
#define FLASH_SPI_INSTANCE SPI2
|
||||
|
@ -45,11 +46,11 @@
|
|||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_VCP
|
||||
#define USE_UART1
|
||||
|
|
|
@ -51,7 +51,8 @@
|
|||
|
||||
// MPU6500 interrupt
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA5
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA5
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
@ -92,31 +93,27 @@
|
|||
#define SDCARD_DMA_CHANNEL_TX DMA1_Channel5
|
||||
#endif
|
||||
|
||||
#define MPU6000_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define MPU6500_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN SPI1_NSS_PIN
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_GYRO
|
||||
#ifdef LUXV2_RACE
|
||||
#define USE_GYRO_MPU6000
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
#else
|
||||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
#endif
|
||||
|
||||
#define USE_ACC
|
||||
#ifdef LUXV2_RACE
|
||||
#define USE_ACC_MPU6000
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
#else
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
#endif
|
||||
|
||||
#define USE_VCP
|
||||
|
|
|
@ -40,29 +40,28 @@
|
|||
#define SPI1_MISO_PIN PA6
|
||||
#define SPI1_MOSI_PIN PA7
|
||||
|
||||
#define MPU6000_CS_PIN PC2
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PC2
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define MPU6500_CS_PIN PC2
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PC2
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC3
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC3
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW270_DEG
|
||||
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
//#define GYRO_1_ALIGN CW180_DEG // XXX MPU6500 align, must be configured after flashing
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW270_DEG
|
||||
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
//#define ACC_1_ALIGN CW180_DEG // XXX MPU6500 align, must be configured after flashing
|
||||
|
||||
#define USE_MAG
|
||||
#define USE_MAG_HMC5883
|
||||
|
|
|
@ -38,27 +38,22 @@
|
|||
#define SPI1_MISO_PIN PA6
|
||||
#define SPI1_MOSI_PIN PA7
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
|
||||
#define MPU6500_CS_PIN PA4
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA1
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA1
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW180_DEG
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
|
||||
// *************** Baro **************************
|
||||
#define USE_I2C
|
||||
|
|
|
@ -37,19 +37,20 @@
|
|||
#define SPI1_MISO_PIN PA6
|
||||
#define SPI1_MOSI_PIN PA7
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA1
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA1
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
|
||||
// *************** SPI2 OSD *****************************
|
||||
#define USE_SPI_DEVICE_2
|
||||
|
|
|
@ -41,29 +41,28 @@
|
|||
#define SPI1_MISO_PIN PA6
|
||||
#define SPI1_MOSI_PIN PA7
|
||||
|
||||
#define MPU6500_CS_PIN PC2
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PC2
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define ICM20689_CS_PIN PC2
|
||||
#define ICM20689_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC3
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC3
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW180_DEG
|
||||
|
||||
#define USE_GYRO_SPI_ICM20689
|
||||
#define GYRO_ICM20689_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
//#define GYRO_ICM20689_ALIGN CW90_DEG // XXX has to be post-flash configured
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_SPI_ICM20689
|
||||
#define ACC_ICM20689_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
//#define ACC_ICM20689_ALIGN CW90_DEG // XXX has to be post-flash configured
|
||||
|
||||
// *************** Baro **************************
|
||||
#define USE_I2C
|
||||
|
|
|
@ -51,11 +51,11 @@
|
|||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6050
|
||||
#define GYRO_MPU6050_ALIGN CW0_DEG
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6050
|
||||
#define ACC_MPU6050_ALIGN CW0_DEG
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_BARO
|
||||
#define USE_BARO_MS5611
|
||||
|
|
|
@ -38,14 +38,15 @@
|
|||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6050
|
||||
#define GYRO_MPU6050_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6050
|
||||
#define ACC_MPU6050_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA13
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA13
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_VCP
|
||||
|
|
|
@ -33,7 +33,8 @@
|
|||
|
||||
// MPU6050 interrupts
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA15
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA15
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
|
@ -41,20 +42,16 @@
|
|||
#define USE_ACC
|
||||
|
||||
#define USE_GYRO_MPU6050
|
||||
#define GYRO_MPU6050_ALIGN CW180_DEG
|
||||
|
||||
#define USE_ACC_MPU6050
|
||||
|
||||
#define ACC_MPU6050_ALIGN CW180_DEG
|
||||
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
|
||||
#define MPU6000_CS_GPIO GPIOB
|
||||
#define MPU6000_CS_PIN PB12
|
||||
#define MPU6000_SPI_INSTANCE SPI2
|
||||
#define GYRO_1_CS_PIN PB12
|
||||
#define GYRO_1_SPI_INSTANCE SPI2
|
||||
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
|
||||
#define USE_VCP
|
||||
#define USE_UART1
|
||||
|
|
|
@ -38,20 +38,21 @@
|
|||
#define BEEPER_PIN PB4
|
||||
#define BEEPER_INVERTED
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
|
||||
// MPU6000 interrupts
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC5
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC5
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
//#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready
|
||||
|
|
|
@ -31,17 +31,18 @@
|
|||
#define BEEPER_INVERTED
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC13
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC13
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define ENSURE_MPU_DATA_READY_IS_LOW
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6050
|
||||
#define GYRO_MPU6050_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6050
|
||||
#define ACC_MPU6050_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_BARO
|
||||
#define USE_BARO_MS5611
|
||||
|
|
|
@ -61,6 +61,8 @@ void targetConfiguration(void)
|
|||
|
||||
motorConfigMutable()->minthrottle = 1049;
|
||||
|
||||
gyroDeviceConfigMutable()->extiTag = selectMPUIntExtiConfigByHardwareRevision();
|
||||
|
||||
gyroConfigMutable()->gyro_hardware_lpf = GYRO_HARDWARE_LPF_1KHZ_SAMPLE;
|
||||
gyroConfigMutable()->gyro_soft_lpf_hz = 100;
|
||||
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
|
||||
|
|
|
@ -58,7 +58,8 @@
|
|||
|
||||
#define USE_EXTI
|
||||
#define MAG_INT_EXTI PC14
|
||||
#define MPU_INT_EXTI PC13
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC13
|
||||
#define MMA8451_INT_PIN PA5
|
||||
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
@ -74,8 +75,8 @@
|
|||
#define FLASH_CS_PIN NAZE_SPI_CS_PIN
|
||||
#define FLASH_SPI_INSTANCE NAZE_SPI_INSTANCE
|
||||
|
||||
#define MPU6500_CS_PIN NAZE_SPI_CS_PIN
|
||||
#define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE
|
||||
#define GYRO_1_CS_PIN NAZE_SPI_CS_PIN
|
||||
#define GYRO_1_SPI_INSTANCE NAZE_SPI_INSTANCE
|
||||
|
||||
#define USE_FLASHFS
|
||||
#define USE_FLASH_M25P16
|
||||
|
@ -86,9 +87,7 @@
|
|||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
|
||||
#define GYRO_MPU3050_ALIGN CW0_DEG
|
||||
#define GYRO_MPU6050_ALIGN CW0_DEG
|
||||
#define GYRO_MPU6500_ALIGN CW0_DEG
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_ACC
|
||||
//#define USE_ACC_ADXL345
|
||||
|
@ -99,10 +98,9 @@
|
|||
#define USE_ACC_SPI_MPU6500
|
||||
|
||||
//#define ACC_ADXL345_ALIGN CW270_DEG
|
||||
#define ACC_MPU6050_ALIGN CW0_DEG
|
||||
//#define ACC_MMA8452_ALIGN CW90_DEG
|
||||
//#define ACC_BMA280_ALIGN CW0_DEG
|
||||
#define ACC_MPU6500_ALIGN CW0_DEG
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_BARO
|
||||
#define USE_BARO_MS5611 // needed for Flip32 board
|
||||
|
|
|
@ -2,10 +2,6 @@ F1_TARGETS += $(TARGET)
|
|||
FEATURES = ONBOARDFLASH
|
||||
|
||||
TARGET_SRC = \
|
||||
drivers/accgyro_legacy/accgyro_adxl345.c \
|
||||
drivers/accgyro_legacy/accgyro_bma280.c \
|
||||
drivers/accgyro_legacy/accgyro_l3g4200d.c \
|
||||
drivers/accgyro_legacy/accgyro_mma845x.c \
|
||||
drivers/accgyro/accgyro_mpu.c \
|
||||
drivers/accgyro/accgyro_mpu3050.c \
|
||||
drivers/accgyro/accgyro_mpu6050.c \
|
||||
|
|
|
@ -37,22 +37,21 @@
|
|||
|
||||
// MPU6500 interrupt
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PB2
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PB2
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
||||
#define MPU6500_CS_PIN PC4
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PC4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW0_DEG
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW0_DEG
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
|
||||
#define USE_SDCARD
|
||||
|
||||
|
|
|
@ -41,14 +41,12 @@
|
|||
#define USE_GYRO_SPI_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
|
||||
#define MPU6500_CS_PIN PB12
|
||||
#define MPU6500_SPI_INSTANCE SPI2
|
||||
|
||||
#define MPU6000_CS_PIN PB12
|
||||
#define MPU6000_SPI_INSTANCE SPI2
|
||||
#define GYRO_1_CS_PIN PB12
|
||||
#define GYRO_1_SPI_INSTANCE SPI2
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PA8
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PA8
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_BARO
|
||||
|
|
|
@ -51,11 +51,8 @@
|
|||
#define USE_ACC_SPI_MPU6500
|
||||
#define USE_ACC_SPI_MPU9250
|
||||
|
||||
#define MPU9250_CS_PIN PB12
|
||||
#define MPU9250_SPI_INSTANCE SPI2
|
||||
|
||||
#define MPU6500_CS_PIN PB12
|
||||
#define MPU6500_SPI_INSTANCE SPI2
|
||||
#define GYRO_1_SPI_INSTANCE SPI2
|
||||
#define GYRO_1_CS_PIN PB12
|
||||
|
||||
#define USE_EXTI
|
||||
|
||||
|
|
|
@ -33,19 +33,23 @@
|
|||
#define BEEPER_PIN PA0
|
||||
#define BEEPER_INVERTED
|
||||
|
||||
#undef USE_MULTI_GYRO // XXX Drop this if target has I2C configured
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_FAKE_ACC
|
||||
#define USE_ACC_MPU6050
|
||||
#define ACC_MPU6050_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_FAKE_GYRO
|
||||
#define USE_GYRO_MPU6050
|
||||
#define GYRO_MPU6050_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
// MPU6050 interrupts
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define MPU_INT_EXTI PB15
|
||||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PB15
|
||||
#define USE_EXTI
|
||||
|
||||
#define USE_MAG
|
||||
|
|
|
@ -33,19 +33,23 @@
|
|||
//#define BEEPER_PIN PA0
|
||||
//#define BEEPER_INVERTED
|
||||
|
||||
#undef USE_MULTI_GYRO
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_FAKE_ACC
|
||||
#define USE_ACC_MPU6050
|
||||
#define ACC_MPU6050_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_FAKE_GYRO
|
||||
#define USE_GYRO_MPU6050
|
||||
#define GYRO_MPU6050_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
|
||||
// MPU6050 interrupts
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
#define MPU_INT_EXTI PB15
|
||||
#define USE_EXTI
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PINPB15
|
||||
#define USE_EXTI
|
||||
|
||||
#define USE_MAG
|
||||
|
|
|
@ -56,19 +56,20 @@
|
|||
#define BEEPER_INVERTED
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC13
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC13
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define ACC_MPU6000_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
|
||||
#define BMP280_SPI_INSTANCE SPI1
|
||||
#define BMP280_CS_PIN PA13
|
||||
|
|
|
@ -71,29 +71,33 @@
|
|||
#define INVERTER_PIN_UART1 PC0 // DYS F4 Pro; Omnibus F4 AIO (1st gen) have a FIXED inverter on UART1
|
||||
#endif
|
||||
|
||||
#define USE_EXTI
|
||||
|
||||
#define USE_MULTI_GYRO
|
||||
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
|
||||
// MPU6000 interrupts
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#if defined(OMNIBUSF4SD)
|
||||
#define GYRO_MPU6000_ALIGN CW270_DEG
|
||||
#define ACC_MPU6000_ALIGN CW270_DEG
|
||||
#define GYRO_1_ALIGN CW270_DEG
|
||||
#define ACC_1_ALIGN CW270_DEG
|
||||
#elif defined(XRACERF4) || defined(EXUAVF4PRO)
|
||||
#define GYRO_MPU6000_ALIGN CW90_DEG
|
||||
#define ACC_MPU6000_ALIGN CW90_DEG
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
#else
|
||||
#define GYRO_MPU6000_ALIGN CW180_DEG
|
||||
#define ACC_MPU6000_ALIGN CW180_DEG
|
||||
#define GYRO_1_ALIGN CW180_DEG
|
||||
#define ACC_1_ALIGN CW180_DEG
|
||||
#endif
|
||||
|
||||
// Support for iFlight OMNIBUS F4 V3
|
||||
|
@ -102,12 +106,15 @@
|
|||
#if defined (OMNIBUSF4SD) || defined(OMNIBUSF4BASE)
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define MPU6500_CS_PIN MPU6000_CS_PIN
|
||||
#define MPU6500_SPI_INSTANCE MPU6000_SPI_INSTANCE
|
||||
#define GYRO_MPU6500_ALIGN GYRO_MPU6000_ALIGN
|
||||
#define ACC_MPU6500_ALIGN ACC_MPU6000_ALIGN
|
||||
#endif
|
||||
|
||||
// Dummy defines
|
||||
#define GYRO_2_SPI_INSTANCE GYRO_1_SPI_INSTANCE
|
||||
#define GYRO_2_CS_PIN NONE
|
||||
#define GYRO_2_ALIGN ALIGN_DEFAULT
|
||||
#define GYRO_2_EXTI_PIN NONE
|
||||
#define ACC_2_ALIGN ALIGN_DEFAULT
|
||||
|
||||
#define USE_MAG
|
||||
#define USE_MAG_HMC5883
|
||||
#define USE_MAG_QMC5883
|
||||
|
|
|
@ -68,7 +68,9 @@
|
|||
|
||||
// MPU6000 interrupts
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define GYRO_2_EXTI_PIN NONE
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_MAG
|
||||
|
|
|
@ -46,66 +46,48 @@
|
|||
#define USE_GYRO
|
||||
#define USE_DUAL_GYRO
|
||||
// ICM-20608-G
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
//#define MPU_INT_EXTI PE8
|
||||
|
||||
// MPU6000
|
||||
#define USE_ACC_MPU6000
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define USE_GYRO_MPU6000
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
//#define MPU_INT_EXTI PD0
|
||||
|
||||
// Provisioned, but not used
|
||||
#define GYRO_1_EXTI_PIN NONE
|
||||
#define GYRO_2_EXTI_PIN NONE
|
||||
|
||||
#if defined(OMNIBUSF7V2)
|
||||
#define MPU6000_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define MPU6500_CS_PIN SPI3_NSS_PIN
|
||||
#define MPU6500_SPI_INSTANCE SPI3
|
||||
#define GYRO_1_CS_PIN MPU6500_CS_PIN
|
||||
#define GYRO_2_CS_PIN MPU6000_CS_PIN
|
||||
#define GYRO_MPU6500_ALIGN CW90_DEG
|
||||
#define ACC_MPU6500_ALIGN CW90_DEG
|
||||
#define GYRO_MPU6000_ALIGN ALIGN_DEFAULT
|
||||
#define ACC_MPU6000_ALIGN ALIGN_DEFAULT
|
||||
#define ACC_1_ALIGN ACC_MPU6500_ALIGN
|
||||
#define ACC_2_ALIGN ACC_MPU6000_ALIGN
|
||||
#define GYRO_1_ALIGN GYRO_MPU6500_ALIGN
|
||||
#define GYRO_2_ALIGN GYRO_MPU6000_ALIGN
|
||||
#define GYRO_1_SPI_INSTANCE MPU6500_SPI_INSTANCE
|
||||
#define GYRO_2_SPI_INSTANCE MPU6000_SPI_INSTANCE
|
||||
#elif defined(FPVM_BETAFLIGHTF7)
|
||||
#define MPU6000_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define MPU6500_CS_PIN SPI3_NSS_PIN
|
||||
#define MPU6500_SPI_INSTANCE SPI3
|
||||
#define GYRO_1_CS_PIN MPU6000_CS_PIN
|
||||
#define GYRO_2_CS_PIN MPU6500_CS_PIN
|
||||
#define GYRO_MPU6500_ALIGN CW270_DEG
|
||||
#define ACC_MPU6500_ALIGN CW270_DEG
|
||||
#define GYRO_MPU6000_ALIGN CW90_DEG
|
||||
#define ACC_MPU6000_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN ACC_MPU6000_ALIGN
|
||||
#define ACC_2_ALIGN ACC_MPU6500_ALIGN
|
||||
#define GYRO_1_ALIGN GYRO_MPU6000_ALIGN
|
||||
#define GYRO_2_ALIGN GYRO_MPU6500_ALIGN
|
||||
#define GYRO_1_SPI_INSTANCE MPU6000_SPI_INSTANCE
|
||||
#define GYRO_2_SPI_INSTANCE MPU6500_SPI_INSTANCE
|
||||
#else
|
||||
#define MPU6000_CS_PIN SPI3_NSS_PIN
|
||||
#define MPU6000_SPI_INSTANCE SPI3
|
||||
#define MPU6500_CS_PIN SPI1_NSS_PIN
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN MPU6000_CS_PIN
|
||||
#define GYRO_2_CS_PIN MPU6500_CS_PIN
|
||||
#define ACC_1_ALIGN ALIGN_DEFAULT
|
||||
#define ACC_2_ALIGN ALIGN_DEFAULT
|
||||
#define GYRO_1_ALIGN ALIGN_DEFAULT
|
||||
#define GYRO_1_SPI_INSTANCE SPI3
|
||||
#define GYRO_1_CS_PIN PA15
|
||||
#define GYRO_2_SPI_INSTANCE SPI1
|
||||
#define GYRO_2_CS_PIN PA4
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
#define GYRO_2_ALIGN ALIGN_DEFAULT
|
||||
#define GYRO_1_SPI_INSTANCE MPU6000_SPI_INSTANCE
|
||||
#define GYRO_2_SPI_INSTANCE MPU6500_SPI_INSTANCE
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
#define ACC_2_ALIGN ALIGN_DEFAULT
|
||||
|
||||
#elif defined(FPVM_BETAFLIGHTF7)
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_2_SPI_INSTANCE SPI3
|
||||
#define GYRO_2_CS_PIN PA15
|
||||
#define GYRO_1_ALIGN CW90_DEG
|
||||
#define ACC_1_ALIGN CW90_DEG
|
||||
#define GYRO_2_ALIGN CW270_DEG
|
||||
#define ACC_2_ALIGN CW270_DEG
|
||||
|
||||
#else
|
||||
#define GYRO_1_SPI_INSTANCE SPI3
|
||||
#define GYRO_1_CS_PIN PA15
|
||||
#define GYRO_2_SPI_INSTANCE SPI1
|
||||
#define GYRO_2_CS_PIN PA4
|
||||
#define GYRO_1_ALIGN ALIGN_DEFAULT
|
||||
#define ACC_1_ALIGN ALIGN_DEFAULT
|
||||
#define GYRO_2_ALIGN ALIGN_DEFAULT
|
||||
#define ACC_2_ALIGN ALIGN_DEFAULT
|
||||
#endif
|
||||
|
||||
// TODO: dual gyro support
|
||||
|
|
|
@ -296,6 +296,7 @@ void updateHardwareRevision(void)
|
|||
// Empty
|
||||
}
|
||||
|
||||
// XXX Can be gone as sensors/gyro.c is not calling this anymore
|
||||
ioTag_t selectMPUIntExtiConfigByHardwareRevision(void)
|
||||
{
|
||||
return IO_TAG_NONE;
|
||||
|
|
|
@ -46,6 +46,10 @@
|
|||
|
||||
#define USE_ACC
|
||||
#define USE_GYRO
|
||||
// MPU interrupts
|
||||
//#define USE_EXTI
|
||||
//#define USE_GYRO_EXTI
|
||||
//#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
// For debugging with NUC405RG
|
||||
#define USE_FAKE_ACC
|
||||
|
@ -57,25 +61,20 @@
|
|||
#define USE_ACC_SPI_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
|
||||
#define USE_DUAL_GYRO
|
||||
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PB12 // Onboard IMU
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
#define GYRO_1_EXTI_PIN NONE
|
||||
|
||||
#define GYRO_2_SPI_INSTANCE SPI1
|
||||
#define GYRO_2_CS_PIN PA8 // External IMU
|
||||
#define GYRO_2_ALIGN CW0_DEG
|
||||
#define ACC_2_ALIGN CW0_DEG
|
||||
#define GYRO_2_EXTI_PIN NONE
|
||||
|
||||
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
|
||||
|
||||
// MPU interrupts
|
||||
//#define USE_EXTI
|
||||
//#define MPU_INT_EXTI PC4
|
||||
//#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
#define USE_MAG
|
||||
#define MAG_I2C_INSTANCE (I2CDEV_1)
|
||||
#define USE_MAG_HMC5883
|
||||
|
|
|
@ -30,7 +30,8 @@
|
|||
#define INVERTER_PIN_UART1 PC3 // PC3 used as sBUS inverter select GPIO
|
||||
|
||||
#define USE_EXTI
|
||||
#define MPU_INT_EXTI PC4
|
||||
#define USE_GYRO_EXTI
|
||||
#define GYRO_1_EXTI_PIN PC4
|
||||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
|
||||
// DEFINE SPI USAGE
|
||||
|
@ -38,14 +39,14 @@
|
|||
#define USE_SPI_DEVICE_1
|
||||
|
||||
// MPU 6000
|
||||
#define MPU6000_CS_PIN PA4
|
||||
#define MPU6000_SPI_INSTANCE SPI1
|
||||
#define GYRO_1_CS_PIN PA4
|
||||
#define GYRO_1_SPI_INSTANCE SPI1
|
||||
#define USE_ACC
|
||||
#define USE_ACC_SPI_MPU6000
|
||||
#define USE_GYRO
|
||||
#define USE_GYRO_SPI_MPU6000
|
||||
#define GYRO_MPU6000_ALIGN CW0_DEG
|
||||
#define ACC_MPU6000_ALIGN CW0_DEG
|
||||
#define GYRO_1_ALIGN CW0_DEG
|
||||
#define ACC_1_ALIGN CW0_DEG
|
||||
|
||||
// DEFINE OSD
|
||||
#define USE_SPI_DEVICE_2
|
||||
|
|
Some files were not shown because too many files have changed in this diff Show more
Loading…
Add table
Add a link
Reference in a new issue