mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 01:35:35 +03:00
Added direct support for MPU9250 gyro
This commit is contained in:
Martin Budden
parent
8fab92f405
commit
74fe97dde6
11 changed files with 374 additions and 49 deletions
|
|
@ -42,6 +42,7 @@
|
|||
#include "accgyro_mpu6500.h"
|
||||
#include "accgyro_spi_mpu6000.h"
|
||||
#include "accgyro_spi_mpu6500.h"
|
||||
#include "accgyro_spi_mpu9250.h"
|
||||
#include "accgyro_mpu.h"
|
||||
|
||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
|
|
@ -242,6 +243,19 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_GYRO_SPI_MPU9250
|
||||
if (mpu9250SpiDetect()) {
|
||||
gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
|
||||
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||
gyro->mpuConfiguration.read = mpu9250ReadRegister;
|
||||
gyro->mpuConfiguration.slowread = mpu9250SlowReadRegister;
|
||||
gyro->mpuConfiguration.verifywrite = verifympu9250WriteRegister;
|
||||
gyro->mpuConfiguration.write = mpu9250WriteRegister;
|
||||
gyro->mpuConfiguration.reset = mpu9250ResetGyro;
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_GYRO_SPI_ICM20608
|
||||
if (icm20608SpiDetect()) {
|
||||
mpuDetectionResult.sensor = ICM_20608_SPI;
|
||||
|
|
|
|||
235
src/main/drivers/accgyro_spi_mpu9250.c
Normal file
235
src/main/drivers/accgyro_spi_mpu9250.c
Normal file
|
|
@ -0,0 +1,235 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
* Authors:
|
||||
* Dominic Clifton - Cleanflight implementation
|
||||
* John Ihlein - Initial FF32 code
|
||||
* Kalyn Doerr (RS2K) - Robust rewrite
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "io.h"
|
||||
|
||||
#include "system.h"
|
||||
#include "exti.h"
|
||||
#include "bus_spi.h"
|
||||
#include "gyro_sync.h"
|
||||
#include "light_led.h"
|
||||
|
||||
#include "sensor.h"
|
||||
#include "accgyro.h"
|
||||
#include "accgyro_mpu.h"
|
||||
#include "accgyro_spi_mpu9250.h"
|
||||
|
||||
static bool mpuSpi9250InitDone = false;
|
||||
|
||||
static IO_t mpuSpi9250CsPin = IO_NONE;
|
||||
|
||||
#define DISABLE_MPU9250 IOHi(mpuSpi9250CsPin)
|
||||
#define ENABLE_MPU9250 IOLo(mpuSpi9250CsPin)
|
||||
|
||||
bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
||||
{
|
||||
ENABLE_MPU9250;
|
||||
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
|
||||
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
|
||||
DISABLE_MPU9250;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
||||
{
|
||||
ENABLE_MPU9250;
|
||||
delayMicroseconds(1);
|
||||
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
|
||||
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
|
||||
DISABLE_MPU9250;
|
||||
delayMicroseconds(1);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool mpu9250WriteRegister(uint8_t reg, uint8_t data)
|
||||
{
|
||||
ENABLE_MPU9250;
|
||||
delayMicroseconds(1);
|
||||
spiTransferByte(MPU9250_SPI_INSTANCE, reg);
|
||||
spiTransferByte(MPU9250_SPI_INSTANCE, data);
|
||||
DISABLE_MPU9250;
|
||||
delayMicroseconds(1);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool verifympu9250WriteRegister(uint8_t reg, uint8_t data)
|
||||
{
|
||||
mpu9250WriteRegister(reg, data);
|
||||
delayMicroseconds(100);
|
||||
|
||||
uint8_t attemptsRemaining = 20;
|
||||
do {
|
||||
uint8_t in;
|
||||
mpu9250SlowReadRegister(reg, 1, &in);
|
||||
if (in == data) {
|
||||
return true;
|
||||
} else {
|
||||
debug[3]++;
|
||||
mpu9250WriteRegister(reg, data);
|
||||
delayMicroseconds(100);
|
||||
}
|
||||
} while (attemptsRemaining--);
|
||||
return false;
|
||||
}
|
||||
|
||||
void mpu9250ResetGyro(void)
|
||||
{
|
||||
// Device Reset
|
||||
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
||||
delay(150);
|
||||
}
|
||||
|
||||
static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
if (mpuSpi9250InitDone) {
|
||||
return;
|
||||
}
|
||||
|
||||
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
|
||||
|
||||
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
||||
delay(50);
|
||||
|
||||
verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||
|
||||
//Fchoice_b defaults to 00 which makes fchoice 11
|
||||
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
|
||||
verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, raGyroConfigData);
|
||||
|
||||
if (gyro->lpf == 4) {
|
||||
verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
|
||||
} else if (gyro->lpf < 4) {
|
||||
verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
|
||||
} else if (gyro->lpf > 4) {
|
||||
verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF
|
||||
}
|
||||
|
||||
verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
|
||||
|
||||
verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
|
||||
verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||
|
||||
#if defined(USE_MPU_DATA_READY_SIGNAL)
|
||||
verifympu9250WriteRegister(MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
|
||||
#endif
|
||||
|
||||
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
|
||||
|
||||
mpuSpi9250InitDone = true; //init done
|
||||
}
|
||||
|
||||
bool mpu9250SpiDetect(void)
|
||||
{
|
||||
/* not the best place for this - should really have an init method */
|
||||
#ifdef MPU9250_CS_PIN
|
||||
mpuSpi9250CsPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN));
|
||||
#endif
|
||||
IOInit(mpuSpi9250CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
|
||||
IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG);
|
||||
|
||||
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
|
||||
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
||||
|
||||
uint8_t attemptsRemaining = 20;
|
||||
do {
|
||||
delay(150);
|
||||
uint8_t in;
|
||||
mpu9250ReadRegister(MPU_RA_WHO_AM_I, 1, &in);
|
||||
if (in == MPU9250_WHO_AM_I_CONST) {
|
||||
break;
|
||||
}
|
||||
if (!attemptsRemaining) {
|
||||
return false;
|
||||
}
|
||||
} while (attemptsRemaining--);
|
||||
|
||||
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void mpu9250SpiAccInit(accDev_t *acc)
|
||||
{
|
||||
acc->acc_1G = 512 * 8;
|
||||
}
|
||||
|
||||
bool mpu9250SpiAccDetect(accDev_t *acc)
|
||||
{
|
||||
if (acc->mpuDetectionResult.sensor != MPU_9250_SPI) {
|
||||
return false;
|
||||
}
|
||||
|
||||
acc->init = mpu9250SpiAccInit;
|
||||
acc->read = mpuAccRead;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void mpu9250SpiGyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
mpuGyroInit(gyro);
|
||||
|
||||
mpu9250AccAndGyroInit(gyro);
|
||||
|
||||
spiResetErrorCounter(MPU9250_SPI_INSTANCE);
|
||||
|
||||
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers
|
||||
|
||||
mpuGyroRead(gyro);
|
||||
|
||||
if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) {
|
||||
spiResetErrorCounter(MPU9250_SPI_INSTANCE);
|
||||
failureMode(FAILURE_GYRO_INIT_FAILED);
|
||||
}
|
||||
}
|
||||
|
||||
bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
|
||||
{
|
||||
if (gyro->mpuDetectionResult.sensor != MPU_9250_SPI) {
|
||||
return false;
|
||||
}
|
||||
|
||||
gyro->init = mpu9250SpiGyroInit;
|
||||
gyro->read = mpuGyroRead;
|
||||
gyro->intStatus = mpuCheckDataReady;
|
||||
|
||||
// 16.4 dps/lsb scalefactor
|
||||
gyro->scale = 1.0f / 16.4f;
|
||||
|
||||
return true;
|
||||
}
|
||||
36
src/main/drivers/accgyro_spi_mpu9250.h
Normal file
36
src/main/drivers/accgyro_spi_mpu9250.h
Normal file
|
|
@ -0,0 +1,36 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#define mpu9250_CONFIG 0x1A
|
||||
|
||||
/* We should probably use these. :)
|
||||
#define BITS_DLPF_CFG_256HZ 0x00
|
||||
#define BITS_DLPF_CFG_188HZ 0x01
|
||||
#define BITS_DLPF_CFG_98HZ 0x02
|
||||
#define BITS_DLPF_CFG_42HZ 0x03
|
||||
#define BITS_DLPF_CFG_20HZ 0x04
|
||||
#define BITS_DLPF_CFG_10HZ 0x05
|
||||
#define BITS_DLPF_CFG_5HZ 0x06
|
||||
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
|
||||
*/
|
||||
|
||||
#define GYRO_SCALE_FACTOR 0.00053292f // (4/131) * pi/180 (32.75 LSB = 1 DPS)
|
||||
|
||||
#define MPU9250_WHO_AM_I_CONST (0x71)
|
||||
|
||||
#define MPU9250_BIT_RESET (0x80)
|
||||
|
||||
// RF = Register Flag
|
||||
#define MPU_RF_DATA_RDY_EN (1 << 0)
|
||||
|
||||
void mpu9250ResetGyro(void);
|
||||
|
||||
bool mpu9250SpiDetect(void);
|
||||
|
||||
bool mpu9250SpiAccDetect(accDev_t *acc);
|
||||
bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
|
||||
|
||||
bool mpu9250WriteRegister(uint8_t reg, uint8_t data);
|
||||
bool verifympu9250WriteRegister(uint8_t reg, uint8_t data);
|
||||
bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
|
||||
bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
|
||||
|
|
@ -163,9 +163,9 @@ static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT
|
|||
|
||||
/* Sensor names (used in lookup tables for *_hardware settings and in status command output) */
|
||||
// sync with gyroSensor_e
|
||||
static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE"};
|
||||
static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "FAKE"};
|
||||
// sync with accelerationSensor_e
|
||||
static const char * const lookupTableAccHardware[] = { "NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE"};
|
||||
static const char * const lookupTableAccHardware[] = { "NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "FAKE"};
|
||||
#if (FLASH_SIZE > 64)
|
||||
// sync with baroSensor_e
|
||||
static const char * const lookupTableBaroHardware[] = { "NONE", "AUTO", "BMP085", "MS5611", "BMP280", "FAKE"};
|
||||
|
|
|
|||
|
|
@ -44,6 +44,7 @@
|
|||
#include "drivers/accgyro_lsm303dlhc.h"
|
||||
#include "drivers/accgyro_spi_mpu6000.h"
|
||||
#include "drivers/accgyro_spi_mpu6500.h"
|
||||
#include "drivers/accgyro_spi_mpu9250.h"
|
||||
#include "drivers/logging.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
||||
|
|
@ -94,7 +95,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
|
|||
accelerationSensor_e accHardware = ACC_NONE;
|
||||
|
||||
#ifdef USE_ACC_ADXL345
|
||||
drv_adxl345_config_t acc_params;
|
||||
#endif
|
||||
|
||||
dev->accAlign = ALIGN_DEFAULT;
|
||||
|
|
@ -104,8 +104,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
|
|||
switch (accHardwareToUse) {
|
||||
case ACC_AUTODETECT:
|
||||
; // fallthrough
|
||||
case ACC_ADXL345: // ADXL345
|
||||
#ifdef USE_ACC_ADXL345
|
||||
case ACC_ADXL345: {
|
||||
drv_adxl345_config_t acc_params;
|
||||
acc_params.useFifo = false;
|
||||
acc_params.dataRate = 800; // unused currently
|
||||
#ifdef NAZE
|
||||
|
|
@ -119,14 +120,15 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
|
|||
accHardware = ACC_ADXL345;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
|
||||
if (accHardwareToUse != ACC_AUTODETECT) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
case ACC_LSM303DLHC:
|
||||
#ifdef USE_ACC_LSM303DLHC
|
||||
case ACC_LSM303DLHC:
|
||||
if (lsm303dlhcAccDetect(dev)) {
|
||||
#ifdef ACC_LSM303DLHC_ALIGN
|
||||
dev->accAlign = ACC_LSM303DLHC_ALIGN;
|
||||
|
|
@ -134,14 +136,14 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
|
|||
accHardware = ACC_LSM303DLHC;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
|
||||
if (accHardwareToUse != ACC_AUTODETECT) {
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
case ACC_MPU6050: // MPU6050
|
||||
#ifdef USE_ACC_MPU6050
|
||||
case ACC_MPU6050: // MPU6050
|
||||
if (mpu6050AccDetect(dev)) {
|
||||
#ifdef ACC_MPU6050_ALIGN
|
||||
dev->accAlign = ACC_MPU6050_ALIGN;
|
||||
|
|
@ -149,14 +151,14 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
|
|||
accHardware = ACC_MPU6050;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
|
||||
if (accHardwareToUse != ACC_AUTODETECT) {
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
case ACC_MMA8452: // MMA8452
|
||||
#ifdef USE_ACC_MMA8452
|
||||
case ACC_MMA8452: // MMA8452
|
||||
#ifdef NAZE
|
||||
// Not supported with this frequency
|
||||
if (hardwareRevision < NAZE32_REV5 && mma8452Detect(dev)) {
|
||||
|
|
@ -169,14 +171,14 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
|
|||
accHardware = ACC_MMA8452;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
|
||||
if (accHardwareToUse != ACC_AUTODETECT) {
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
case ACC_BMA280: // BMA280
|
||||
#ifdef USE_ACC_BMA280
|
||||
case ACC_BMA280: // BMA280
|
||||
if (bma280Detect(dev)) {
|
||||
#ifdef ACC_BMA280_ALIGN
|
||||
dev->accAlign = ACC_BMA280_ALIGN;
|
||||
|
|
@ -184,14 +186,14 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
|
|||
accHardware = ACC_BMA280;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
|
||||
if (accHardwareToUse != ACC_AUTODETECT) {
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
case ACC_MPU6000:
|
||||
#ifdef USE_ACC_SPI_MPU6000
|
||||
case ACC_MPU6000:
|
||||
if (mpu6000SpiAccDetect(dev)) {
|
||||
#ifdef ACC_MPU6000_ALIGN
|
||||
dev->accAlign = ACC_MPU6000_ALIGN;
|
||||
|
|
@ -199,14 +201,14 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
|
|||
accHardware = ACC_MPU6000;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
|
||||
if (accHardwareToUse != ACC_AUTODETECT) {
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
case ACC_MPU6500:
|
||||
#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
|
||||
case ACC_MPU6500:
|
||||
#ifdef USE_ACC_SPI_MPU6500
|
||||
if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) {
|
||||
#else
|
||||
|
|
@ -218,24 +220,40 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
|
|||
accHardware = ACC_MPU6500;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
|
||||
if (accHardwareToUse != ACC_AUTODETECT) {
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_ACC_SPI_MPU9250)
|
||||
case ACC_MPU9250:
|
||||
if (mpu9250SpiAccDetect(dev)) {
|
||||
#ifdef ACC_MPU9250_ALIGN
|
||||
dev->accAlign = ACC_MPU9250_ALIGN;
|
||||
#endif
|
||||
accHardware = ACC_MPU9250;
|
||||
break;
|
||||
}
|
||||
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
|
||||
if (accHardwareToUse != ACC_AUTODETECT) {
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
case ACC_FAKE:
|
||||
#ifdef USE_FAKE_ACC
|
||||
case ACC_FAKE:
|
||||
if (fakeAccDetect(dev)) {
|
||||
accHardware = ACC_FAKE;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
/* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
|
||||
if (accHardwareToUse != ACC_AUTODETECT) {
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
default:
|
||||
case ACC_NONE: // disable ACC
|
||||
accHardware = ACC_NONE;
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -33,7 +33,8 @@ typedef enum {
|
|||
ACC_LSM303DLHC = 6,
|
||||
ACC_MPU6000 = 7,
|
||||
ACC_MPU6500 = 8,
|
||||
ACC_FAKE = 9,
|
||||
ACC_MPU9250 = 9,
|
||||
ACC_FAKE = 10,
|
||||
ACC_MAX = ACC_FAKE
|
||||
} accelerationSensor_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -45,6 +45,7 @@
|
|||
#include "drivers/accgyro_lsm303dlhc.h"
|
||||
#include "drivers/accgyro_spi_mpu6000.h"
|
||||
#include "drivers/accgyro_spi_mpu6500.h"
|
||||
#include "drivers/accgyro_spi_mpu9250.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/logging.h"
|
||||
|
|
@ -106,9 +107,13 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
|
|||
.gyro_soft_notch_cutoff_2 = 1
|
||||
);
|
||||
|
||||
#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)
|
||||
#if defined(USE_GYRO_MPU3050) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)
|
||||
#define USE_GYRO_MPU
|
||||
#endif
|
||||
|
||||
static const extiConfig_t *selectMPUIntExtiConfig(void)
|
||||
{
|
||||
#ifdef USE_GYRO_MPU
|
||||
#if defined(MPU_INT_EXTI)
|
||||
static const extiConfig_t mpuIntExtiConfig = { .tag = IO_TAG(MPU_INT_EXTI) };
|
||||
return &mpuIntExtiConfig;
|
||||
|
|
@ -116,9 +121,12 @@ static const extiConfig_t *selectMPUIntExtiConfig(void)
|
|||
return selectMPUIntExtiConfigByHardwareRevision();
|
||||
#else
|
||||
return NULL;
|
||||
#endif // MPU_INT_EXTI
|
||||
return NULL;
|
||||
#else
|
||||
return NULL;
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
|
||||
{
|
||||
|
|
@ -126,9 +134,10 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
|
|||
|
||||
switch(gyroHardware) {
|
||||
case GYRO_AUTODETECT:
|
||||
; // fallthrough
|
||||
case GYRO_MPU6050:
|
||||
// fallthrough
|
||||
|
||||
#ifdef USE_GYRO_MPU6050
|
||||
case GYRO_MPU6050:
|
||||
if (mpu6050GyroDetect(dev)) {
|
||||
gyroHardware = GYRO_MPU6050;
|
||||
#ifdef GYRO_MPU6050_ALIGN
|
||||
|
|
@ -136,10 +145,11 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
|
|||
#endif
|
||||
break;
|
||||
}
|
||||
// fallthrough
|
||||
#endif
|
||||
; // fallthrough
|
||||
case GYRO_L3G4200D:
|
||||
|
||||
#ifdef USE_GYRO_L3G4200D
|
||||
case GYRO_L3G4200D:
|
||||
if (l3g4200dDetect(dev)) {
|
||||
gyroHardware = GYRO_L3G4200D;
|
||||
#ifdef GYRO_L3G4200D_ALIGN
|
||||
|
|
@ -147,11 +157,11 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
|
|||
#endif
|
||||
break;
|
||||
}
|
||||
// fallthrough
|
||||
#endif
|
||||
; // fallthrough
|
||||
|
||||
case GYRO_MPU3050:
|
||||
#ifdef USE_GYRO_MPU3050
|
||||
case GYRO_MPU3050:
|
||||
if (mpu3050Detect(dev)) {
|
||||
gyroHardware = GYRO_MPU3050;
|
||||
#ifdef GYRO_MPU3050_ALIGN
|
||||
|
|
@ -159,11 +169,11 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
|
|||
#endif
|
||||
break;
|
||||
}
|
||||
// fallthrough
|
||||
#endif
|
||||
; // fallthrough
|
||||
|
||||
case GYRO_L3GD20:
|
||||
#ifdef USE_GYRO_L3GD20
|
||||
case GYRO_L3GD20:
|
||||
if (l3gd20Detect(dev)) {
|
||||
gyroHardware = GYRO_L3GD20;
|
||||
#ifdef GYRO_L3GD20_ALIGN
|
||||
|
|
@ -171,11 +181,11 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
|
|||
#endif
|
||||
break;
|
||||
}
|
||||
// fallthrough
|
||||
#endif
|
||||
; // fallthrough
|
||||
|
||||
case GYRO_MPU6000:
|
||||
#ifdef USE_GYRO_SPI_MPU6000
|
||||
case GYRO_MPU6000:
|
||||
if (mpu6000SpiGyroDetect(dev)) {
|
||||
gyroHardware = GYRO_MPU6000;
|
||||
#ifdef GYRO_MPU6000_ALIGN
|
||||
|
|
@ -183,11 +193,11 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
|
|||
#endif
|
||||
break;
|
||||
}
|
||||
// fallthrough
|
||||
#endif
|
||||
; // fallthrough
|
||||
|
||||
case GYRO_MPU6500:
|
||||
#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
|
||||
case GYRO_MPU6500:
|
||||
#ifdef USE_GYRO_SPI_MPU6500
|
||||
if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
|
||||
#else
|
||||
|
|
@ -199,17 +209,30 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
|
|||
#endif
|
||||
break;
|
||||
}
|
||||
// fallthrough
|
||||
#endif
|
||||
|
||||
#ifdef USE_GYRO_SPI_MPU9250
|
||||
case GYRO_MPU9250:
|
||||
if (mpu9250SpiGyroDetect(dev)) {
|
||||
gyroHardware = GYRO_MPU9250;
|
||||
#ifdef GYRO_MPU9250_ALIGN
|
||||
dev->gyroAlign = GYRO_MPU9250_ALIGN;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
// fallthrough
|
||||
#endif
|
||||
; // fallthrough
|
||||
|
||||
case GYRO_FAKE:
|
||||
#ifdef USE_FAKE_GYRO
|
||||
case GYRO_FAKE:
|
||||
if (fakeGyroDetect(dev)) {
|
||||
gyroHardware = GYRO_FAKE;
|
||||
break;
|
||||
}
|
||||
// fallthrough
|
||||
#endif
|
||||
; // fallthrough
|
||||
|
||||
default:
|
||||
case GYRO_NONE:
|
||||
gyroHardware = GYRO_NONE;
|
||||
|
|
@ -227,12 +250,10 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
|
|||
bool gyroInit(void)
|
||||
{
|
||||
memset(&gyro, 0, sizeof(gyro));
|
||||
#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)
|
||||
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
|
||||
#ifdef USE_GYRO_MPU
|
||||
mpuDetect(&gyro.dev);
|
||||
mpuReset = gyro.dev.mpuConfiguration.reset;
|
||||
#else
|
||||
const extiConfig_t *extiConfig = NULL;
|
||||
#endif
|
||||
gyro.dev.mpuIntExtiConfig = extiConfig;
|
||||
if (gyroDetect(&gyro.dev, GYRO_AUTODETECT) == GYRO_NONE) {
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@ typedef enum {
|
|||
GYRO_L3GD20,
|
||||
GYRO_MPU6000,
|
||||
GYRO_MPU6500,
|
||||
GYRO_MPU9250,
|
||||
GYRO_FAKE
|
||||
} gyroSensor_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -43,18 +43,16 @@
|
|||
#define USE_MPU_DATA_READY_SIGNAL
|
||||
//#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready
|
||||
#define MPU_INT_EXTI PC5
|
||||
#define MPU6500_CS_PIN PC4
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define MPU9250_CS_PIN PC4
|
||||
#define MPU9250_SPI_INSTANCE SPI1
|
||||
|
||||
#define ACC
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define ACC_MPU6500_ALIGN CW0_DEG
|
||||
#define USE_ACC_SPI_MPU9250
|
||||
#define ACC_MPU9250_ALIGN CW0_DEG
|
||||
|
||||
#define GYRO
|
||||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define GYRO_MPU6500_ALIGN CW0_DEG
|
||||
#define USE_GYRO_SPI_MPU9250
|
||||
#define GYRO_MPU9250_ALIGN CW0_DEG
|
||||
|
||||
#define MAG
|
||||
#define USE_MAG_AK8963
|
||||
|
|
|
|||
|
|
@ -2,8 +2,8 @@ F405_TARGETS += $(TARGET)
|
|||
FEATURES += SDCARD VCP ONBOARDFLASH
|
||||
|
||||
TARGET_SRC = \
|
||||
drivers/accgyro_spi_mpu6500.c \
|
||||
drivers/accgyro_mpu6500.c \
|
||||
drivers/accgyro_spi_mpu9250.c \
|
||||
drivers/barometer_bmp085.c \
|
||||
drivers/barometer_bmp280.c \
|
||||
drivers/barometer_ms5611.c \
|
||||
|
|
|
|||
|
|
@ -69,6 +69,7 @@ TEST(SensorGyro, Init)
|
|||
TEST(SensorGyro, Read)
|
||||
{
|
||||
gyroInit();
|
||||
EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
|
||||
fakeGyroSet(5, 6, 7);
|
||||
const bool read = gyro.dev.read(&gyro.dev);
|
||||
EXPECT_EQ(true, read);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue