mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 17:25:18 +03:00
Converted to using gyroDev_t and accDev_t as in betaflight
This commit is contained in:
Martin Budden
parent
6caf2152e8
commit
14ec437311
48 changed files with 291 additions and 275 deletions
|
|
@ -1313,8 +1313,8 @@ static bool blackboxWriteSysinfo()
|
|||
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", 100); //For compatibility reasons write rc_rate 100
|
||||
BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.motorConfig.minthrottle);
|
||||
BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.motorConfig.maxthrottle);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale));
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.acc_1G);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.dev.scale));
|
||||
BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.dev.acc_1G);
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
|
||||
|
|
@ -1336,7 +1336,7 @@ static bool blackboxWriteSysinfo()
|
|||
}
|
||||
);
|
||||
|
||||
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime);
|
||||
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.dev.targetLooptime);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcExpo8);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rcYawExpo:%d", masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcYawExpo8);
|
||||
BLACKBOX_PRINT_HEADER_LINE("thrMid:%d", masterConfig.controlRateProfiles[masterConfig.current_profile_index].thrMid8);
|
||||
|
|
|
|||
|
|
@ -623,7 +623,7 @@ bool blackboxDeviceOpen(void)
|
|||
* = floor((looptime_ns * 3) / 500.0)
|
||||
* = (looptime_ns * 3) / 500
|
||||
*/
|
||||
blackboxMaxHeaderBytesPerIteration = constrain((gyro.targetLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION);
|
||||
blackboxMaxHeaderBytesPerIteration = constrain((gyro.dev.targetLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION);
|
||||
|
||||
return blackboxPort != NULL;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -407,7 +407,7 @@ uint32_t getPidUpdateRate(void) {
|
|||
}
|
||||
|
||||
uint32_t getGyroUpdateRate(void) {
|
||||
return gyro.targetLooptime;
|
||||
return gyro.dev.targetLooptime;
|
||||
}
|
||||
|
||||
uint16_t getAccUpdateRate(void) {
|
||||
|
|
|
|||
|
|
@ -17,24 +17,37 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "sensor.h"
|
||||
#include "common/axis.h"
|
||||
#include "drivers/sensor.h"
|
||||
|
||||
#ifndef MPU_I2C_INSTANCE
|
||||
#define MPU_I2C_INSTANCE I2C_DEVICE
|
||||
#endif
|
||||
|
||||
typedef struct gyro_s {
|
||||
#define GYRO_LPF_256HZ 0
|
||||
#define GYRO_LPF_188HZ 1
|
||||
#define GYRO_LPF_98HZ 2
|
||||
#define GYRO_LPF_42HZ 3
|
||||
#define GYRO_LPF_20HZ 4
|
||||
#define GYRO_LPF_10HZ 5
|
||||
#define GYRO_LPF_5HZ 6
|
||||
#define GYRO_LPF_NONE 7
|
||||
|
||||
typedef struct gyroDev_s {
|
||||
sensorGyroInitFuncPtr init; // initialize function
|
||||
sensorReadFuncPtr read; // read 3 axis data function
|
||||
sensorGyroReadFuncPtr read; // read 3 axis data function
|
||||
sensorReadFuncPtr temperature; // read temperature if available
|
||||
sensorInterruptFuncPtr intStatus;
|
||||
sensorGyroInterruptStatusFuncPtr intStatus;
|
||||
float scale; // scalefactor
|
||||
uint32_t targetLooptime;
|
||||
} gyro_t;
|
||||
volatile bool dataReady;
|
||||
uint16_t lpf;
|
||||
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
||||
} gyroDev_t;
|
||||
|
||||
typedef struct acc_s {
|
||||
typedef struct accDev_s {
|
||||
sensorAccInitFuncPtr init; // initialize function
|
||||
sensorReadFuncPtr read; // read 3 axis data function
|
||||
uint16_t acc_1G;
|
||||
char revisionCode; // a revision code for the sensor, if known
|
||||
} acc_t;
|
||||
} accDev_t;
|
||||
|
|
|
|||
|
|
@ -56,12 +56,12 @@
|
|||
#define ADXL345_RANGE_16G 0x03
|
||||
#define ADXL345_FIFO_STREAM 0x80
|
||||
|
||||
static void adxl345Init(acc_t *acc);
|
||||
static void adxl345Init(accDev_t *acc);
|
||||
static bool adxl345Read(int16_t *accelData);
|
||||
|
||||
static bool useFifo = false;
|
||||
|
||||
bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc)
|
||||
bool adxl345Detect(accDev_t *acc, drv_adxl345_config_t *init)
|
||||
{
|
||||
bool ack = false;
|
||||
uint8_t sig = 0;
|
||||
|
|
@ -78,7 +78,7 @@ bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc)
|
|||
return true;
|
||||
}
|
||||
|
||||
static void adxl345Init(acc_t *acc)
|
||||
static void adxl345Init(accDev_t *acc)
|
||||
{
|
||||
if (useFifo) {
|
||||
uint8_t fifoDepth = 16;
|
||||
|
|
|
|||
|
|
@ -22,4 +22,4 @@ typedef struct drv_adxl345_config_s {
|
|||
uint16_t dataRate;
|
||||
} drv_adxl345_config_t;
|
||||
|
||||
bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc);
|
||||
bool adxl345Detect(accDev_t *acc, drv_adxl345_config_t *init);
|
||||
|
|
|
|||
|
|
@ -32,10 +32,10 @@
|
|||
#define BMA280_PMU_BW 0x10
|
||||
#define BMA280_PMU_RANGE 0x0F
|
||||
|
||||
static void bma280Init(acc_t *acc);
|
||||
static void bma280Init(accDev_t *acc);
|
||||
static bool bma280Read(int16_t *accelData);
|
||||
|
||||
bool bma280Detect(acc_t *acc)
|
||||
bool bma280Detect(accDev_t *acc)
|
||||
{
|
||||
bool ack = false;
|
||||
uint8_t sig = 0;
|
||||
|
|
@ -49,7 +49,7 @@ bool bma280Detect(acc_t *acc)
|
|||
return true;
|
||||
}
|
||||
|
||||
static void bma280Init(acc_t *acc)
|
||||
static void bma280Init(accDev_t *acc)
|
||||
{
|
||||
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
|
||||
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW
|
||||
|
|
|
|||
|
|
@ -17,4 +17,4 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
bool bma280Detect(acc_t *acc);
|
||||
bool bma280Detect(accDev_t *acc);
|
||||
|
|
|
|||
|
|
@ -31,9 +31,9 @@
|
|||
|
||||
static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
|
||||
|
||||
static void fakeGyroInit(uint8_t lpf)
|
||||
static void fakeGyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
UNUSED(lpf);
|
||||
UNUSED(gyro);
|
||||
}
|
||||
|
||||
void fakeGyroSet(int16_t x, int16_t y, int16_t z)
|
||||
|
|
@ -43,11 +43,11 @@ void fakeGyroSet(int16_t x, int16_t y, int16_t z)
|
|||
fakeGyroADC[Z] = z;
|
||||
}
|
||||
|
||||
static bool fakeGyroRead(int16_t *gyroADC)
|
||||
static bool fakeGyroRead(gyroDev_t *gyro)
|
||||
{
|
||||
gyroADC[X] = fakeGyroADC[X];
|
||||
gyroADC[Y] = fakeGyroADC[Y];
|
||||
gyroADC[Z] = fakeGyroADC[Z];
|
||||
gyro->gyroADCRaw[X] = fakeGyroADC[X];
|
||||
gyro->gyroADCRaw[Y] = fakeGyroADC[Y];
|
||||
gyro->gyroADCRaw[Z] = fakeGyroADC[Z];
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -57,12 +57,13 @@ static bool fakeGyroReadTemp(int16_t *tempData)
|
|||
return true;
|
||||
}
|
||||
|
||||
static bool fakeGyroInitStatus(void)
|
||||
static bool fakeGyroInitStatus(gyroDev_t *gyro)
|
||||
{
|
||||
UNUSED(gyro);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool fakeGyroDetect(gyro_t *gyro)
|
||||
bool fakeGyroDetect(gyroDev_t *gyro)
|
||||
{
|
||||
gyro->init = fakeGyroInit;
|
||||
gyro->intStatus = fakeGyroInitStatus;
|
||||
|
|
@ -78,7 +79,7 @@ bool fakeGyroDetect(gyro_t *gyro)
|
|||
|
||||
static int16_t fakeAccData[XYZ_AXIS_COUNT];
|
||||
|
||||
static void fakeAccInit(acc_t *acc)
|
||||
static void fakeAccInit(accDev_t *acc)
|
||||
{
|
||||
UNUSED(acc);
|
||||
}
|
||||
|
|
@ -98,7 +99,7 @@ static bool fakeAccRead(int16_t *accData)
|
|||
return true;
|
||||
}
|
||||
|
||||
bool fakeAccDetect(acc_t *acc)
|
||||
bool fakeAccDetect(accDev_t *acc)
|
||||
{
|
||||
acc->init = fakeAccInit;
|
||||
acc->read = fakeAccRead;
|
||||
|
|
|
|||
|
|
@ -17,10 +17,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
struct acc_s;
|
||||
bool fakeAccDetect(struct acc_s *acc);
|
||||
bool fakeAccDetect(accDev_t *acc);
|
||||
void fakeAccSet(int16_t x, int16_t y, int16_t z);
|
||||
|
||||
struct gyro_s;
|
||||
bool fakeGyroDetect(struct gyro_s *gyro);
|
||||
bool fakeGyroDetect(gyroDev_t *gyro);
|
||||
void fakeGyroSet(int16_t x, int16_t y, int16_t z);
|
||||
|
|
|
|||
|
|
@ -54,10 +54,10 @@
|
|||
#define L3G4200D_DLPF_78HZ 0x80
|
||||
#define L3G4200D_DLPF_93HZ 0xC0
|
||||
|
||||
static void l3g4200dInit(uint8_t lpf);
|
||||
static bool l3g4200dRead(int16_t *gyroADC);
|
||||
static void l3g4200dInit(gyroDev_t *gyro);
|
||||
static bool l3g4200dRead(gyroDev_t *gyro);
|
||||
|
||||
bool l3g4200dDetect(gyro_t *gyro)
|
||||
bool l3g4200dDetect(gyroDev_t *gyro)
|
||||
{
|
||||
uint8_t deviceid;
|
||||
|
||||
|
|
@ -76,13 +76,13 @@ bool l3g4200dDetect(gyro_t *gyro)
|
|||
return true;
|
||||
}
|
||||
|
||||
static void l3g4200dInit(uint8_t lpf)
|
||||
static void l3g4200dInit(gyroDev_t *gyro)
|
||||
{
|
||||
bool ack;
|
||||
|
||||
uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
|
||||
|
||||
switch (lpf) {
|
||||
switch (gyro->lpf) {
|
||||
default:
|
||||
case 3: // BITS_DLPF_CFG_42HZ
|
||||
mpuLowPassFilter = L3G4200D_DLPF_32HZ;
|
||||
|
|
@ -109,7 +109,7 @@ static void l3g4200dInit(uint8_t lpf)
|
|||
}
|
||||
|
||||
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
|
||||
static bool l3g4200dRead(int16_t *gyroADC)
|
||||
static bool l3g4200dRead(gyroDev_t *gyro)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
|
||||
|
|
@ -117,9 +117,9 @@ static bool l3g4200dRead(int16_t *gyroADC)
|
|||
return false;
|
||||
}
|
||||
|
||||
gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
|
||||
gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
|
||||
gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
|
||||
gyro->gyroADCRaw[X] = (int16_t)((buf[0] << 8) | buf[1]);
|
||||
gyro->gyroADCRaw[Y] = (int16_t)((buf[2] << 8) | buf[3]);
|
||||
gyro->gyroADCRaw[Z] = (int16_t)((buf[4] << 8) | buf[5]);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,4 +17,4 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
bool l3g4200dDetect(gyro_t *gyro);
|
||||
bool l3g4200dDetect(gyroDev_t *gyro);
|
||||
|
|
|
|||
|
|
@ -84,9 +84,9 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
|
|||
spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD);
|
||||
}
|
||||
|
||||
void l3gd20GyroInit(uint8_t lpf)
|
||||
void l3gd20GyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
UNUSED(lpf); // FIXME use it!
|
||||
UNUSED(gyro); // FIXME use it!
|
||||
|
||||
l3gd20SpiInit(L3GD20_SPI);
|
||||
|
||||
|
|
@ -120,7 +120,7 @@ void l3gd20GyroInit(uint8_t lpf)
|
|||
delay(100);
|
||||
}
|
||||
|
||||
static bool l3gd20GyroRead(int16_t *gyroADC)
|
||||
static bool l3gd20GyroRead(gyroDev_t *gyro)
|
||||
{
|
||||
uint8_t buf[6];
|
||||
|
||||
|
|
@ -134,9 +134,9 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
|
|||
|
||||
DISABLE_L3GD20;
|
||||
|
||||
gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
|
||||
gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
|
||||
gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
|
||||
gyro->gyroADCRaw[0] = (int16_t)((buf[0] << 8) | buf[1]);
|
||||
gyro->gyroADCRaw[1] = (int16_t)((buf[2] << 8) | buf[3]);
|
||||
gyro->gyroADCRaw[2] = (int16_t)((buf[4] << 8) | buf[5]);
|
||||
|
||||
#if 0
|
||||
debug[0] = (int16_t)((buf[1] << 8) | buf[0]);
|
||||
|
|
@ -150,7 +150,7 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
|
|||
// Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
|
||||
#define L3GD20_GYRO_SCALE_FACTOR 0.07f
|
||||
|
||||
bool l3gd20Detect(gyro_t *gyro)
|
||||
bool l3gd20Detect(gyroDev_t *gyro)
|
||||
{
|
||||
gyro->init = l3gd20GyroInit;
|
||||
gyro->read = l3gd20GyroRead;
|
||||
|
|
|
|||
|
|
@ -17,4 +17,4 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
bool l3gd20Detect(gyro_t *gyro);
|
||||
bool l3gd20Detect(gyroDev_t *gyro);
|
||||
|
|
|
|||
|
|
@ -115,7 +115,7 @@ int32_t accelSummedSamples100Hz[3];
|
|||
|
||||
int32_t accelSummedSamples500Hz[3];
|
||||
|
||||
void lsm303dlhcAccInit(acc_t *acc)
|
||||
void lsm303dlhcAccInit(accDev_t *acc)
|
||||
{
|
||||
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT);
|
||||
|
||||
|
|
@ -157,7 +157,7 @@ static bool lsm303dlhcAccRead(int16_t *gyroADC)
|
|||
return true;
|
||||
}
|
||||
|
||||
bool lsm303dlhcAccDetect(acc_t *acc)
|
||||
bool lsm303dlhcAccDetect(accDev_t *acc)
|
||||
{
|
||||
bool ack;
|
||||
uint8_t status;
|
||||
|
|
|
|||
|
|
@ -195,10 +195,10 @@ typedef struct {
|
|||
/** @defgroup Acc_Full_Scale_Selection
|
||||
* @{
|
||||
*/
|
||||
#define LSM303DLHC_FULLSCALE_2G ((uint8_t)0x00) /*!< ±2 g */
|
||||
#define LSM303DLHC_FULLSCALE_4G ((uint8_t)0x10) /*!< ±4 g */
|
||||
#define LSM303DLHC_FULLSCALE_8G ((uint8_t)0x20) /*!< ±8 g */
|
||||
#define LSM303DLHC_FULLSCALE_16G ((uint8_t)0x30) /*!< ±16 g */
|
||||
#define LSM303DLHC_FULLSCALE_2G ((uint8_t)0x00) /*!< +/-2 g */
|
||||
#define LSM303DLHC_FULLSCALE_4G ((uint8_t)0x10) /*!< +/-4 g */
|
||||
#define LSM303DLHC_FULLSCALE_8G ((uint8_t)0x20) /*!< +/-8 g */
|
||||
#define LSM303DLHC_FULLSCALE_16G ((uint8_t)0x30) /*!< +/-16 g */
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
|
@ -388,13 +388,13 @@ typedef struct {
|
|||
/** @defgroup Mag_Full_Scale
|
||||
* @{
|
||||
*/
|
||||
#define LSM303DLHC_FS_1_3_GA ((uint8_t) 0x20) /*!< Full scale = ±1.3 Gauss */
|
||||
#define LSM303DLHC_FS_1_9_GA ((uint8_t) 0x40) /*!< Full scale = ±1.9 Gauss */
|
||||
#define LSM303DLHC_FS_2_5_GA ((uint8_t) 0x60) /*!< Full scale = ±2.5 Gauss */
|
||||
#define LSM303DLHC_FS_4_0_GA ((uint8_t) 0x80) /*!< Full scale = ±4.0 Gauss */
|
||||
#define LSM303DLHC_FS_4_7_GA ((uint8_t) 0xA0) /*!< Full scale = ±4.7 Gauss */
|
||||
#define LSM303DLHC_FS_5_6_GA ((uint8_t) 0xC0) /*!< Full scale = ±5.6 Gauss */
|
||||
#define LSM303DLHC_FS_8_1_GA ((uint8_t) 0xE0) /*!< Full scale = ±8.1 Gauss */
|
||||
#define LSM303DLHC_FS_1_3_GA ((uint8_t) 0x20) /*!< Full scale = +/-1.3 Gauss */
|
||||
#define LSM303DLHC_FS_1_9_GA ((uint8_t) 0x40) /*!< Full scale = +/-1.9 Gauss */
|
||||
#define LSM303DLHC_FS_2_5_GA ((uint8_t) 0x60) /*!< Full scale = +/-2.5 Gauss */
|
||||
#define LSM303DLHC_FS_4_0_GA ((uint8_t) 0x80) /*!< Full scale = +/-4.0 Gauss */
|
||||
#define LSM303DLHC_FS_4_7_GA ((uint8_t) 0xA0) /*!< Full scale = +/-4.7 Gauss */
|
||||
#define LSM303DLHC_FS_5_6_GA ((uint8_t) 0xC0) /*!< Full scale = +/-5.6 Gauss */
|
||||
#define LSM303DLHC_FS_8_1_GA ((uint8_t) 0xE0) /*!< Full scale = +/-8.1 Gauss */
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
|
@ -438,5 +438,5 @@ typedef struct {
|
|||
#define LSM303DLHC_TEMPSENSOR_DISABLE ((uint8_t) 0x00) /*!< Temp sensor Disable */
|
||||
|
||||
|
||||
bool lsm303dlhcAccDetect(acc_t *acc);
|
||||
bool lsm303dlhcAccDetect(accDev_t *acc);
|
||||
|
||||
|
|
|
|||
|
|
@ -81,10 +81,10 @@
|
|||
|
||||
static uint8_t device_id;
|
||||
|
||||
static void mma8452Init(acc_t *acc);
|
||||
static void mma8452Init(accDev_t *acc);
|
||||
static bool mma8452Read(int16_t *accelData);
|
||||
|
||||
bool mma8452Detect(acc_t *acc)
|
||||
bool mma8452Detect(accDev_t *acc)
|
||||
{
|
||||
bool ack = false;
|
||||
uint8_t sig = 0;
|
||||
|
|
@ -114,7 +114,7 @@ static inline void mma8451ConfigureInterrupt(void)
|
|||
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
|
||||
}
|
||||
|
||||
static void mma8452Init(acc_t *acc)
|
||||
static void mma8452Init(accDev_t *acc)
|
||||
{
|
||||
|
||||
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff
|
||||
|
|
|
|||
|
|
@ -17,4 +17,4 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
bool mma8452Detect(acc_t *acc);
|
||||
bool mma8452Detect(accDev_t *acc);
|
||||
|
|
|
|||
|
|
@ -51,8 +51,6 @@ static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
|
|||
|
||||
static void mpu6050FindRevision(void);
|
||||
|
||||
static volatile bool mpuDataReady;
|
||||
|
||||
#ifdef USE_SPI
|
||||
static bool detectSPISensorsAndUpdateDetectionResult(void);
|
||||
#endif
|
||||
|
|
@ -144,6 +142,16 @@ static bool detectSPISensorsAndUpdateDetectionResult(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_GYRO_SPI_ICM20689
|
||||
if (icm20689SpiDetect()) {
|
||||
mpuDetectionResult.sensor = ICM_20689_SPI;
|
||||
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||
mpuConfiguration.read = icm20689ReadRegister;
|
||||
mpuConfiguration.write = icm20689WriteRegister;
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_GYRO_SPI_MPU6000
|
||||
if (mpu6000SpiDetect()) {
|
||||
mpuDetectionResult.sensor = MPU_60x0_SPI;
|
||||
|
|
@ -210,12 +218,21 @@ static void mpu6050FindRevision(void)
|
|||
}
|
||||
}
|
||||
|
||||
extiCallbackRec_t mpuIntCallbackRec;
|
||||
typedef struct mpuIntRec_s {
|
||||
extiCallbackRec_t exti;
|
||||
gyroDev_t *gyro;
|
||||
} mpuIntRec_t;
|
||||
|
||||
void mpuIntExtiHandler(extiCallbackRec_t *cb)
|
||||
mpuIntRec_t mpuIntRec;
|
||||
|
||||
/*
|
||||
* Gyro interrupt service routine
|
||||
*/
|
||||
static void mpuIntExtiHandler(extiCallbackRec_t *cb)
|
||||
{
|
||||
UNUSED(cb);
|
||||
mpuDataReady = true;
|
||||
mpuIntRec_t *rec = container_of(cb, mpuIntRec_t, exti);
|
||||
gyroDev_t *gyro = rec->gyro;
|
||||
gyro->dataReady = true;
|
||||
|
||||
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT
|
||||
static uint32_t lastCalledAt = 0;
|
||||
|
|
@ -226,7 +243,7 @@ void mpuIntExtiHandler(extiCallbackRec_t *cb)
|
|||
#endif
|
||||
}
|
||||
|
||||
void mpuIntExtiInit(void)
|
||||
static void mpuIntExtiInit(gyroDev_t *gyro)
|
||||
{
|
||||
static bool mpuExtiInitDone = false;
|
||||
|
||||
|
|
@ -234,6 +251,7 @@ void mpuIntExtiInit(void)
|
|||
return;
|
||||
}
|
||||
|
||||
mpuIntRec.gyro = gyro;
|
||||
#if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)
|
||||
|
||||
IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag);
|
||||
|
|
@ -245,12 +263,19 @@ void mpuIntExtiInit(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#if defined (STM32F7)
|
||||
IOInit(mpuIntIO, OWNER_MPU, RESOURCE_EXTI, 0);
|
||||
EXTIHandlerInit(&mpuIntRec.exti, mpuIntExtiHandler);
|
||||
EXTIConfig(mpuIntIO, &mpuIntRec.exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL)); // TODO - maybe pullup / pulldown ?
|
||||
#else
|
||||
|
||||
IOInit(mpuIntIO, OWNER_MPU, RESOURCE_EXTI, 0);
|
||||
IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
|
||||
|
||||
EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
|
||||
EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
|
||||
EXTIHandlerInit(&mpuIntRec.exti, mpuIntExtiHandler);
|
||||
EXTIConfig(mpuIntIO, &mpuIntRec.exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
|
||||
EXTIEnable(mpuIntIO, true);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
mpuExtiInitDone = true;
|
||||
|
|
@ -284,28 +309,33 @@ bool mpuAccRead(int16_t *accData)
|
|||
return true;
|
||||
}
|
||||
|
||||
bool mpuGyroRead(int16_t *gyroADC)
|
||||
bool mpuGyroRead(gyroDev_t *gyro)
|
||||
{
|
||||
uint8_t data[6];
|
||||
|
||||
bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
|
||||
const bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
|
||||
if (!ack) {
|
||||
return false;
|
||||
}
|
||||
|
||||
gyroADC[0] = (int16_t)((data[0] << 8) | data[1]);
|
||||
gyroADC[1] = (int16_t)((data[2] << 8) | data[3]);
|
||||
gyroADC[2] = (int16_t)((data[4] << 8) | data[5]);
|
||||
gyro->gyroADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
|
||||
gyro->gyroADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
|
||||
gyro->gyroADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool checkMPUDataReady(void)
|
||||
void mpuGyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
mpuIntExtiInit(gyro);
|
||||
}
|
||||
|
||||
bool checkMPUDataReady(gyroDev_t* gyro)
|
||||
{
|
||||
bool ret;
|
||||
if (mpuDataReady) {
|
||||
if (gyro->dataReady) {
|
||||
ret = true;
|
||||
mpuDataReady= false;
|
||||
gyro->dataReady= false;
|
||||
} else {
|
||||
ret = false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,7 +17,6 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "io_types.h"
|
||||
#include "exti.h"
|
||||
|
||||
// MPU6050
|
||||
|
|
@ -168,7 +167,8 @@ typedef enum {
|
|||
MPU_60x0_SPI,
|
||||
MPU_65xx_I2C,
|
||||
MPU_65xx_SPI,
|
||||
MPU_9250_SPI
|
||||
MPU_9250_SPI,
|
||||
ICM_20689_SPI
|
||||
} detectedMPUSensor_e;
|
||||
|
||||
typedef enum {
|
||||
|
|
@ -184,8 +184,9 @@ typedef struct mpuDetectionResult_s {
|
|||
extern mpuDetectionResult_t mpuDetectionResult;
|
||||
|
||||
void configureMPUDataReadyInterruptHandling(void);
|
||||
void mpuIntExtiInit(void);
|
||||
struct gyroDev_s;
|
||||
void mpuGyroInit(struct gyroDev_s *gyro);
|
||||
bool mpuAccRead(int16_t *accData);
|
||||
bool mpuGyroRead(int16_t *gyroADC);
|
||||
bool mpuGyroRead(struct gyroDev_s *gyro);
|
||||
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
|
||||
bool checkMPUDataReady(void);
|
||||
bool checkMPUDataReady(struct gyroDev_s *gyro);
|
||||
|
|
|
|||
|
|
@ -46,10 +46,10 @@
|
|||
#define MPU3050_USER_RESET 0x01
|
||||
#define MPU3050_CLK_SEL_PLL_GX 0x01
|
||||
|
||||
static void mpu3050Init(uint8_t lpf);
|
||||
static void mpu3050Init(gyroDev_t *gyro);
|
||||
static bool mpu3050ReadTemp(int16_t *tempData);
|
||||
|
||||
bool mpu3050Detect(gyro_t *gyro)
|
||||
bool mpu3050Detect(gyroDev_t *gyro)
|
||||
{
|
||||
if (mpuDetectionResult.sensor != MPU_3050) {
|
||||
return false;
|
||||
|
|
@ -65,7 +65,7 @@ bool mpu3050Detect(gyro_t *gyro)
|
|||
return true;
|
||||
}
|
||||
|
||||
static void mpu3050Init(uint8_t lpf)
|
||||
static void mpu3050Init(gyroDev_t *gyro)
|
||||
{
|
||||
bool ack;
|
||||
|
||||
|
|
@ -75,7 +75,7 @@ static void mpu3050Init(uint8_t lpf)
|
|||
if (!ack)
|
||||
failureMode(FAILURE_ACC_INIT);
|
||||
|
||||
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | lpf);
|
||||
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
|
||||
mpuConfiguration.write(MPU3050_INT_CFG, 0);
|
||||
mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
|
||||
mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
|
||||
|
|
|
|||
|
|
@ -26,4 +26,4 @@
|
|||
#define MPU3050_USER_CTRL 0x3D
|
||||
#define MPU3050_PWR_MGM 0x3E
|
||||
|
||||
bool mpu3050Detect(gyro_t *gyro);
|
||||
bool mpu3050Detect(gyroDev_t *gyro);
|
||||
|
|
|
|||
|
|
@ -50,10 +50,10 @@
|
|||
|
||||
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
|
||||
|
||||
static void mpu6050AccInit(acc_t *acc);
|
||||
static void mpu6050GyroInit(uint8_t lpf);
|
||||
static void mpu6050AccInit(accDev_t *acc);
|
||||
static void mpu6050GyroInit(gyroDev_t *gyro);
|
||||
|
||||
bool mpu6050AccDetect(acc_t *acc)
|
||||
bool mpu6050AccDetect(accDev_t *acc)
|
||||
{
|
||||
if (mpuDetectionResult.sensor != MPU_60x0) {
|
||||
return false;
|
||||
|
|
@ -66,7 +66,7 @@ bool mpu6050AccDetect(acc_t *acc)
|
|||
return true;
|
||||
}
|
||||
|
||||
bool mpu6050GyroDetect(gyro_t *gyro)
|
||||
bool mpu6050GyroDetect(gyroDev_t *gyro)
|
||||
{
|
||||
if (mpuDetectionResult.sensor != MPU_60x0) {
|
||||
return false;
|
||||
|
|
@ -81,13 +81,11 @@ bool mpu6050GyroDetect(gyro_t *gyro)
|
|||
return true;
|
||||
}
|
||||
|
||||
static void mpu6050AccInit(acc_t *acc)
|
||||
static void mpu6050AccInit(accDev_t *acc)
|
||||
{
|
||||
mpuIntExtiInit();
|
||||
|
||||
switch (mpuDetectionResult.resolution) {
|
||||
case MPU_HALF_RESOLUTION:
|
||||
acc->acc_1G = 256 * 8;
|
||||
acc->acc_1G = 256 * 4;
|
||||
break;
|
||||
case MPU_FULL_RESOLUTION:
|
||||
acc->acc_1G = 512 * 8;
|
||||
|
|
@ -95,18 +93,16 @@ static void mpu6050AccInit(acc_t *acc)
|
|||
}
|
||||
}
|
||||
|
||||
static void mpu6050GyroInit(uint8_t lpf)
|
||||
static void mpu6050GyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
bool ack;
|
||||
mpuGyroInit(gyro);
|
||||
|
||||
mpuIntExtiInit();
|
||||
|
||||
ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
|
||||
bool ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
|
||||
delay(100);
|
||||
ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
|
||||
ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
|
||||
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
|
||||
ack = mpuConfiguration.write(MPU_RA_CONFIG, lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
|
||||
ack = mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
|
||||
ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
|
||||
|
||||
// ACC Init stuff.
|
||||
|
|
|
|||
|
|
@ -17,5 +17,5 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
bool mpu6050AccDetect(acc_t *acc);
|
||||
bool mpu6050GyroDetect(gyro_t *gyro);
|
||||
bool mpu6050AccDetect(accDev_t *acc);
|
||||
bool mpu6050GyroDetect(gyroDev_t *gyro);
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@
|
|||
#include "accgyro_mpu.h"
|
||||
#include "accgyro_mpu6500.h"
|
||||
|
||||
bool mpu6500AccDetect(acc_t *acc)
|
||||
bool mpu6500AccDetect(accDev_t *acc)
|
||||
{
|
||||
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
|
||||
return false;
|
||||
|
|
@ -46,7 +46,7 @@ bool mpu6500AccDetect(acc_t *acc)
|
|||
return true;
|
||||
}
|
||||
|
||||
bool mpu6500GyroDetect(gyro_t *gyro)
|
||||
bool mpu6500GyroDetect(gyroDev_t *gyro)
|
||||
{
|
||||
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
|
||||
return false;
|
||||
|
|
@ -62,16 +62,14 @@ bool mpu6500GyroDetect(gyro_t *gyro)
|
|||
return true;
|
||||
}
|
||||
|
||||
void mpu6500AccInit(acc_t *acc)
|
||||
void mpu6500AccInit(accDev_t *acc)
|
||||
{
|
||||
mpuIntExtiInit();
|
||||
|
||||
acc->acc_1G = 512 * 8;
|
||||
acc->acc_1G = 512 * 4;
|
||||
}
|
||||
|
||||
void mpu6500GyroInit(uint8_t lpf)
|
||||
void mpu6500GyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
mpuIntExtiInit();
|
||||
mpuGyroInit(gyro);
|
||||
|
||||
#ifdef NAZE
|
||||
// FIXME target specific code in driver code.
|
||||
|
|
@ -98,7 +96,7 @@ void mpu6500GyroInit(uint8_t lpf)
|
|||
delay(15);
|
||||
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
|
||||
delay(15);
|
||||
mpuConfiguration.write(MPU_RA_CONFIG, lpf);
|
||||
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
|
||||
delay(15);
|
||||
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); // Get Divider
|
||||
delay(100);
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@
|
|||
#define MPU6500_WHO_AM_I_CONST (0x70)
|
||||
#define MPU9250_WHO_AM_I_CONST (0x71)
|
||||
#define ICM20608G_WHO_AM_I_CONST (0xAF)
|
||||
#define ICM20602_WHO_AM_I_CONST (0x12)
|
||||
|
||||
#define MPU6500_BIT_RESET (0x80)
|
||||
#define MPU6500_BIT_INT_ANYRD_2CLEAR (1 << 4)
|
||||
|
|
@ -27,8 +28,8 @@
|
|||
#define MPU6500_BIT_I2C_IF_DIS (1 << 4)
|
||||
#define MPU6500_BIT_RAW_RDY_EN (0x01)
|
||||
|
||||
bool mpu6500AccDetect(acc_t *acc);
|
||||
bool mpu6500GyroDetect(gyro_t *gyro);
|
||||
bool mpu6500AccDetect(accDev_t *acc);
|
||||
bool mpu6500GyroDetect(gyroDev_t *gyro);
|
||||
|
||||
void mpu6500AccInit(acc_t *acc);
|
||||
void mpu6500GyroInit(uint8_t lpf);
|
||||
void mpu6500AccInit(accDev_t *acc);
|
||||
void mpu6500GyroInit(gyroDev_t *gyro);
|
||||
|
|
|
|||
|
|
@ -124,32 +124,29 @@ bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
|||
return true;
|
||||
}
|
||||
|
||||
void mpu6000SpiGyroInit(uint8_t lpf)
|
||||
void mpu6000SpiGyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
mpuIntExtiInit();
|
||||
mpuGyroInit(gyro);
|
||||
|
||||
mpu6000AccAndGyroInit();
|
||||
|
||||
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
|
||||
|
||||
// Accel and Gyro DLPF Setting
|
||||
mpu6000WriteRegister(MPU6000_CONFIG, lpf);
|
||||
mpu6000WriteRegister(MPU6000_CONFIG, gyro->lpf);
|
||||
delayMicroseconds(1);
|
||||
|
||||
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock
|
||||
|
||||
int16_t data[3];
|
||||
mpuGyroRead(data);
|
||||
mpuGyroRead(gyro);
|
||||
|
||||
if (((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) {
|
||||
if (((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) {
|
||||
failureMode(FAILURE_GYRO_INIT_FAILED);
|
||||
}
|
||||
}
|
||||
|
||||
void mpu6000SpiAccInit(acc_t *acc)
|
||||
void mpu6000SpiAccInit(accDev_t *acc)
|
||||
{
|
||||
mpuIntExtiInit();
|
||||
|
||||
acc->acc_1G = 512 * 8;
|
||||
}
|
||||
|
||||
|
|
@ -180,7 +177,6 @@ bool mpu6000SpiDetect(void)
|
|||
}
|
||||
} while (attemptsRemaining--);
|
||||
|
||||
|
||||
mpu6000ReadRegister(MPU_RA_PRODUCT_ID, 1, &in);
|
||||
|
||||
/* look for a product ID we recognise */
|
||||
|
|
@ -207,7 +203,6 @@ bool mpu6000SpiDetect(void)
|
|||
|
||||
static void mpu6000AccAndGyroInit(void)
|
||||
{
|
||||
|
||||
if (mpuSpi6000InitDone) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -245,7 +240,6 @@ static void mpu6000AccAndGyroInit(void)
|
|||
mpu6000WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
|
||||
delayMicroseconds(15);
|
||||
|
||||
|
||||
mpu6000WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR
|
||||
delayMicroseconds(15);
|
||||
|
||||
|
|
@ -260,7 +254,7 @@ static void mpu6000AccAndGyroInit(void)
|
|||
mpuSpi6000InitDone = true;
|
||||
}
|
||||
|
||||
bool mpu6000SpiAccDetect(acc_t *acc)
|
||||
bool mpu6000SpiAccDetect(accDev_t *acc)
|
||||
{
|
||||
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
|
||||
return false;
|
||||
|
|
@ -272,7 +266,7 @@ bool mpu6000SpiAccDetect(acc_t *acc)
|
|||
return true;
|
||||
}
|
||||
|
||||
bool mpu6000SpiGyroDetect(gyro_t *gyro)
|
||||
bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
|
||||
{
|
||||
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
|
||||
return false;
|
||||
|
|
|
|||
|
|
@ -17,8 +17,8 @@
|
|||
|
||||
bool mpu6000SpiDetect(void);
|
||||
|
||||
bool mpu6000SpiAccDetect(acc_t *acc);
|
||||
bool mpu6000SpiGyroDetect(gyro_t *gyro);
|
||||
bool mpu6000SpiAccDetect(accDev_t *acc);
|
||||
bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
|
||||
|
||||
bool mpu6000WriteRegister(uint8_t reg, uint8_t data);
|
||||
bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
|
||||
|
|
|
|||
|
|
@ -84,19 +84,22 @@ bool mpu6500SpiDetect(void)
|
|||
|
||||
mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp);
|
||||
|
||||
if (tmp == MPU6500_WHO_AM_I_CONST || tmp == MPU9250_WHO_AM_I_CONST || tmp == ICM20608G_WHO_AM_I_CONST) {
|
||||
if (tmp == MPU6500_WHO_AM_I_CONST ||
|
||||
tmp == MPU9250_WHO_AM_I_CONST ||
|
||||
tmp == ICM20608G_WHO_AM_I_CONST ||
|
||||
tmp == ICM20602_WHO_AM_I_CONST) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void mpu6500SpiAccInit(acc_t *acc)
|
||||
void mpu6500SpiAccInit(accDev_t *acc)
|
||||
{
|
||||
mpu6500AccInit(acc);
|
||||
}
|
||||
|
||||
bool mpu6500SpiAccDetect(acc_t *acc)
|
||||
bool mpu6500SpiAccDetect(accDev_t *acc)
|
||||
{
|
||||
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
|
||||
return false;
|
||||
|
|
@ -108,12 +111,12 @@ bool mpu6500SpiAccDetect(acc_t *acc)
|
|||
return true;
|
||||
}
|
||||
|
||||
void mpu6500SpiGyroInit(uint8_t lpf)
|
||||
void mpu6500SpiGyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
|
||||
delayMicroseconds(1);
|
||||
|
||||
mpu6500GyroInit(lpf);
|
||||
mpu6500GyroInit(gyro);
|
||||
|
||||
// Disable Primary I2C Interface
|
||||
mpu6500WriteRegister(MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
|
||||
|
|
@ -123,7 +126,7 @@ void mpu6500SpiGyroInit(uint8_t lpf)
|
|||
delayMicroseconds(1);
|
||||
}
|
||||
|
||||
bool mpu6500SpiGyroDetect(gyro_t *gyro)
|
||||
bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
|
||||
{
|
||||
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
|
||||
return false;
|
||||
|
|
|
|||
|
|
@ -19,11 +19,11 @@
|
|||
|
||||
bool mpu6500SpiDetect(void);
|
||||
|
||||
bool mpu6500SpiAccDetect(acc_t *acc);
|
||||
bool mpu6500SpiGyroDetect(gyro_t *gyro);
|
||||
bool mpu6500SpiAccDetect(accDev_t *acc);
|
||||
bool mpu6500SpiGyroDetect(gyroDev_t *gyro);
|
||||
|
||||
bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
|
||||
bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
|
||||
|
||||
void mpu6500SpiGyroInit(uint8_t lpf);
|
||||
void mpu6500SpiAccInit(acc_t *acc);
|
||||
void mpu6500SpiGyroInit(gyroDev_t *gyro);
|
||||
void mpu6500SpiAccInit(accDev_t *acc);
|
||||
|
|
|
|||
|
|
@ -96,31 +96,26 @@ bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
|||
return true;
|
||||
}
|
||||
|
||||
void mpu9250SpiGyroInit(uint8_t lpf)
|
||||
void mpu9250SpiGyroInit(gyroDev_t *gyro)
|
||||
{
|
||||
(void)(lpf);
|
||||
mpuGyroInit(gyro);
|
||||
|
||||
mpuIntExtiInit();
|
||||
|
||||
mpu9250AccAndGyroInit(lpf);
|
||||
mpu9250AccAndGyroInit(gyro->lpf);
|
||||
|
||||
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
|
||||
|
||||
int16_t data[3];
|
||||
mpuGyroRead(data);
|
||||
mpuGyroRead(gyro);
|
||||
|
||||
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) {
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
void mpu9250SpiAccInit(acc_t *acc)
|
||||
void mpu9250SpiAccInit(accDev_t *acc)
|
||||
{
|
||||
mpuIntExtiInit();
|
||||
|
||||
acc->acc_1G = 512 * 8;
|
||||
}
|
||||
|
||||
|
|
@ -214,7 +209,7 @@ bool mpu9250SpiDetect(void)
|
|||
return true;
|
||||
}
|
||||
|
||||
bool mpu9250SpiAccDetect(acc_t *acc)
|
||||
bool mpu9250SpiAccDetect(accDev_t *acc)
|
||||
{
|
||||
if (mpuDetectionResult.sensor != MPU_9250_SPI) {
|
||||
return false;
|
||||
|
|
@ -226,7 +221,7 @@ bool mpu9250SpiAccDetect(acc_t *acc)
|
|||
return true;
|
||||
}
|
||||
|
||||
bool mpu9250SpiGyroDetect(gyro_t *gyro)
|
||||
bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
|
||||
{
|
||||
if (mpuDetectionResult.sensor != MPU_9250_SPI) {
|
||||
return false;
|
||||
|
|
|
|||
|
|
@ -27,8 +27,8 @@ void mpu9250ResetGyro(void);
|
|||
|
||||
bool mpu9250SpiDetect(void);
|
||||
|
||||
bool mpu9250SpiAccDetect(acc_t *acc);
|
||||
bool mpu9250SpiGyroDetect(gyro_t *gyro);
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -14,37 +14,23 @@
|
|||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "platform.h"
|
||||
#include "build/build_config.h"
|
||||
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "config/config.h"
|
||||
|
||||
extern gyro_t gyro;
|
||||
#include "sensor.h"
|
||||
#include "accgyro.h"
|
||||
#include "gyro_sync.h"
|
||||
|
||||
static uint8_t mpuDividerDrops;
|
||||
|
||||
bool getMpuDataStatus(gyro_t *gyro)
|
||||
bool gyroSyncCheckUpdate(gyroDev_t *gyro)
|
||||
{
|
||||
return gyro->intStatus();
|
||||
}
|
||||
|
||||
bool gyroSyncCheckUpdate(void)
|
||||
{
|
||||
return getMpuDataStatus(&gyro);
|
||||
if (!gyro->intStatus)
|
||||
return false;
|
||||
return gyro->intStatus(gyro);
|
||||
}
|
||||
|
||||
uint32_t gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator)
|
||||
|
|
|
|||
|
|
@ -15,8 +15,7 @@
|
|||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#define INTERRUPT_WAIT_TIME 10
|
||||
|
||||
bool gyroSyncCheckUpdate(void);
|
||||
struct gyroDev_s;
|
||||
bool gyroSyncCheckUpdate(struct gyroDev_s *gyro);
|
||||
uint8_t gyroMPU6xxxCalculateDivider(void);
|
||||
uint32_t gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);
|
||||
|
|
|
|||
|
|
@ -17,9 +17,12 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
struct acc_s;
|
||||
struct accDev_s;
|
||||
typedef bool (*sensorInitFuncPtr)(void); // sensor init prototype
|
||||
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
||||
typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype
|
||||
typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype
|
||||
typedef bool (*sensorInterruptFuncPtr)(void); // sensor Interrupt Data Ready
|
||||
typedef void (*sensorAccInitFuncPtr)(struct accDev_s *acc); // sensor init prototype
|
||||
struct gyroDev_s;
|
||||
typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro);
|
||||
typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro);
|
||||
typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro);
|
||||
typedef bool (*sensorInterruptFuncPtr)(void);
|
||||
|
|
|
|||
|
|
@ -537,7 +537,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
case MSP_RAW_IMU:
|
||||
{
|
||||
// Hack scale due to choice of units for sensor data in multiwii
|
||||
const uint8_t scale = (acc.acc_1G > 1024) ? 8 : 1;
|
||||
const uint8_t scale = (acc.dev.acc_1G > 1024) ? 8 : 1;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
sbufWriteU16(dst, accADC[i] / scale);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -274,7 +274,7 @@ void fcTasksInit(void)
|
|||
}
|
||||
|
||||
#else
|
||||
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
|
||||
rescheduleTask(TASK_GYROPID, gyro.dev.targetLooptime);
|
||||
setTaskEnabled(TASK_GYROPID, true);
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -182,8 +182,8 @@ void annexCode(void)
|
|||
}
|
||||
|
||||
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
|
||||
if (gyro.temperature)
|
||||
gyro.temperature(&telemTemperature1);
|
||||
if (gyro.dev.temperature)
|
||||
gyro.dev.temperature(&telemTemperature1);
|
||||
}
|
||||
|
||||
void mwDisarm(void)
|
||||
|
|
@ -485,7 +485,7 @@ void filterRc(bool isRXDataNew)
|
|||
#ifdef ASYNC_GYRO_PROCESSING
|
||||
biquadFilterInitLPF(&filteredCycleTimeState, 1, getPidUpdateRate());
|
||||
#else
|
||||
biquadFilterInitLPF(&filteredCycleTimeState, 1, gyro.targetLooptime);
|
||||
biquadFilterInitLPF(&filteredCycleTimeState, 1, gyro.dev.targetLooptime);
|
||||
#endif
|
||||
filterInitialised = true;
|
||||
}
|
||||
|
|
@ -523,9 +523,9 @@ void taskGyro(timeUs_t currentTimeUs) {
|
|||
if (masterConfig.gyroConfig.gyroSync) {
|
||||
while (true) {
|
||||
#ifdef ASYNC_GYRO_PROCESSING
|
||||
if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTimeUs)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
|
||||
if (gyroSyncCheckUpdate(&gyro.dev) || ((currentDeltaTime + (micros() - currentTimeUs)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
|
||||
#else
|
||||
if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTimeUs)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) {
|
||||
if (gyroSyncCheckUpdate(&gyro.dev) || ((currentDeltaTime + (micros() - currentTimeUs)) >= (gyro.dev.targetLooptime + GYRO_WATCHDOG_DELAY))) {
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,7 +30,6 @@
|
|||
|
||||
#include "build/build_config.h"
|
||||
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
|
|
@ -155,7 +154,7 @@ void imuInit(void)
|
|||
int axis;
|
||||
|
||||
smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
|
||||
gyroScale = gyro.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second
|
||||
gyroScale = gyro.dev.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second
|
||||
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
imuAccelInBodyFrame.A[axis] = 0;
|
||||
|
|
@ -417,7 +416,7 @@ static int imuCalculateAccelerometerConfidence(void)
|
|||
}
|
||||
|
||||
// Magnitude^2 in percent of G^2
|
||||
accMagnitude = accMagnitude * 100 / ((int32_t)acc.acc_1G * acc.acc_1G);
|
||||
accMagnitude = accMagnitude * 100 / ((int32_t)acc.dev.acc_1G * acc.dev.acc_1G);
|
||||
|
||||
int32_t nearness = ABS(100 - accMagnitude);
|
||||
|
||||
|
|
@ -521,7 +520,7 @@ static void imuUpdateMeasuredAcceleration(void)
|
|||
#else
|
||||
/* Convert acceleration to cm/s/s */
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
imuAccelInBodyFrame.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc.acc_1G);
|
||||
imuAccelInBodyFrame.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc.dev.acc_1G);
|
||||
imuMeasuredGravityBF.A[axis] = imuAccelInBodyFrame.A[axis];
|
||||
}
|
||||
#endif
|
||||
|
|
@ -573,7 +572,7 @@ void imuUpdateAccelerometer(void)
|
|||
|
||||
#ifdef ASYNC_GYRO_PROCESSING
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
imuAccumulatedAcc[axis] += accADC[axis] * (GRAVITY_CMSS / acc.acc_1G);
|
||||
imuAccumulatedAcc[axis] += accADC[axis] * (GRAVITY_CMSS / acc.dev.acc_1G);
|
||||
}
|
||||
imuAccumulatedAccCount++;
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -519,7 +519,7 @@ void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *co
|
|||
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
// Step 1: Calculate gyro rates
|
||||
pidState[axis].gyroRate = gyroADC[axis] * gyro.scale;
|
||||
pidState[axis].gyroRate = gyroADC[axis] * gyro.dev.scale;
|
||||
|
||||
// Step 2: Read target
|
||||
float rateTarget;
|
||||
|
|
|
|||
|
|
@ -446,7 +446,7 @@ void filterServos(void)
|
|||
// Initialize servo lowpass filter (servos are calculated at looptime rate)
|
||||
if (!servoFilterIsSet) {
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
biquadFilterInitLPF(&servoFitlerState[servoIdx], servoMixerConfig->servo_lowpass_freq, gyro.targetLooptime);
|
||||
biquadFilterInitLPF(&servoFitlerState[servoIdx], servoMixerConfig->servo_lowpass_freq, gyro.dev.targetLooptime);
|
||||
}
|
||||
|
||||
servoFilterIsSet = true;
|
||||
|
|
|
|||
|
|
@ -2900,8 +2900,8 @@ static void cliStatus(char *cmdline)
|
|||
|
||||
cliPrintf(", %s=%s", sensorTypeNames[i], sensorHardware);
|
||||
|
||||
if (mask == SENSOR_ACC && acc.revisionCode) {
|
||||
cliPrintf(".%c", acc.revisionCode);
|
||||
if (mask == SENSOR_ACC && acc.dev.revisionCode) {
|
||||
cliPrintf(".%c", acc.dev.revisionCode);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -53,6 +53,8 @@ static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
|
|||
|
||||
void accInit(uint32_t targetLooptime)
|
||||
{
|
||||
acc.dev.acc_1G = 256; // set default
|
||||
acc.dev.init(&acc.dev);
|
||||
accTargetLooptime = targetLooptime;
|
||||
if (accLpfCutHz) {
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
|
|
@ -159,7 +161,7 @@ void performAcclerationCalibration(void)
|
|||
accSample[Y] = accSamples[axis][Y] / CALIBRATING_ACC_CYCLES - accZero->raw[Y];
|
||||
accSample[Z] = accSamples[axis][Z] / CALIBRATING_ACC_CYCLES - accZero->raw[Z];
|
||||
|
||||
sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc.acc_1G);
|
||||
sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc.dev.acc_1G);
|
||||
}
|
||||
|
||||
sensorCalibrationSolveForScale(&calState, accTmp);
|
||||
|
|
@ -183,7 +185,7 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f
|
|||
|
||||
void updateAccelerationReadings(void)
|
||||
{
|
||||
if (!acc.read(accADCRaw)) {
|
||||
if (!acc.dev.read(accADCRaw)) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -37,6 +37,10 @@ typedef enum {
|
|||
ACC_MAX = ACC_FAKE
|
||||
} accelerationSensor_e;
|
||||
|
||||
typedef struct acc_s {
|
||||
accDev_t dev;
|
||||
} acc_t;
|
||||
|
||||
extern sensor_align_e accAlign;
|
||||
extern acc_t acc;
|
||||
|
||||
|
|
|
|||
|
|
@ -25,6 +25,8 @@
|
|||
#include "common/maths.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#include "io/beeper.h"
|
||||
#include "io/statusindicator.h"
|
||||
|
||||
|
|
@ -48,17 +50,20 @@ static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
|
|||
|
||||
void gyroInit(const gyroConfig_t *gyroConfigToUse)
|
||||
{
|
||||
/*
|
||||
* After refactoring this function is always called after gyro sampling rate is known, so
|
||||
* no additional condition is required
|
||||
*/
|
||||
gyroConfig = gyroConfigToUse;
|
||||
// After refactoring this function is always called after gyro sampling rate is known, so
|
||||
// no additional condition is required
|
||||
// Set gyro sample rate before driver initialisation
|
||||
gyro.dev.lpf = gyroConfig->gyro_lpf;
|
||||
gyro.dev.targetLooptime = gyroSetSampleRate(gyroConfig->looptime, gyroConfig->gyro_lpf, gyroConfig->gyroSync, gyroConfig->gyroSyncDenominator);
|
||||
// driver initialisation
|
||||
gyro.dev.init(&gyro.dev);
|
||||
if (gyroConfig->gyro_soft_lpf_hz) {
|
||||
for (int axis = 0; axis < 3; axis++) {
|
||||
#ifdef ASYNC_GYRO_PROCESSING
|
||||
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, getGyroUpdateRate());
|
||||
#else
|
||||
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
|
||||
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, gyro.dev.targetLooptime);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
|
@ -125,17 +130,15 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
|
|||
|
||||
void gyroUpdate(void)
|
||||
{
|
||||
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
||||
|
||||
// range: +/- 8192; +/- 2000 deg/sec
|
||||
if (!gyro.read(gyroADCRaw)) {
|
||||
if (!gyro.dev.read(&gyro.dev)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Prepare a copy of int32_t gyroADC for mangling to prevent overflow
|
||||
gyroADC[X] = gyroADCRaw[X];
|
||||
gyroADC[Y] = gyroADCRaw[Y];
|
||||
gyroADC[Z] = gyroADCRaw[Z];
|
||||
gyroADC[X] = gyro.dev.gyroADCRaw[X];
|
||||
gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
|
||||
gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
|
||||
|
||||
if (gyroConfig->gyro_soft_lpf_hz) {
|
||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
|
|
|
|||
|
|
@ -29,10 +29,13 @@ typedef enum {
|
|||
GYRO_MPU6000,
|
||||
GYRO_MPU6500,
|
||||
GYRO_MPU9250,
|
||||
GYRO_FAKE,
|
||||
GYRO_MAX = GYRO_FAKE
|
||||
GYRO_FAKE
|
||||
} gyroSensor_e;
|
||||
|
||||
typedef struct gyro_s {
|
||||
gyroDev_t dev;
|
||||
} gyro_t;
|
||||
|
||||
extern gyro_t gyro;
|
||||
|
||||
extern int32_t gyroADC[XYZ_AXIS_COUNT];
|
||||
|
|
|
|||
|
|
@ -49,7 +49,6 @@
|
|||
#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/barometer.h"
|
||||
#include "drivers/barometer_bmp085.h"
|
||||
|
|
@ -111,7 +110,7 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
|
|||
#endif
|
||||
}
|
||||
|
||||
bool detectGyro(void)
|
||||
static bool detectGyro(gyroDev_t *dev)
|
||||
{
|
||||
gyroSensor_e gyroHardware = GYRO_DEFAULT;
|
||||
|
||||
|
|
@ -122,7 +121,7 @@ bool detectGyro(void)
|
|||
; // fallthrough
|
||||
case GYRO_MPU6050:
|
||||
#ifdef USE_GYRO_MPU6050
|
||||
if (mpu6050GyroDetect(&gyro)) {
|
||||
if (mpu6050GyroDetect(dev)) {
|
||||
gyroHardware = GYRO_MPU6050;
|
||||
#ifdef GYRO_MPU6050_ALIGN
|
||||
gyroAlign = GYRO_MPU6050_ALIGN;
|
||||
|
|
@ -133,7 +132,7 @@ bool detectGyro(void)
|
|||
; // fallthrough
|
||||
case GYRO_L3G4200D:
|
||||
#ifdef USE_GYRO_L3G4200D
|
||||
if (l3g4200dDetect(&gyro)) {
|
||||
if (l3g4200dDetect(dev)) {
|
||||
gyroHardware = GYRO_L3G4200D;
|
||||
#ifdef GYRO_L3G4200D_ALIGN
|
||||
gyroAlign = GYRO_L3G4200D_ALIGN;
|
||||
|
|
@ -145,7 +144,7 @@ bool detectGyro(void)
|
|||
|
||||
case GYRO_MPU3050:
|
||||
#ifdef USE_GYRO_MPU3050
|
||||
if (mpu3050Detect(&gyro)) {
|
||||
if (mpu3050Detect(dev)) {
|
||||
gyroHardware = GYRO_MPU3050;
|
||||
#ifdef GYRO_MPU3050_ALIGN
|
||||
gyroAlign = GYRO_MPU3050_ALIGN;
|
||||
|
|
@ -157,7 +156,7 @@ bool detectGyro(void)
|
|||
|
||||
case GYRO_L3GD20:
|
||||
#ifdef USE_GYRO_L3GD20
|
||||
if (l3gd20Detect(&gyro)) {
|
||||
if (l3gd20Detect(dev)) {
|
||||
gyroHardware = GYRO_L3GD20;
|
||||
#ifdef GYRO_L3GD20_ALIGN
|
||||
gyroAlign = GYRO_L3GD20_ALIGN;
|
||||
|
|
@ -169,7 +168,7 @@ bool detectGyro(void)
|
|||
|
||||
case GYRO_MPU6000:
|
||||
#ifdef USE_GYRO_SPI_MPU6000
|
||||
if (mpu6000SpiGyroDetect(&gyro)) {
|
||||
if (mpu6000SpiGyroDetect(dev)) {
|
||||
gyroHardware = GYRO_MPU6000;
|
||||
#ifdef GYRO_MPU6000_ALIGN
|
||||
gyroAlign = GYRO_MPU6000_ALIGN;
|
||||
|
|
@ -182,9 +181,9 @@ bool detectGyro(void)
|
|||
case GYRO_MPU6500:
|
||||
#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
|
||||
#ifdef USE_GYRO_SPI_MPU6500
|
||||
if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro)) {
|
||||
if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
|
||||
#else
|
||||
if (mpu6500GyroDetect(&gyro)) {
|
||||
if (mpu6500GyroDetect(dev)) {
|
||||
#endif
|
||||
gyroHardware = GYRO_MPU6500;
|
||||
#ifdef GYRO_MPU6500_ALIGN
|
||||
|
|
@ -198,7 +197,7 @@ bool detectGyro(void)
|
|||
|
||||
case GYRO_MPU9250:
|
||||
#ifdef USE_GYRO_SPI_MPU9250
|
||||
if (mpu9250SpiGyroDetect(&gyro))
|
||||
if (mpu9250SpiGyroDetect(dev))
|
||||
{
|
||||
gyroHardware = GYRO_MPU9250;
|
||||
#ifdef GYRO_MPU9250_ALIGN
|
||||
|
|
@ -211,7 +210,7 @@ bool detectGyro(void)
|
|||
; // fallthrough
|
||||
case GYRO_FAKE:
|
||||
#ifdef USE_FAKE_GYRO
|
||||
if (fakeGyroDetect(&gyro)) {
|
||||
if (fakeGyroDetect(dev)) {
|
||||
gyroHardware = GYRO_FAKE;
|
||||
break;
|
||||
}
|
||||
|
|
@ -233,7 +232,7 @@ bool detectGyro(void)
|
|||
return true;
|
||||
}
|
||||
|
||||
static bool detectAcc(accelerationSensor_e accHardwareToUse)
|
||||
static bool detectAcc(accDev_t *dev, accelerationSensor_e accHardwareToUse)
|
||||
{
|
||||
accelerationSensor_e accHardware;
|
||||
|
||||
|
|
@ -252,9 +251,9 @@ retry:
|
|||
acc_params.useFifo = false;
|
||||
acc_params.dataRate = 800; // unused currently
|
||||
#ifdef NAZE
|
||||
if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) {
|
||||
if (hardwareRevision < NAZE32_REV5 && adxl345Detect(dev, &acc_params)) {
|
||||
#else
|
||||
if (adxl345Detect(&acc_params, &acc)) {
|
||||
if (adxl345Detect(dev, &acc_params)) {
|
||||
#endif
|
||||
#ifdef ACC_ADXL345_ALIGN
|
||||
accAlign = ACC_ADXL345_ALIGN;
|
||||
|
|
@ -266,7 +265,7 @@ retry:
|
|||
; // fallthrough
|
||||
case ACC_LSM303DLHC:
|
||||
#ifdef USE_ACC_LSM303DLHC
|
||||
if (lsm303dlhcAccDetect(&acc)) {
|
||||
if (lsm303dlhcAccDetect(dev)) {
|
||||
#ifdef ACC_LSM303DLHC_ALIGN
|
||||
accAlign = ACC_LSM303DLHC_ALIGN;
|
||||
#endif
|
||||
|
|
@ -277,7 +276,7 @@ retry:
|
|||
; // fallthrough
|
||||
case ACC_MPU6050: // MPU6050
|
||||
#ifdef USE_ACC_MPU6050
|
||||
if (mpu6050AccDetect(&acc)) {
|
||||
if (mpu6050AccDetect(dev)) {
|
||||
#ifdef ACC_MPU6050_ALIGN
|
||||
accAlign = ACC_MPU6050_ALIGN;
|
||||
#endif
|
||||
|
|
@ -290,9 +289,9 @@ retry:
|
|||
#ifdef USE_ACC_MMA8452
|
||||
#ifdef NAZE
|
||||
// Not supported with this frequency
|
||||
if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
|
||||
if (hardwareRevision < NAZE32_REV5 && mma8452Detect(dev)) {
|
||||
#else
|
||||
if (mma8452Detect(&acc)) {
|
||||
if (mma8452Detect(dev)) {
|
||||
#endif
|
||||
#ifdef ACC_MMA8452_ALIGN
|
||||
accAlign = ACC_MMA8452_ALIGN;
|
||||
|
|
@ -304,7 +303,7 @@ retry:
|
|||
; // fallthrough
|
||||
case ACC_BMA280: // BMA280
|
||||
#ifdef USE_ACC_BMA280
|
||||
if (bma280Detect(&acc)) {
|
||||
if (bma280Detect(dev)) {
|
||||
#ifdef ACC_BMA280_ALIGN
|
||||
accAlign = ACC_BMA280_ALIGN;
|
||||
#endif
|
||||
|
|
@ -315,7 +314,7 @@ retry:
|
|||
; // fallthrough
|
||||
case ACC_MPU6000:
|
||||
#ifdef USE_ACC_SPI_MPU6000
|
||||
if (mpu6000SpiAccDetect(&acc)) {
|
||||
if (mpu6000SpiAccDetect(dev)) {
|
||||
#ifdef ACC_MPU6000_ALIGN
|
||||
accAlign = ACC_MPU6000_ALIGN;
|
||||
#endif
|
||||
|
|
@ -327,9 +326,9 @@ retry:
|
|||
case ACC_MPU6500:
|
||||
#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
|
||||
#ifdef USE_ACC_SPI_MPU6500
|
||||
if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc)) {
|
||||
if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) {
|
||||
#else
|
||||
if (mpu6500AccDetect(&acc)) {
|
||||
if (mpu6500AccDetect(dev)) {
|
||||
#endif
|
||||
#ifdef ACC_MPU6500_ALIGN
|
||||
accAlign = ACC_MPU6500_ALIGN;
|
||||
|
|
@ -340,7 +339,7 @@ retry:
|
|||
#endif
|
||||
case ACC_MPU9250:
|
||||
#ifdef USE_ACC_SPI_MPU9250
|
||||
if (mpu9250SpiAccDetect(&acc)) {
|
||||
if (mpu9250SpiAccDetect(dev)) {
|
||||
#ifdef ACC_MPU9250_ALIGN
|
||||
accAlign = ACC_MPU9250_ALIGN;
|
||||
#endif
|
||||
|
|
@ -351,7 +350,7 @@ retry:
|
|||
; // fallthrough
|
||||
case ACC_FAKE:
|
||||
#ifdef USE_FAKE_ACC
|
||||
if (fakeAccDetect(&acc)) {
|
||||
if (fakeAccDetect(dev)) {
|
||||
accHardware = ACC_FAKE;
|
||||
break;
|
||||
}
|
||||
|
|
@ -680,38 +679,26 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
|
|||
int16_t magDeclinationFromConfig,
|
||||
const gyroConfig_t *gyroConfig)
|
||||
{
|
||||
memset(&acc, 0, sizeof(acc));
|
||||
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) || defined(USE_GYRO_SPI_MPU9250)
|
||||
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
|
||||
detectMpu(extiConfig);
|
||||
#endif
|
||||
|
||||
if (!detectGyro()) {
|
||||
memset(&gyro, 0, sizeof(gyro));
|
||||
if (!detectGyro(&gyro.dev)) {
|
||||
return false;
|
||||
}
|
||||
gyroInit(gyroConfig);
|
||||
|
||||
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
|
||||
// Set gyro sample rate before initialisation
|
||||
gyro.targetLooptime = gyroSetSampleRate(gyroConfig->looptime, gyroConfig->gyro_lpf, gyroConfig->gyroSync, gyroConfig->gyroSyncDenominator);
|
||||
gyro.init(gyroConfig->gyro_lpf); // driver initialisation
|
||||
gyroInit(gyroConfig); // sensor initialisation
|
||||
|
||||
if (detectAcc(sensorSelectionConfig->acc_hardware)) {
|
||||
acc.acc_1G = 256; // set default
|
||||
acc.init(&acc);
|
||||
#ifdef ASYNC_GYRO_PROCESSING
|
||||
/*
|
||||
* ACC will be updated at its own rate
|
||||
*/
|
||||
memset(&acc, 0, sizeof(acc));
|
||||
if (detectAcc(&acc.dev, sensorSelectionConfig->acc_hardware)) {
|
||||
#ifdef ASYNC_GYRO_PROCESSING
|
||||
// ACC will be updated at its own rate
|
||||
accInit(getAccUpdateRate());
|
||||
#else
|
||||
/*
|
||||
* acc updated at same frequency in taskMainPidLoop in mw.c
|
||||
*/
|
||||
accInit(gyro.targetLooptime);
|
||||
#endif
|
||||
#else
|
||||
// acc updated at same frequency in taskMainPidLoop in mw.c
|
||||
accInit(gyro.dev.targetLooptime);
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef BARO
|
||||
|
|
|
|||
|
|
@ -164,7 +164,7 @@ static void sendAccel(void)
|
|||
|
||||
for (i = 0; i < 3; i++) {
|
||||
sendDataHead(ID_ACC_X + i);
|
||||
serialize16(accADC[i] * 1000 / acc.acc_1G);
|
||||
serialize16(accADC[i] * 1000 / acc.dev.acc_1G);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue