Merge pull request #2586 from martinbudden/bf_gyro_spi_selection
Added runtime setting of gyro SPI pin
This commit is contained in:
commit
3545775fdf
|
@ -49,6 +49,7 @@ typedef struct gyroDev_s {
|
||||||
sensorGyroInterruptStatusFuncPtr intStatus;
|
sensorGyroInterruptStatusFuncPtr intStatus;
|
||||||
sensorGyroUpdateFuncPtr update;
|
sensorGyroUpdateFuncPtr update;
|
||||||
extiCallbackRec_t exti;
|
extiCallbackRec_t exti;
|
||||||
|
busDevice_t bus;
|
||||||
float scale; // scalefactor
|
float scale; // scalefactor
|
||||||
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
||||||
int32_t gyroZero[XYZ_AXIS_COUNT];
|
int32_t gyroZero[XYZ_AXIS_COUNT];
|
||||||
|
@ -66,6 +67,7 @@ typedef struct gyroDev_s {
|
||||||
typedef struct accDev_s {
|
typedef struct accDev_s {
|
||||||
sensorAccInitFuncPtr init; // initialize function
|
sensorAccInitFuncPtr init; // initialize function
|
||||||
sensorAccReadFuncPtr read; // read 3 axis data function
|
sensorAccReadFuncPtr read; // read 3 axis data function
|
||||||
|
busDevice_t bus;
|
||||||
uint16_t acc_1G;
|
uint16_t acc_1G;
|
||||||
int16_t ADCRaw[XYZ_AXIS_COUNT];
|
int16_t ADCRaw[XYZ_AXIS_COUNT];
|
||||||
char revisionCode; // a revision code for the sensor, if known
|
char revisionCode; // a revision code for the sensor, if known
|
||||||
|
|
|
@ -41,9 +41,10 @@
|
||||||
#include "accgyro_mpu3050.h"
|
#include "accgyro_mpu3050.h"
|
||||||
#include "accgyro_mpu6050.h"
|
#include "accgyro_mpu6050.h"
|
||||||
#include "accgyro_mpu6500.h"
|
#include "accgyro_mpu6500.h"
|
||||||
|
#include "accgyro_spi_bmi160.h"
|
||||||
|
#include "accgyro_spi_icm20689.h"
|
||||||
#include "accgyro_spi_mpu6000.h"
|
#include "accgyro_spi_mpu6000.h"
|
||||||
#include "accgyro_spi_mpu6500.h"
|
#include "accgyro_spi_mpu6500.h"
|
||||||
#include "accgyro_spi_icm20689.h"
|
|
||||||
#include "accgyro_spi_mpu9250.h"
|
#include "accgyro_spi_mpu9250.h"
|
||||||
#include "accgyro_mpu.h"
|
#include "accgyro_mpu.h"
|
||||||
|
|
||||||
|
@ -75,7 +76,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
|
||||||
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
|
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
|
||||||
|
|
||||||
// determine product ID and accel revision
|
// determine product ID and accel revision
|
||||||
ack = gyro->mpuConfiguration.readFn(MPU_RA_XA_OFFS_H, 6, readBuffer);
|
ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_XA_OFFS_H, 6, readBuffer);
|
||||||
revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
|
revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
|
||||||
if (revision) {
|
if (revision) {
|
||||||
/* Congrats, these parts are better. */
|
/* Congrats, these parts are better. */
|
||||||
|
@ -89,7 +90,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
|
||||||
failureMode(FAILURE_ACC_INCOMPATIBLE);
|
failureMode(FAILURE_ACC_INCOMPATIBLE);
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
ack = gyro->mpuConfiguration.readFn(MPU_RA_PRODUCT_ID, 1, &productId);
|
ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_PRODUCT_ID, 1, &productId);
|
||||||
revision = productId & 0x0F;
|
revision = productId & 0x0F;
|
||||||
if (!revision) {
|
if (!revision) {
|
||||||
failureMode(FAILURE_ACC_INCOMPATIBLE);
|
failureMode(FAILURE_ACC_INCOMPATIBLE);
|
||||||
|
@ -160,14 +161,16 @@ static void mpuIntExtiInit(gyroDev_t *gyro)
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
|
bool mpuReadRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data)
|
||||||
{
|
{
|
||||||
|
UNUSED(bus);
|
||||||
bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data);
|
bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data);
|
||||||
return ack;
|
return ack;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data)
|
bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data)
|
||||||
{
|
{
|
||||||
|
UNUSED(bus);
|
||||||
bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data);
|
bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data);
|
||||||
return ack;
|
return ack;
|
||||||
}
|
}
|
||||||
|
@ -176,7 +179,7 @@ bool mpuAccRead(accDev_t *acc)
|
||||||
{
|
{
|
||||||
uint8_t data[6];
|
uint8_t data[6];
|
||||||
|
|
||||||
bool ack = acc->mpuConfiguration.readFn(MPU_RA_ACCEL_XOUT_H, 6, data);
|
bool ack = acc->mpuConfiguration.readFn(&acc->bus, MPU_RA_ACCEL_XOUT_H, 6, data);
|
||||||
if (!ack) {
|
if (!ack) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -199,7 +202,7 @@ bool mpuGyroRead(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
uint8_t data[6];
|
uint8_t data[6];
|
||||||
|
|
||||||
const bool ack = gyro->mpuConfiguration.readFn(gyro->mpuConfiguration.gyroReadXRegister, 6, data);
|
const bool ack = gyro->mpuConfiguration.readFn(&gyro->bus, gyro->mpuConfiguration.gyroReadXRegister, 6, data);
|
||||||
if (!ack) {
|
if (!ack) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -226,104 +229,112 @@ bool mpuCheckDataReady(gyroDev_t* gyro)
|
||||||
#ifdef USE_SPI
|
#ifdef USE_SPI
|
||||||
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
|
UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI
|
||||||
#ifdef USE_GYRO_SPI_MPU6000
|
#ifdef USE_GYRO_SPI_MPU6000
|
||||||
if (mpu6000SpiDetect()) {
|
#ifdef MPU6000_CS_PIN
|
||||||
|
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.spi.csnPin;
|
||||||
|
#endif
|
||||||
|
if (mpu6000SpiDetect(&gyro->bus)) {
|
||||||
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
|
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
|
||||||
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||||
gyro->mpuConfiguration.readFn = mpu6000ReadRegister;
|
gyro->mpuConfiguration.readFn = mpu6000SpiReadRegister;
|
||||||
gyro->mpuConfiguration.writeFn = mpu6000WriteRegister;
|
gyro->mpuConfiguration.writeFn = mpu6000SpiWriteRegister;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_GYRO_SPI_MPU6500
|
#ifdef USE_GYRO_SPI_MPU6500
|
||||||
uint8_t mpu6500Sensor = mpu6500SpiDetect();
|
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.spi.csnPin;
|
||||||
|
const uint8_t mpu6500Sensor = mpu6500SpiDetect(&gyro->bus);
|
||||||
|
// some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
|
||||||
if (mpu6500Sensor != MPU_NONE) {
|
if (mpu6500Sensor != MPU_NONE) {
|
||||||
gyro->mpuDetectionResult.sensor = mpu6500Sensor;
|
gyro->mpuDetectionResult.sensor = mpu6500Sensor;
|
||||||
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||||
gyro->mpuConfiguration.readFn = mpu6500ReadRegister;
|
gyro->mpuConfiguration.readFn = mpu6500SpiReadRegister;
|
||||||
gyro->mpuConfiguration.writeFn = mpu6500WriteRegister;
|
gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_GYRO_SPI_MPU9250
|
#ifdef USE_GYRO_SPI_MPU9250
|
||||||
if (mpu9250SpiDetect()) {
|
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.spi.csnPin;
|
||||||
|
if (mpu9250SpiDetect(&gyro->bus)) {
|
||||||
gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
|
gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
|
||||||
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||||
gyro->mpuConfiguration.readFn = mpu9250ReadRegister;
|
gyro->mpuConfiguration.readFn = mpu9250SpiReadRegister;
|
||||||
gyro->mpuConfiguration.slowreadFn = mpu9250SlowReadRegister;
|
gyro->mpuConfiguration.slowreadFn = mpu9250SpiSlowReadRegister;
|
||||||
gyro->mpuConfiguration.verifywriteFn = verifympu9250WriteRegister;
|
gyro->mpuConfiguration.verifywriteFn = verifympu9250SpiWriteRegister;
|
||||||
gyro->mpuConfiguration.writeFn = mpu9250WriteRegister;
|
gyro->mpuConfiguration.writeFn = mpu9250SpiWriteRegister;
|
||||||
gyro->mpuConfiguration.resetFn = mpu9250ResetGyro;
|
gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_GYRO_SPI_ICM20689
|
#ifdef USE_GYRO_SPI_ICM20689
|
||||||
if (icm20689SpiDetect()) {
|
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.spi.csnPin;
|
||||||
|
if (icm20689SpiDetect(&gyro->bus)) {
|
||||||
gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
|
gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
|
||||||
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||||
gyro->mpuConfiguration.readFn = icm20689ReadRegister;
|
gyro->mpuConfiguration.readFn = icm20689SpiReadRegister;
|
||||||
gyro->mpuConfiguration.writeFn = icm20689WriteRegister;
|
gyro->mpuConfiguration.writeFn = icm20689SpiWriteRegister;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef USE_ACCGYRO_BMI160
|
||||||
|
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI160_CS_PIN)) : gyro->bus.spi.csnPin;
|
||||||
|
if (bmi160Detect(&gyro->bus)) {
|
||||||
|
gyro->mpuDetectionResult.sensor = BMI_160_SPI;
|
||||||
|
gyro->mpuConfiguration.readFn = bmi160SpiReadRegister;
|
||||||
|
gyro->mpuConfiguration.writeFn = bmi160SpiWriteRegister;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
UNUSED(gyro);
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
|
void mpuDetect(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
|
|
||||||
// MPU datasheet specifies 30ms.
|
// MPU datasheet specifies 30ms.
|
||||||
delay(35);
|
delay(35);
|
||||||
|
|
||||||
#ifndef USE_I2C
|
|
||||||
uint8_t sig = 0;
|
uint8_t sig = 0;
|
||||||
bool ack = false;
|
#ifdef USE_I2C
|
||||||
|
bool ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I, 1, &sig);
|
||||||
#else
|
#else
|
||||||
uint8_t sig;
|
bool ack = false;
|
||||||
bool ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
|
|
||||||
#endif
|
#endif
|
||||||
if (ack) {
|
if (ack) {
|
||||||
gyro->mpuConfiguration.readFn = mpuReadRegisterI2C;
|
gyro->mpuConfiguration.readFn = mpuReadRegisterI2C;
|
||||||
gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C;
|
gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C;
|
||||||
} else {
|
} else {
|
||||||
#ifdef USE_SPI
|
#ifdef USE_SPI
|
||||||
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro);
|
detectSPISensorsAndUpdateDetectionResult(gyro);
|
||||||
UNUSED(detectedSpiSensor);
|
|
||||||
#endif
|
#endif
|
||||||
|
return;
|
||||||
return &gyro->mpuDetectionResult;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||||
|
|
||||||
// If an MPU3050 is connected sig will contain 0.
|
// If an MPU3050 is connected sig will contain 0.
|
||||||
uint8_t inquiryResult;
|
uint8_t inquiryResult;
|
||||||
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
|
ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
|
||||||
inquiryResult &= MPU_INQUIRY_MASK;
|
inquiryResult &= MPU_INQUIRY_MASK;
|
||||||
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
|
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
|
||||||
gyro->mpuDetectionResult.sensor = MPU_3050;
|
gyro->mpuDetectionResult.sensor = MPU_3050;
|
||||||
gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
|
gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
|
||||||
return &gyro->mpuDetectionResult;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
sig &= MPU_INQUIRY_MASK;
|
sig &= MPU_INQUIRY_MASK;
|
||||||
|
|
||||||
if (sig == MPUx0x0_WHO_AM_I_CONST) {
|
if (sig == MPUx0x0_WHO_AM_I_CONST) {
|
||||||
|
|
||||||
gyro->mpuDetectionResult.sensor = MPU_60x0;
|
gyro->mpuDetectionResult.sensor = MPU_60x0;
|
||||||
|
|
||||||
mpu6050FindRevision(gyro);
|
mpu6050FindRevision(gyro);
|
||||||
} else if (sig == MPU6500_WHO_AM_I_CONST) {
|
} else if (sig == MPU6500_WHO_AM_I_CONST) {
|
||||||
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
|
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
|
||||||
}
|
}
|
||||||
|
|
||||||
return &gyro->mpuDetectionResult;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void mpuGyroInit(gyroDev_t *gyro)
|
void mpuGyroInit(gyroDev_t *gyro)
|
||||||
|
|
|
@ -124,8 +124,8 @@
|
||||||
// RF = Register Flag
|
// RF = Register Flag
|
||||||
#define MPU_RF_DATA_RDY_EN (1 << 0)
|
#define MPU_RF_DATA_RDY_EN (1 << 0)
|
||||||
|
|
||||||
typedef bool (*mpuReadRegisterFnPtr)(uint8_t reg, uint8_t length, uint8_t* data);
|
typedef bool (*mpuReadRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data);
|
||||||
typedef bool (*mpuWriteRegisterFnPtr)(uint8_t reg, uint8_t data);
|
typedef bool (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data);
|
||||||
typedef void(*mpuResetFnPtr)(void);
|
typedef void(*mpuResetFnPtr)(void);
|
||||||
|
|
||||||
extern mpuResetFnPtr mpuResetFn;
|
extern mpuResetFnPtr mpuResetFn;
|
||||||
|
@ -177,7 +177,8 @@ typedef enum {
|
||||||
MPU_9250_SPI,
|
MPU_9250_SPI,
|
||||||
ICM_20689_SPI,
|
ICM_20689_SPI,
|
||||||
ICM_20608_SPI,
|
ICM_20608_SPI,
|
||||||
ICM_20602_SPI
|
ICM_20602_SPI,
|
||||||
|
BMI_160_SPI,
|
||||||
} mpuSensor_e;
|
} mpuSensor_e;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
@ -190,12 +191,15 @@ typedef struct mpuDetectionResult_s {
|
||||||
mpu6050Resolution_e resolution;
|
mpu6050Resolution_e resolution;
|
||||||
} mpuDetectionResult_t;
|
} mpuDetectionResult_t;
|
||||||
|
|
||||||
|
bool mpuReadRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data);
|
||||||
|
bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data);
|
||||||
|
|
||||||
struct gyroDev_s;
|
struct gyroDev_s;
|
||||||
void mpuGyroInit(struct gyroDev_s *gyro);
|
void mpuGyroInit(struct gyroDev_s *gyro);
|
||||||
struct accDev_s;
|
struct accDev_s;
|
||||||
bool mpuAccRead(struct accDev_s *acc);
|
bool mpuAccRead(struct accDev_s *acc);
|
||||||
bool mpuGyroRead(struct gyroDev_s *gyro);
|
bool mpuGyroRead(struct gyroDev_s *gyro);
|
||||||
mpuDetectionResult_t *mpuDetect(struct gyroDev_s *gyro);
|
void mpuDetect(struct gyroDev_s *gyro);
|
||||||
bool mpuCheckDataReady(struct gyroDev_s *gyro);
|
bool mpuCheckDataReady(struct gyroDev_s *gyro);
|
||||||
void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn);
|
void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn);
|
||||||
|
|
||||||
|
|
|
@ -53,21 +53,20 @@ static void mpu3050Init(gyroDev_t *gyro)
|
||||||
|
|
||||||
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
|
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
|
||||||
|
|
||||||
ack = gyro->mpuConfiguration.writeFn(MPU3050_SMPLRT_DIV, 0);
|
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_SMPLRT_DIV, 0);
|
||||||
if (!ack)
|
if (!ack)
|
||||||
failureMode(FAILURE_ACC_INIT);
|
failureMode(FAILURE_ACC_INIT);
|
||||||
|
|
||||||
gyro->mpuConfiguration.writeFn(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
|
||||||
gyro->mpuConfiguration.writeFn(MPU3050_INT_CFG, 0);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_INT_CFG, 0);
|
||||||
gyro->mpuConfiguration.writeFn(MPU3050_USER_CTRL, MPU3050_USER_RESET);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET);
|
||||||
gyro->mpuConfiguration.writeFn(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
|
static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
|
||||||
{
|
{
|
||||||
UNUSED(gyro);
|
|
||||||
uint8_t buf[2];
|
uint8_t buf[2];
|
||||||
if (!gyro->mpuConfiguration.readFn(MPU3050_TEMP_OUT, 2, buf)) {
|
if (!gyro->mpuConfiguration.readFn(&gyro->bus, MPU3050_TEMP_OUT, 2, buf)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -79,23 +79,23 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
mpuGyroInit(gyro);
|
mpuGyroInit(gyro);
|
||||||
|
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
|
||||||
delay(100);
|
delay(100);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //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
|
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
|
||||||
gyro->mpuConfiguration.writeFn(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)
|
gyro->mpuConfiguration.writeFn(&gyro->bus, 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)
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
|
||||||
|
|
||||||
// ACC Init stuff.
|
// ACC Init stuff.
|
||||||
// Accel scale 8g (4096 LSB/g)
|
// Accel scale 8g (4096 LSB/g)
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
||||||
|
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG,
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG,
|
||||||
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
|
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
|
||||||
|
|
||||||
#ifdef USE_MPU_DATA_READY_SIGNAL
|
#ifdef USE_MPU_DATA_READY_SIGNAL
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -55,34 +55,34 @@ void mpu6500GyroInit(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
mpuGyroInit(gyro);
|
mpuGyroInit(gyro);
|
||||||
|
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
|
||||||
delay(100);
|
delay(100);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x07);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07);
|
||||||
delay(100);
|
delay(100);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
|
||||||
delay(100);
|
delay(100);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||||
delay(15);
|
delay(15);
|
||||||
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
|
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
|
||||||
delay(15);
|
delay(15);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
||||||
delay(15);
|
delay(15);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
|
||||||
delay(15);
|
delay(15);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
|
||||||
delay(100);
|
delay(100);
|
||||||
|
|
||||||
// Data ready interrupt configuration
|
// Data ready interrupt configuration
|
||||||
#ifdef USE_MPU9250_MAG
|
#ifdef USE_MPU9250_MAG
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||||
#else
|
#else
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
|
||||||
#endif
|
#endif
|
||||||
delay(15);
|
delay(15);
|
||||||
|
|
||||||
#ifdef USE_MPU_DATA_READY_SIGNAL
|
#ifdef USE_MPU_DATA_READY_SIGNAL
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
|
||||||
#endif
|
#endif
|
||||||
delay(15);
|
delay(15);
|
||||||
}
|
}
|
||||||
|
|
|
@ -37,24 +37,16 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#include "common/axis.h"
|
|
||||||
#include "common/maths.h"
|
|
||||||
|
|
||||||
#include "system.h"
|
#include "system.h"
|
||||||
#include "io.h"
|
#include "io.h"
|
||||||
#include "exti.h"
|
#include "exti.h"
|
||||||
#include "nvic.h"
|
#include "nvic.h"
|
||||||
#include "bus_spi.h"
|
#include "bus_spi.h"
|
||||||
|
|
||||||
#include "gyro_sync.h"
|
|
||||||
|
|
||||||
#include "sensor.h"
|
#include "sensor.h"
|
||||||
#include "accgyro.h"
|
#include "accgyro.h"
|
||||||
#include "accgyro_spi_bmi160.h"
|
#include "accgyro_spi_bmi160.h"
|
||||||
|
|
||||||
#include "config/config_eeprom.h"
|
|
||||||
#include "config/config_profile.h"
|
|
||||||
#include "fc/runtime_config.h"
|
|
||||||
|
|
||||||
#ifdef USE_ACCGYRO_BMI160
|
#ifdef USE_ACCGYRO_BMI160
|
||||||
|
|
||||||
|
@ -90,38 +82,36 @@
|
||||||
#define BMI160_REG_CONF_NVM_PROG_EN 0x02
|
#define BMI160_REG_CONF_NVM_PROG_EN 0x02
|
||||||
|
|
||||||
///* Global Variables */
|
///* Global Variables */
|
||||||
static volatile bool BMI160InitDone = false;
|
static volatile bool BMI160InitDone = false;
|
||||||
static volatile bool BMI160Detected = false;
|
static volatile bool BMI160Detected = false;
|
||||||
static volatile bool bmi160DataReady = false;
|
static volatile bool bmi160DataReady = false;
|
||||||
static volatile bool bmi160ExtiInitDone = false;
|
static volatile bool bmi160ExtiInitDone = false;
|
||||||
|
|
||||||
//! Private functions
|
//! Private functions
|
||||||
static int32_t BMI160_Config();
|
static int32_t BMI160_Config(const busDevice_t *bus);
|
||||||
static int32_t BMI160_do_foc();
|
static int32_t BMI160_do_foc(const busDevice_t *bus);
|
||||||
static uint8_t BMI160_ReadReg(uint8_t reg);
|
static uint8_t BMI160_ReadReg(const busDevice_t *bus, uint8_t reg);
|
||||||
static int32_t BMI160_WriteReg(uint8_t reg, uint8_t data);
|
static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data);
|
||||||
|
|
||||||
static IO_t bmi160CsPin = IO_NONE;
|
#define DISABLE_BMI160(spiCsnPin) IOHi(spiCsnPin)
|
||||||
#define DISABLE_BMI160 IOHi(bmi160CsPin)
|
#define ENABLE_BMI160(spiCsnPin) IOLo(spiCsnPin)
|
||||||
#define ENABLE_BMI160 IOLo(bmi160CsPin)
|
|
||||||
|
|
||||||
|
|
||||||
bool BMI160_Detect()
|
bool bmi160Detect(const busDevice_t *bus)
|
||||||
{
|
{
|
||||||
if (BMI160Detected)
|
if (BMI160Detected)
|
||||||
return true;
|
return true;
|
||||||
bmi160CsPin = IOGetByTag(IO_TAG(BMI160_CS_PIN));
|
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
||||||
IOInit(bmi160CsPin, OWNER_MPU_CS, 0);
|
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
|
||||||
IOConfigGPIO(bmi160CsPin, SPI_IO_CS_CFG);
|
|
||||||
|
|
||||||
spiSetDivisor(BMI160_SPI_INSTANCE, BMI160_SPI_DIVISOR);
|
spiSetDivisor(BMI160_SPI_INSTANCE, BMI160_SPI_DIVISOR);
|
||||||
|
|
||||||
/* Read this address to acticate SPI (see p. 84) */
|
/* Read this address to acticate SPI (see p. 84) */
|
||||||
BMI160_ReadReg(0x7F);
|
BMI160_ReadReg(bus, 0x7F);
|
||||||
delay(10); // Give SPI some time to start up
|
delay(10); // Give SPI some time to start up
|
||||||
|
|
||||||
/* Check the chip ID */
|
/* Check the chip ID */
|
||||||
if (BMI160_ReadReg(BMI160_REG_CHIPID) != 0xd1){
|
if (BMI160_ReadReg(bus, BMI160_REG_CHIPID) != 0xd1){
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -134,13 +124,13 @@ bool BMI160_Detect()
|
||||||
* @brief Initialize the BMI160 6-axis sensor.
|
* @brief Initialize the BMI160 6-axis sensor.
|
||||||
* @return 0 for success, -1 for failure to allocate, -10 for failure to get irq
|
* @return 0 for success, -1 for failure to allocate, -10 for failure to get irq
|
||||||
*/
|
*/
|
||||||
static void BMI160_Init()
|
static void BMI160_Init(const busDevice_t *bus)
|
||||||
{
|
{
|
||||||
if (BMI160InitDone || !BMI160Detected)
|
if (BMI160InitDone || !BMI160Detected)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
/* Configure the BMI160 Sensor */
|
/* Configure the BMI160 Sensor */
|
||||||
if (BMI160_Config() != 0){
|
if (BMI160_Config(bus) != 0){
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -148,7 +138,7 @@ static void BMI160_Init()
|
||||||
|
|
||||||
/* Perform fast offset compensation if requested */
|
/* Perform fast offset compensation if requested */
|
||||||
if (do_foc) {
|
if (do_foc) {
|
||||||
BMI160_do_foc();
|
BMI160_do_foc(bus);
|
||||||
}
|
}
|
||||||
|
|
||||||
BMI160InitDone = true;
|
BMI160InitDone = true;
|
||||||
|
@ -158,69 +148,69 @@ static void BMI160_Init()
|
||||||
/**
|
/**
|
||||||
* @brief Configure the sensor
|
* @brief Configure the sensor
|
||||||
*/
|
*/
|
||||||
static int32_t BMI160_Config()
|
static int32_t BMI160_Config(const busDevice_t *bus)
|
||||||
{
|
{
|
||||||
|
|
||||||
// Set normal power mode for gyro and accelerometer
|
// Set normal power mode for gyro and accelerometer
|
||||||
if (BMI160_WriteReg(BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL) != 0){
|
if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL) != 0){
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
delay(100); // can take up to 80ms
|
delay(100); // can take up to 80ms
|
||||||
|
|
||||||
if (BMI160_WriteReg(BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL) != 0){
|
if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL) != 0){
|
||||||
return -2;
|
return -2;
|
||||||
}
|
}
|
||||||
delay(5); // can take up to 3.8ms
|
delay(5); // can take up to 3.8ms
|
||||||
|
|
||||||
// Verify that normal power mode was entered
|
// Verify that normal power mode was entered
|
||||||
uint8_t pmu_status = BMI160_ReadReg(BMI160_REG_PMU_STAT);
|
uint8_t pmu_status = BMI160_ReadReg(bus, BMI160_REG_PMU_STAT);
|
||||||
if ((pmu_status & 0x3C) != 0x14){
|
if ((pmu_status & 0x3C) != 0x14){
|
||||||
return -3;
|
return -3;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set odr and ranges
|
// Set odr and ranges
|
||||||
// Set acc_us = 0 acc_bwp = 0b010 so only the first filter stage is used
|
// Set acc_us = 0 acc_bwp = 0b010 so only the first filter stage is used
|
||||||
if (BMI160_WriteReg(BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz) != 0){
|
if (BMI160_WriteReg(bus, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz) != 0){
|
||||||
return -3;
|
return -3;
|
||||||
}
|
}
|
||||||
delay(1);
|
delay(1);
|
||||||
|
|
||||||
// Set gyr_bwp = 0b010 so only the first filter stage is used
|
// Set gyr_bwp = 0b010 so only the first filter stage is used
|
||||||
if (BMI160_WriteReg(BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz) != 0){
|
if (BMI160_WriteReg(bus, BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz) != 0){
|
||||||
return -4;
|
return -4;
|
||||||
}
|
}
|
||||||
delay(1);
|
delay(1);
|
||||||
|
|
||||||
if (BMI160_WriteReg(BMI160_REG_ACC_RANGE, BMI160_RANGE_8G) != 0){
|
if (BMI160_WriteReg(bus, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G) != 0){
|
||||||
return -5;
|
return -5;
|
||||||
}
|
}
|
||||||
delay(1);
|
delay(1);
|
||||||
|
|
||||||
if (BMI160_WriteReg(BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS) != 0){
|
if (BMI160_WriteReg(bus, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS) != 0){
|
||||||
return -6;
|
return -6;
|
||||||
}
|
}
|
||||||
delay(1);
|
delay(1);
|
||||||
|
|
||||||
// Enable offset compensation
|
// Enable offset compensation
|
||||||
uint8_t val = BMI160_ReadReg(BMI160_REG_OFFSET_0);
|
uint8_t val = BMI160_ReadReg(bus, BMI160_REG_OFFSET_0);
|
||||||
if (BMI160_WriteReg(BMI160_REG_OFFSET_0, val | 0xC0) != 0){
|
if (BMI160_WriteReg(bus, BMI160_REG_OFFSET_0, val | 0xC0) != 0){
|
||||||
return -7;
|
return -7;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Enable data ready interrupt
|
// Enable data ready interrupt
|
||||||
if (BMI160_WriteReg(BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY) != 0){
|
if (BMI160_WriteReg(bus, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY) != 0){
|
||||||
return -8;
|
return -8;
|
||||||
}
|
}
|
||||||
delay(1);
|
delay(1);
|
||||||
|
|
||||||
// Enable INT1 pin
|
// Enable INT1 pin
|
||||||
if (BMI160_WriteReg(BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG) != 0){
|
if (BMI160_WriteReg(bus, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG) != 0){
|
||||||
return -9;
|
return -9;
|
||||||
}
|
}
|
||||||
delay(1);
|
delay(1);
|
||||||
|
|
||||||
// Map data ready interrupt to INT1 pin
|
// Map data ready interrupt to INT1 pin
|
||||||
if (BMI160_WriteReg(BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY) != 0){
|
if (BMI160_WriteReg(bus, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY) != 0){
|
||||||
return -10;
|
return -10;
|
||||||
}
|
}
|
||||||
delay(1);
|
delay(1);
|
||||||
|
@ -228,22 +218,22 @@ static int32_t BMI160_Config()
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int32_t BMI160_do_foc()
|
static int32_t BMI160_do_foc(const busDevice_t *bus)
|
||||||
{
|
{
|
||||||
// assume sensor is mounted on top
|
// assume sensor is mounted on top
|
||||||
uint8_t val = 0x7D;;
|
uint8_t val = 0x7D;;
|
||||||
if (BMI160_WriteReg(BMI160_REG_FOC_CONF, val) != 0) {
|
if (BMI160_WriteReg(bus, BMI160_REG_FOC_CONF, val) != 0) {
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Start FOC
|
// Start FOC
|
||||||
if (BMI160_WriteReg(BMI160_REG_CMD, BMI160_CMD_START_FOC) != 0) {
|
if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_CMD_START_FOC) != 0) {
|
||||||
return -2;
|
return -2;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Wait for FOC to complete
|
// Wait for FOC to complete
|
||||||
for (int i=0; i<50; i++) {
|
for (int i=0; i<50; i++) {
|
||||||
val = BMI160_ReadReg(BMI160_REG_STATUS);
|
val = BMI160_ReadReg(bus, BMI160_REG_STATUS);
|
||||||
if (val & BMI160_REG_STATUS_FOC_RDY) {
|
if (val & BMI160_REG_STATUS_FOC_RDY) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -254,18 +244,18 @@ static int32_t BMI160_do_foc()
|
||||||
}
|
}
|
||||||
|
|
||||||
// Program NVM
|
// Program NVM
|
||||||
val = BMI160_ReadReg(BMI160_REG_CONF);
|
val = BMI160_ReadReg(bus, BMI160_REG_CONF);
|
||||||
if (BMI160_WriteReg(BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN) != 0) {
|
if (BMI160_WriteReg(bus, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN) != 0) {
|
||||||
return -4;
|
return -4;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (BMI160_WriteReg(BMI160_REG_CMD, BMI160_CMD_PROG_NVM) != 0) {
|
if (BMI160_WriteReg(bus, BMI160_REG_CMD, BMI160_CMD_PROG_NVM) != 0) {
|
||||||
return -5;
|
return -5;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Wait for NVM programming to complete
|
// Wait for NVM programming to complete
|
||||||
for (int i=0; i<50; i++) {
|
for (int i=0; i<50; i++) {
|
||||||
val = BMI160_ReadReg(BMI160_REG_STATUS);
|
val = BMI160_ReadReg(bus, BMI160_REG_STATUS);
|
||||||
if (val & BMI160_REG_STATUS_NVM_RDY) {
|
if (val & BMI160_REG_STATUS_NVM_RDY) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -283,20 +273,29 @@ static int32_t BMI160_do_foc()
|
||||||
* @returns The register value
|
* @returns The register value
|
||||||
* @param reg[in] Register address to be read
|
* @param reg[in] Register address to be read
|
||||||
*/
|
*/
|
||||||
static uint8_t BMI160_ReadReg(uint8_t reg)
|
uint8_t BMI160_ReadReg(const busDevice_t *bus, uint8_t reg)
|
||||||
{
|
{
|
||||||
uint8_t data;
|
uint8_t data;
|
||||||
|
|
||||||
ENABLE_BMI160;
|
ENABLE_BMI160(bus->spi.csnPin);
|
||||||
|
|
||||||
spiTransferByte(BMI160_SPI_INSTANCE, 0x80 | reg); // request byte
|
spiTransferByte(BMI160_SPI_INSTANCE, 0x80 | reg); // request byte
|
||||||
spiTransfer(BMI160_SPI_INSTANCE, &data, NULL, 1); // receive response
|
spiTransfer(BMI160_SPI_INSTANCE, &data, NULL, 1); // receive response
|
||||||
|
|
||||||
DISABLE_BMI160;
|
DISABLE_BMI160(bus->spi.csnPin);
|
||||||
|
|
||||||
return data;
|
return data;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool bmi160SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
|
||||||
|
{
|
||||||
|
ENABLE_BMI160(bus->spi.csnPin);
|
||||||
|
spiTransferByte(BMI160_SPI_INSTANCE, reg | 0x80); // read transaction
|
||||||
|
spiTransfer(BMI160_SPI_INSTANCE, data, NULL, length);
|
||||||
|
ENABLE_BMI160(bus->spi.csnPin);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Writes one byte to the BMI160 register
|
* @brief Writes one byte to the BMI160 register
|
||||||
|
@ -304,18 +303,22 @@ static uint8_t BMI160_ReadReg(uint8_t reg)
|
||||||
* \param[in] data Byte to write
|
* \param[in] data Byte to write
|
||||||
* @returns 0 when success
|
* @returns 0 when success
|
||||||
*/
|
*/
|
||||||
static int32_t BMI160_WriteReg(uint8_t reg, uint8_t data)
|
static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data)
|
||||||
{
|
{
|
||||||
ENABLE_BMI160;
|
ENABLE_BMI160(bus->spi.csnPin);
|
||||||
|
|
||||||
spiTransferByte(BMI160_SPI_INSTANCE, 0x7f & reg);
|
spiTransferByte(BMI160_SPI_INSTANCE, 0x7f & reg);
|
||||||
spiTransferByte(BMI160_SPI_INSTANCE, data);
|
spiTransferByte(BMI160_SPI_INSTANCE, data);
|
||||||
|
|
||||||
DISABLE_BMI160;
|
DISABLE_BMI160(bus->spi.csnPin);
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool bmi160SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
|
||||||
|
{
|
||||||
|
return BMI160_WriteReg(bus, reg, data);
|
||||||
|
}
|
||||||
|
|
||||||
extiCallbackRec_t bmi160IntCallbackRec;
|
extiCallbackRec_t bmi160IntCallbackRec;
|
||||||
|
|
||||||
|
@ -362,9 +365,9 @@ bool bmi160AccRead(accDev_t *acc)
|
||||||
uint8_t bmi160_rec_buf[BUFFER_SIZE];
|
uint8_t bmi160_rec_buf[BUFFER_SIZE];
|
||||||
uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0};
|
uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0};
|
||||||
|
|
||||||
ENABLE_BMI160;
|
ENABLE_BMI160(acc->bus.spi.csnPin);
|
||||||
spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response
|
spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response
|
||||||
DISABLE_BMI160;
|
DISABLE_BMI160(acc->bus.spi.csnPin);
|
||||||
|
|
||||||
acc->ADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_XOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_XOUT_L]);
|
acc->ADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_XOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_XOUT_L]);
|
||||||
acc->ADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_YOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_YOUT_L]);
|
acc->ADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_ACCEL_YOUT_H] << 8) | bmi160_rec_buf[IDX_ACCEL_YOUT_L]);
|
||||||
|
@ -390,9 +393,9 @@ bool bmi160GyroRead(gyroDev_t *gyro)
|
||||||
uint8_t bmi160_rec_buf[BUFFER_SIZE];
|
uint8_t bmi160_rec_buf[BUFFER_SIZE];
|
||||||
uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0};
|
uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0};
|
||||||
|
|
||||||
ENABLE_BMI160;
|
ENABLE_BMI160(gyro->bus.spi.csnPin);
|
||||||
spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response
|
spiTransfer(BMI160_SPI_INSTANCE, bmi160_rec_buf, bmi160_tx_buf, BUFFER_SIZE); // receive response
|
||||||
DISABLE_BMI160;
|
DISABLE_BMI160(gyro->bus.spi.csnPin);
|
||||||
|
|
||||||
gyro->gyroADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_GYRO_XOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_XOUT_L]);
|
gyro->gyroADCRaw[X] = (int16_t)((bmi160_rec_buf[IDX_GYRO_XOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_XOUT_L]);
|
||||||
gyro->gyroADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_GYRO_YOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_YOUT_L]);
|
gyro->gyroADCRaw[Y] = (int16_t)((bmi160_rec_buf[IDX_GYRO_YOUT_H] << 8) | bmi160_rec_buf[IDX_GYRO_YOUT_L]);
|
||||||
|
@ -416,13 +419,13 @@ bool checkBMI160DataReady(gyroDev_t* gyro)
|
||||||
|
|
||||||
void bmi160SpiGyroInit(gyroDev_t *gyro)
|
void bmi160SpiGyroInit(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
BMI160_Init();
|
BMI160_Init(gyro->bus.spi.csnPin);
|
||||||
bmi160IntExtiInit(gyro);
|
bmi160IntExtiInit(gyro);
|
||||||
}
|
}
|
||||||
|
|
||||||
void bmi160SpiAccInit(accDev_t *acc)
|
void bmi160SpiAccInit(accDev_t *acc)
|
||||||
{
|
{
|
||||||
BMI160_Init();
|
BMI160_Init(acc->bus.spi.csnPin);
|
||||||
|
|
||||||
acc->acc_1G = 512 * 8;
|
acc->acc_1G = 512 * 8;
|
||||||
}
|
}
|
||||||
|
@ -430,7 +433,7 @@ void bmi160SpiAccInit(accDev_t *acc)
|
||||||
|
|
||||||
bool bmi160SpiAccDetect(accDev_t *acc)
|
bool bmi160SpiAccDetect(accDev_t *acc)
|
||||||
{
|
{
|
||||||
if (!BMI160_Detect()) {
|
if (!bmi160Detect(acc->bus.spi.csnPin)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -443,7 +446,7 @@ bool bmi160SpiAccDetect(accDev_t *acc)
|
||||||
|
|
||||||
bool bmi160SpiGyroDetect(gyroDev_t *gyro)
|
bool bmi160SpiGyroDetect(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
if (!BMI160_Detect()) {
|
if (!bmi160Detect(gyro->bus.spi.csnPin)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -34,6 +34,8 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "sensor.h"
|
||||||
|
|
||||||
enum pios_bmi160_orientation { // clockwise rotation from board forward
|
enum pios_bmi160_orientation { // clockwise rotation from board forward
|
||||||
PIOS_BMI160_TOP_0DEG,
|
PIOS_BMI160_TOP_0DEG,
|
||||||
PIOS_BMI160_TOP_90DEG,
|
PIOS_BMI160_TOP_90DEG,
|
||||||
|
@ -66,5 +68,9 @@ enum bmi160_gyro_range {
|
||||||
BMI160_RANGE_2000DPS = 0x00,
|
BMI160_RANGE_2000DPS = 0x00,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
bool bmi160SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
|
||||||
|
bool bmi160SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
|
||||||
|
|
||||||
|
bool bmi160Detect(const busDevice_t *bus);
|
||||||
bool bmi160SpiAccDetect(accDev_t *acc);
|
bool bmi160SpiAccDetect(accDev_t *acc);
|
||||||
bool bmi160SpiGyroDetect(gyroDev_t *gyro);
|
bool bmi160SpiGyroDetect(gyroDev_t *gyro);
|
||||||
|
|
|
@ -36,32 +36,30 @@
|
||||||
#include "accgyro_mpu.h"
|
#include "accgyro_mpu.h"
|
||||||
#include "accgyro_spi_icm20689.h"
|
#include "accgyro_spi_icm20689.h"
|
||||||
|
|
||||||
#define DISABLE_ICM20689 IOHi(icmSpi20689CsPin)
|
#define DISABLE_ICM20689(spiCsnPin) IOHi(spiCsnPin)
|
||||||
#define ENABLE_ICM20689 IOLo(icmSpi20689CsPin)
|
#define ENABLE_ICM20689(spiCsnPin) IOLo(spiCsnPin)
|
||||||
|
|
||||||
static IO_t icmSpi20689CsPin = IO_NONE;
|
bool icm20689SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
|
||||||
|
|
||||||
bool icm20689WriteRegister(uint8_t reg, uint8_t data)
|
|
||||||
{
|
{
|
||||||
ENABLE_ICM20689;
|
ENABLE_ICM20689(bus->spi.csnPin);
|
||||||
spiTransferByte(ICM20689_SPI_INSTANCE, reg);
|
spiTransferByte(ICM20689_SPI_INSTANCE, reg);
|
||||||
spiTransferByte(ICM20689_SPI_INSTANCE, data);
|
spiTransferByte(ICM20689_SPI_INSTANCE, data);
|
||||||
DISABLE_ICM20689;
|
DISABLE_ICM20689(bus->spi.csnPin);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
bool icm20689SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
|
||||||
{
|
{
|
||||||
ENABLE_ICM20689;
|
ENABLE_ICM20689(bus->spi.csnPin);
|
||||||
spiTransferByte(ICM20689_SPI_INSTANCE, reg | 0x80); // read transaction
|
spiTransferByte(ICM20689_SPI_INSTANCE, reg | 0x80); // read transaction
|
||||||
spiTransfer(ICM20689_SPI_INSTANCE, data, NULL, length);
|
spiTransfer(ICM20689_SPI_INSTANCE, data, NULL, length);
|
||||||
DISABLE_ICM20689;
|
DISABLE_ICM20689(bus->spi.csnPin);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void icm20689SpiInit(void)
|
static void icm20689SpiInit(const busDevice_t *bus)
|
||||||
{
|
{
|
||||||
static bool hardwareInitialised = false;
|
static bool hardwareInitialised = false;
|
||||||
|
|
||||||
|
@ -69,30 +67,29 @@ static void icm20689SpiInit(void)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
icmSpi20689CsPin = IOGetByTag(IO_TAG(ICM20689_CS_PIN));
|
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
||||||
IOInit(icmSpi20689CsPin, OWNER_MPU_CS, 0);
|
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
|
||||||
IOConfigGPIO(icmSpi20689CsPin, SPI_IO_CS_CFG);
|
|
||||||
|
|
||||||
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
|
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
|
||||||
|
|
||||||
hardwareInitialised = true;
|
hardwareInitialised = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool icm20689SpiDetect(void)
|
bool icm20689SpiDetect(const busDevice_t *bus)
|
||||||
{
|
{
|
||||||
uint8_t tmp;
|
uint8_t tmp;
|
||||||
uint8_t attemptsRemaining = 20;
|
uint8_t attemptsRemaining = 20;
|
||||||
|
|
||||||
icm20689SpiInit();
|
icm20689SpiInit(bus);
|
||||||
|
|
||||||
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
|
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
|
||||||
|
|
||||||
icm20689WriteRegister(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
|
icm20689SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
|
||||||
|
|
||||||
do {
|
do {
|
||||||
delay(150);
|
delay(150);
|
||||||
|
|
||||||
icm20689ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp);
|
icm20689SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
|
||||||
if (tmp == ICM20689_WHO_AM_I_CONST) {
|
if (tmp == ICM20689_WHO_AM_I_CONST) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -130,32 +127,32 @@ void icm20689GyroInit(gyroDev_t *gyro)
|
||||||
|
|
||||||
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
|
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
|
||||||
|
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
|
||||||
delay(100);
|
delay(100);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x03);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
|
||||||
delay(100);
|
delay(100);
|
||||||
// gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0);
|
// gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
|
||||||
// delay(100);
|
// delay(100);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||||
delay(15);
|
delay(15);
|
||||||
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
|
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
|
||||||
delay(15);
|
delay(15);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
||||||
delay(15);
|
delay(15);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf);
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
|
||||||
delay(15);
|
delay(15);
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
|
||||||
delay(100);
|
delay(100);
|
||||||
|
|
||||||
// Data ready interrupt configuration
|
// Data ready interrupt configuration
|
||||||
// gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
|
// gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
|
||||||
|
|
||||||
delay(15);
|
delay(15);
|
||||||
|
|
||||||
#ifdef USE_MPU_DATA_READY_SIGNAL
|
#ifdef USE_MPU_DATA_READY_SIGNAL
|
||||||
gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
|
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
|
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
|
||||||
|
|
|
@ -16,6 +16,8 @@
|
||||||
*/
|
*/
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "sensor.h"
|
||||||
|
|
||||||
#define ICM20689_WHO_AM_I_CONST (0x98)
|
#define ICM20689_WHO_AM_I_CONST (0x98)
|
||||||
#define ICM20689_BIT_RESET (0x80)
|
#define ICM20689_BIT_RESET (0x80)
|
||||||
|
|
||||||
|
@ -25,10 +27,10 @@ bool icm20689GyroDetect(gyroDev_t *gyro);
|
||||||
void icm20689AccInit(accDev_t *acc);
|
void icm20689AccInit(accDev_t *acc);
|
||||||
void icm20689GyroInit(gyroDev_t *gyro);
|
void icm20689GyroInit(gyroDev_t *gyro);
|
||||||
|
|
||||||
bool icm20689SpiDetect(void);
|
bool icm20689SpiDetect(const busDevice_t *bus);
|
||||||
|
|
||||||
bool icm20689SpiAccDetect(accDev_t *acc);
|
bool icm20689SpiAccDetect(accDev_t *acc);
|
||||||
bool icm20689SpiGyroDetect(gyroDev_t *gyro);
|
bool icm20689SpiGyroDetect(gyroDev_t *gyro);
|
||||||
|
|
||||||
bool icm20689WriteRegister(uint8_t reg, uint8_t data);
|
bool icm20689SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
|
||||||
bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
|
bool icm20689SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
|
||||||
|
|
|
@ -99,27 +99,25 @@ static bool mpuSpi6000InitDone = false;
|
||||||
#define MPU6000_REV_D9 0x59
|
#define MPU6000_REV_D9 0x59
|
||||||
#define MPU6000_REV_D10 0x5A
|
#define MPU6000_REV_D10 0x5A
|
||||||
|
|
||||||
#define DISABLE_MPU6000 IOHi(mpuSpi6000CsPin)
|
#define DISABLE_MPU6000(spiCsnPin) IOHi(spiCsnPin)
|
||||||
#define ENABLE_MPU6000 IOLo(mpuSpi6000CsPin)
|
#define ENABLE_MPU6000(spiCsnPin) IOLo(spiCsnPin)
|
||||||
|
|
||||||
static IO_t mpuSpi6000CsPin = IO_NONE;
|
bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
|
||||||
|
|
||||||
bool mpu6000WriteRegister(uint8_t reg, uint8_t data)
|
|
||||||
{
|
{
|
||||||
ENABLE_MPU6000;
|
ENABLE_MPU6000(bus->spi.csnPin);
|
||||||
spiTransferByte(MPU6000_SPI_INSTANCE, reg);
|
spiTransferByte(MPU6000_SPI_INSTANCE, reg);
|
||||||
spiTransferByte(MPU6000_SPI_INSTANCE, data);
|
spiTransferByte(MPU6000_SPI_INSTANCE, data);
|
||||||
DISABLE_MPU6000;
|
DISABLE_MPU6000(bus->spi.csnPin);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
|
||||||
{
|
{
|
||||||
ENABLE_MPU6000;
|
ENABLE_MPU6000(bus->spi.csnPin);
|
||||||
spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction
|
spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction
|
||||||
spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length);
|
spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length);
|
||||||
DISABLE_MPU6000;
|
DISABLE_MPU6000(bus->spi.csnPin);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -133,7 +131,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro)
|
||||||
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
|
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
|
||||||
|
|
||||||
// Accel and Gyro DLPF Setting
|
// Accel and Gyro DLPF Setting
|
||||||
mpu6000WriteRegister(MPU6000_CONFIG, gyro->lpf);
|
mpu6000SpiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf);
|
||||||
delayMicroseconds(1);
|
delayMicroseconds(1);
|
||||||
|
|
||||||
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock
|
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock
|
||||||
|
@ -150,25 +148,22 @@ void mpu6000SpiAccInit(accDev_t *acc)
|
||||||
acc->acc_1G = 512 * 4;
|
acc->acc_1G = 512 * 4;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool mpu6000SpiDetect(void)
|
bool mpu6000SpiDetect(const busDevice_t *bus)
|
||||||
{
|
{
|
||||||
uint8_t in;
|
uint8_t in;
|
||||||
uint8_t attemptsRemaining = 5;
|
uint8_t attemptsRemaining = 5;
|
||||||
|
|
||||||
#ifdef MPU6000_CS_PIN
|
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
||||||
mpuSpi6000CsPin = IOGetByTag(IO_TAG(MPU6000_CS_PIN));
|
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
|
||||||
#endif
|
|
||||||
IOInit(mpuSpi6000CsPin, OWNER_MPU_CS, 0);
|
|
||||||
IOConfigGPIO(mpuSpi6000CsPin, SPI_IO_CS_CFG);
|
|
||||||
|
|
||||||
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
|
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
|
||||||
|
|
||||||
mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET);
|
mpu6000SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
|
||||||
|
|
||||||
do {
|
do {
|
||||||
delay(150);
|
delay(150);
|
||||||
|
|
||||||
mpu6000ReadRegister(MPU_RA_WHO_AM_I, 1, &in);
|
mpu6000SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
|
||||||
if (in == MPU6000_WHO_AM_I_CONST) {
|
if (in == MPU6000_WHO_AM_I_CONST) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -177,7 +172,7 @@ bool mpu6000SpiDetect(void)
|
||||||
}
|
}
|
||||||
} while (attemptsRemaining--);
|
} while (attemptsRemaining--);
|
||||||
|
|
||||||
mpu6000ReadRegister(MPU_RA_PRODUCT_ID, 1, &in);
|
mpu6000SpiReadRegister(bus, MPU_RA_PRODUCT_ID, 1, &in);
|
||||||
|
|
||||||
/* look for a product ID we recognise */
|
/* look for a product ID we recognise */
|
||||||
|
|
||||||
|
@ -210,41 +205,41 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
|
||||||
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
|
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
|
||||||
|
|
||||||
// Device Reset
|
// Device Reset
|
||||||
mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET);
|
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
|
||||||
delay(150);
|
delay(150);
|
||||||
|
|
||||||
mpu6000WriteRegister(MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
|
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
|
||||||
delay(150);
|
delay(150);
|
||||||
|
|
||||||
// Clock Source PPL with Z axis gyro reference
|
// Clock Source PPL with Z axis gyro reference
|
||||||
mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
|
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
|
||||||
delayMicroseconds(15);
|
delayMicroseconds(15);
|
||||||
|
|
||||||
// Disable Primary I2C Interface
|
// Disable Primary I2C Interface
|
||||||
mpu6000WriteRegister(MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
|
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
|
||||||
delayMicroseconds(15);
|
delayMicroseconds(15);
|
||||||
|
|
||||||
mpu6000WriteRegister(MPU_RA_PWR_MGMT_2, 0x00);
|
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00);
|
||||||
delayMicroseconds(15);
|
delayMicroseconds(15);
|
||||||
|
|
||||||
// Accel Sample Rate 1kHz
|
// Accel Sample Rate 1kHz
|
||||||
// Gyroscope Output Rate = 1kHz when the DLPF is enabled
|
// Gyroscope Output Rate = 1kHz when the DLPF is enabled
|
||||||
mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
|
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
|
||||||
delayMicroseconds(15);
|
delayMicroseconds(15);
|
||||||
|
|
||||||
// Gyro +/- 1000 DPS Full Scale
|
// Gyro +/- 1000 DPS Full Scale
|
||||||
mpu6000WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
|
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
|
||||||
delayMicroseconds(15);
|
delayMicroseconds(15);
|
||||||
|
|
||||||
// Accel +/- 8 G Full Scale
|
// Accel +/- 8 G Full Scale
|
||||||
mpu6000WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
|
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
|
||||||
delayMicroseconds(15);
|
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
|
mpu6000SpiWriteRegister(&gyro->bus, 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);
|
delayMicroseconds(15);
|
||||||
|
|
||||||
#ifdef USE_MPU_DATA_READY_SIGNAL
|
#ifdef USE_MPU_DATA_READY_SIGNAL
|
||||||
mpu6000WriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
|
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
|
||||||
delayMicroseconds(15);
|
delayMicroseconds(15);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -1,6 +1,8 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "sensor.h"
|
||||||
|
|
||||||
#define MPU6000_CONFIG 0x1A
|
#define MPU6000_CONFIG 0x1A
|
||||||
|
|
||||||
#define BITS_DLPF_CFG_256HZ 0x00
|
#define BITS_DLPF_CFG_256HZ 0x00
|
||||||
|
@ -15,10 +17,10 @@
|
||||||
// RF = Register Flag
|
// RF = Register Flag
|
||||||
#define MPU_RF_DATA_RDY_EN (1 << 0)
|
#define MPU_RF_DATA_RDY_EN (1 << 0)
|
||||||
|
|
||||||
bool mpu6000SpiDetect(void);
|
bool mpu6000SpiDetect(const busDevice_t *bus);
|
||||||
|
|
||||||
bool mpu6000SpiAccDetect(accDev_t *acc);
|
bool mpu6000SpiAccDetect(accDev_t *acc);
|
||||||
bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
|
bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
|
||||||
|
|
||||||
bool mpu6000WriteRegister(uint8_t reg, uint8_t data);
|
bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
|
||||||
bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
|
bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
|
||||||
|
|
|
@ -34,32 +34,32 @@
|
||||||
#include "accgyro_mpu6500.h"
|
#include "accgyro_mpu6500.h"
|
||||||
#include "accgyro_spi_mpu6500.h"
|
#include "accgyro_spi_mpu6500.h"
|
||||||
|
|
||||||
#define DISABLE_MPU6500 IOHi(mpuSpi6500CsPin)
|
#define DISABLE_MPU6500(spiCsnPin) IOHi(spiCsnPin)
|
||||||
#define ENABLE_MPU6500 IOLo(mpuSpi6500CsPin)
|
#define ENABLE_MPU6500(spiCsnPin) IOLo(spiCsnPin)
|
||||||
|
|
||||||
static IO_t mpuSpi6500CsPin = IO_NONE;
|
#define BIT_SLEEP 0x40
|
||||||
|
|
||||||
bool mpu6500WriteRegister(uint8_t reg, uint8_t data)
|
bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
|
||||||
{
|
{
|
||||||
ENABLE_MPU6500;
|
ENABLE_MPU6500(bus->spi.csnPin);
|
||||||
spiTransferByte(MPU6500_SPI_INSTANCE, reg);
|
spiTransferByte(MPU6500_SPI_INSTANCE, reg);
|
||||||
spiTransferByte(MPU6500_SPI_INSTANCE, data);
|
spiTransferByte(MPU6500_SPI_INSTANCE, data);
|
||||||
DISABLE_MPU6500;
|
DISABLE_MPU6500(bus->spi.csnPin);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
|
||||||
{
|
{
|
||||||
ENABLE_MPU6500;
|
ENABLE_MPU6500(bus->spi.csnPin);
|
||||||
spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
|
spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
|
||||||
spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
|
spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
|
||||||
DISABLE_MPU6500;
|
DISABLE_MPU6500(bus->spi.csnPin);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void mpu6500SpiInit(void)
|
static void mpu6500SpiInit(const busDevice_t *bus)
|
||||||
{
|
{
|
||||||
static bool hardwareInitialised = false;
|
static bool hardwareInitialised = false;
|
||||||
|
|
||||||
|
@ -67,24 +67,22 @@ static void mpu6500SpiInit(void)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
mpuSpi6500CsPin = IOGetByTag(IO_TAG(MPU6500_CS_PIN));
|
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
||||||
IOInit(mpuSpi6500CsPin, OWNER_MPU_CS, 0);
|
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
|
||||||
IOConfigGPIO(mpuSpi6500CsPin, SPI_IO_CS_CFG);
|
|
||||||
|
|
||||||
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
|
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
|
||||||
|
|
||||||
hardwareInitialised = true;
|
hardwareInitialised = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint8_t mpuDetected = MPU_NONE;
|
uint8_t mpu6500SpiDetect(const busDevice_t *bus)
|
||||||
uint8_t mpu6500SpiDetect(void)
|
|
||||||
{
|
{
|
||||||
|
mpu6500SpiInit(bus);
|
||||||
|
|
||||||
uint8_t tmp;
|
uint8_t tmp;
|
||||||
|
mpu6500SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
|
||||||
|
|
||||||
mpu6500SpiInit();
|
uint8_t mpuDetected = MPU_NONE;
|
||||||
|
|
||||||
mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp);
|
|
||||||
|
|
||||||
switch (tmp) {
|
switch (tmp) {
|
||||||
case MPU6500_WHO_AM_I_CONST:
|
case MPU6500_WHO_AM_I_CONST:
|
||||||
mpuDetected = MPU_65xx_SPI;
|
mpuDetected = MPU_65xx_SPI;
|
||||||
|
@ -118,7 +116,7 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro)
|
||||||
mpu6500GyroInit(gyro);
|
mpu6500GyroInit(gyro);
|
||||||
|
|
||||||
// Disable Primary I2C Interface
|
// Disable Primary I2C Interface
|
||||||
mpu6500WriteRegister(MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
|
mpu6500SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
|
||||||
delay(100);
|
delay(100);
|
||||||
|
|
||||||
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
|
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
|
||||||
|
@ -127,7 +125,14 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro)
|
||||||
|
|
||||||
bool mpu6500SpiAccDetect(accDev_t *acc)
|
bool mpu6500SpiAccDetect(accDev_t *acc)
|
||||||
{
|
{
|
||||||
if (acc->mpuDetectionResult.sensor != mpuDetected || !mpuDetected) {
|
// MPU6500 is used as a equivalent of other accelerometers by some flight controllers
|
||||||
|
switch (acc->mpuDetectionResult.sensor) {
|
||||||
|
case MPU_65xx_SPI:
|
||||||
|
case MPU_9250_SPI:
|
||||||
|
case ICM_20608_SPI:
|
||||||
|
case ICM_20602_SPI:
|
||||||
|
break;
|
||||||
|
default:
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -139,7 +144,14 @@ bool mpu6500SpiAccDetect(accDev_t *acc)
|
||||||
|
|
||||||
bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
|
bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
if (gyro->mpuDetectionResult.sensor != mpuDetected || !mpuDetected) {
|
// MPU6500 is used as a equivalent of other gyros by some flight controllers
|
||||||
|
switch (gyro->mpuDetectionResult.sensor) {
|
||||||
|
case MPU_65xx_SPI:
|
||||||
|
case MPU_9250_SPI:
|
||||||
|
case ICM_20608_SPI:
|
||||||
|
case ICM_20602_SPI:
|
||||||
|
break;
|
||||||
|
default:
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -17,13 +17,15 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
uint8_t mpu6500SpiDetect(void);
|
#include "sensor.h"
|
||||||
|
|
||||||
void mpu6500SpiAccInit(accDev_t *acc);
|
uint8_t mpu6500SpiDetect(const busDevice_t *bus);
|
||||||
void mpu6500SpiGyroInit(gyroDev_t *gyro);
|
|
||||||
|
|
||||||
bool mpu6500SpiAccDetect(accDev_t *acc);
|
bool mpu6500SpiAccDetect(accDev_t *acc);
|
||||||
bool mpu6500SpiGyroDetect(gyroDev_t *gyro);
|
bool mpu6500SpiGyroDetect(gyroDev_t *gyro);
|
||||||
|
|
||||||
bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
|
bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
|
||||||
bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
|
bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
|
||||||
|
|
||||||
|
void mpu6500SpiGyroInit(gyroDev_t *gyro);
|
||||||
|
void mpu6500SpiAccInit(accDev_t *acc);
|
||||||
|
|
|
@ -50,47 +50,48 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro);
|
||||||
|
|
||||||
static bool mpuSpi9250InitDone = false;
|
static bool mpuSpi9250InitDone = false;
|
||||||
|
|
||||||
static IO_t mpuSpi9250CsPin = IO_NONE;
|
#define DISABLE_MPU9250(spiCsnPin) IOHi(spiCsnPin)
|
||||||
|
#define ENABLE_MPU9250(spiCsnPin) IOLo(spiCsnPin)
|
||||||
|
|
||||||
#define DISABLE_MPU9250 IOHi(mpuSpi9250CsPin)
|
void mpu9250SpiResetGyro(void)
|
||||||
#define ENABLE_MPU9250 IOLo(mpuSpi9250CsPin)
|
|
||||||
|
|
||||||
void mpu9250ResetGyro(void)
|
|
||||||
{
|
{
|
||||||
// Device Reset
|
// Device Reset
|
||||||
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
#ifdef MPU9250_CS_PIN
|
||||||
|
busDevice_t bus = { .spi = { .csnPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN)) } };
|
||||||
|
mpu9250SpiWriteRegister(&bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
||||||
delay(150);
|
delay(150);
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
bool mpu9250WriteRegister(uint8_t reg, uint8_t data)
|
bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
|
||||||
{
|
{
|
||||||
ENABLE_MPU9250;
|
ENABLE_MPU9250(bus->spi.csnPin);
|
||||||
delayMicroseconds(1);
|
delayMicroseconds(1);
|
||||||
spiTransferByte(MPU9250_SPI_INSTANCE, reg);
|
spiTransferByte(MPU9250_SPI_INSTANCE, reg);
|
||||||
spiTransferByte(MPU9250_SPI_INSTANCE, data);
|
spiTransferByte(MPU9250_SPI_INSTANCE, data);
|
||||||
DISABLE_MPU9250;
|
DISABLE_MPU9250(bus->spi.csnPin);
|
||||||
delayMicroseconds(1);
|
delayMicroseconds(1);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
|
||||||
{
|
{
|
||||||
ENABLE_MPU9250;
|
ENABLE_MPU9250(bus->spi.csnPin);
|
||||||
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
|
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
|
||||||
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
|
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
|
||||||
DISABLE_MPU9250;
|
DISABLE_MPU9250(bus->spi.csnPin);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
|
||||||
{
|
{
|
||||||
ENABLE_MPU9250;
|
ENABLE_MPU9250(bus->spi.csnPin);
|
||||||
delayMicroseconds(1);
|
delayMicroseconds(1);
|
||||||
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
|
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
|
||||||
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
|
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
|
||||||
DISABLE_MPU9250;
|
DISABLE_MPU9250(bus->spi.csnPin);
|
||||||
delayMicroseconds(1);
|
delayMicroseconds(1);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
|
@ -119,21 +120,21 @@ void mpu9250SpiAccInit(accDev_t *acc)
|
||||||
acc->acc_1G = 512 * 8;
|
acc->acc_1G = 512 * 8;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool verifympu9250WriteRegister(uint8_t reg, uint8_t data)
|
bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
|
||||||
{
|
{
|
||||||
uint8_t in;
|
uint8_t in;
|
||||||
uint8_t attemptsRemaining = 20;
|
uint8_t attemptsRemaining = 20;
|
||||||
|
|
||||||
mpu9250WriteRegister(reg, data);
|
mpu9250SpiWriteRegister(bus, reg, data);
|
||||||
delayMicroseconds(100);
|
delayMicroseconds(100);
|
||||||
|
|
||||||
do {
|
do {
|
||||||
mpu9250SlowReadRegister(reg, 1, &in);
|
mpu9250SpiSlowReadRegister(bus, reg, 1, &in);
|
||||||
if (in == data) {
|
if (in == data) {
|
||||||
return true;
|
return true;
|
||||||
} else {
|
} else {
|
||||||
debug[3]++;
|
debug[3]++;
|
||||||
mpu9250WriteRegister(reg, data);
|
mpu9250SpiWriteRegister(bus, reg, data);
|
||||||
delayMicroseconds(100);
|
delayMicroseconds(100);
|
||||||
}
|
}
|
||||||
} while (attemptsRemaining--);
|
} while (attemptsRemaining--);
|
||||||
|
@ -148,30 +149,30 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
|
||||||
|
|
||||||
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
|
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
|
||||||
|
|
||||||
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
||||||
delay(50);
|
delay(50);
|
||||||
|
|
||||||
verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||||
|
|
||||||
//Fchoice_b defaults to 00 which makes fchoice 11
|
//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);
|
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);
|
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
|
||||||
|
|
||||||
if (gyro->lpf == 4) {
|
if (gyro->lpf == 4) {
|
||||||
verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
|
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF
|
||||||
} else if (gyro->lpf < 4) {
|
} else if (gyro->lpf < 4) {
|
||||||
verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
|
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
|
||||||
} else if (gyro->lpf > 4) {
|
} else if (gyro->lpf > 4) {
|
||||||
verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF
|
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF
|
||||||
}
|
}
|
||||||
|
|
||||||
verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
|
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
|
||||||
|
|
||||||
verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
|
verifympu9250SpiWriteRegister(&gyro->bus, 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
|
verifympu9250SpiWriteRegister(&gyro->bus, 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)
|
#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.
|
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
|
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
|
||||||
|
@ -179,25 +180,21 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
|
||||||
mpuSpi9250InitDone = true; //init done
|
mpuSpi9250InitDone = true; //init done
|
||||||
}
|
}
|
||||||
|
|
||||||
bool mpu9250SpiDetect(void)
|
bool mpu9250SpiDetect(const busDevice_t *bus)
|
||||||
{
|
{
|
||||||
uint8_t in;
|
uint8_t in;
|
||||||
uint8_t attemptsRemaining = 20;
|
uint8_t attemptsRemaining = 20;
|
||||||
|
|
||||||
/* not the best place for this - should really have an init method */
|
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
||||||
#ifdef MPU9250_CS_PIN
|
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
|
||||||
mpuSpi9250CsPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN));
|
|
||||||
#endif
|
|
||||||
IOInit(mpuSpi9250CsPin, OWNER_MPU_CS, 0);
|
|
||||||
IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG);
|
|
||||||
|
|
||||||
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
|
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
|
||||||
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
||||||
|
|
||||||
do {
|
do {
|
||||||
delay(150);
|
delay(150);
|
||||||
|
|
||||||
mpu9250ReadRegister(MPU_RA_WHO_AM_I, 1, &in);
|
mpu9250SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
|
||||||
if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) {
|
if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,6 +1,8 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "sensor.h"
|
||||||
|
|
||||||
#define mpu9250_CONFIG 0x1A
|
#define mpu9250_CONFIG 0x1A
|
||||||
|
|
||||||
/* We should probably use these. :)
|
/* We should probably use these. :)
|
||||||
|
@ -24,14 +26,14 @@
|
||||||
// RF = Register Flag
|
// RF = Register Flag
|
||||||
#define MPU_RF_DATA_RDY_EN (1 << 0)
|
#define MPU_RF_DATA_RDY_EN (1 << 0)
|
||||||
|
|
||||||
void mpu9250ResetGyro(void);
|
void mpu9250SpiResetGyro(void);
|
||||||
|
|
||||||
bool mpu9250SpiDetect(void);
|
bool mpu9250SpiDetect(const busDevice_t *bus);
|
||||||
|
|
||||||
bool mpu9250SpiAccDetect(accDev_t *acc);
|
bool mpu9250SpiAccDetect(accDev_t *acc);
|
||||||
bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
|
bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
|
||||||
|
|
||||||
bool mpu9250WriteRegister(uint8_t reg, uint8_t data);
|
bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
|
||||||
bool verifympu9250WriteRegister(uint8_t reg, uint8_t data);
|
bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
|
||||||
bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
|
bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
|
||||||
bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
|
bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
|
||||||
|
|
|
@ -22,6 +22,7 @@
|
||||||
typedef struct magDev_s {
|
typedef struct magDev_s {
|
||||||
sensorInitFuncPtr init; // initialize function
|
sensorInitFuncPtr init; // initialize function
|
||||||
sensorReadFuncPtr read; // read 3 axis data function
|
sensorReadFuncPtr read; // read 3 axis data function
|
||||||
|
busDevice_t bus;
|
||||||
sensor_align_e magAlign;
|
sensor_align_e magAlign;
|
||||||
} magDev_t;
|
} magDev_t;
|
||||||
|
|
||||||
|
|
|
@ -86,9 +86,9 @@ static float magGain[3] = { 1.0f, 1.0f, 1.0f };
|
||||||
// Is an separate MPU9250 driver really needed? The GYRO/ACC part between MPU6500 and MPU9250 is exactly the same.
|
// Is an separate MPU9250 driver really needed? The GYRO/ACC part between MPU6500 and MPU9250 is exactly the same.
|
||||||
#if defined(MPU6500_SPI_INSTANCE) && !defined(MPU9250_SPI_INSTANCE)
|
#if defined(MPU6500_SPI_INSTANCE) && !defined(MPU9250_SPI_INSTANCE)
|
||||||
#define MPU9250_SPI_INSTANCE
|
#define MPU9250_SPI_INSTANCE
|
||||||
#define verifympu9250WriteRegister mpu6500WriteRegister
|
#define verifympu9250SpiWriteRegister mpu6500SpiWriteRegister
|
||||||
#define mpu9250WriteRegister mpu6500WriteRegister
|
#define mpu9250SpiWriteRegister mpu6500SpiWriteRegister
|
||||||
#define mpu9250ReadRegister mpu6500ReadRegister
|
#define mpu9250SpiReadRegister mpu6500SpiReadRegister
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
|
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
|
||||||
|
@ -109,22 +109,22 @@ static queuedReadState_t queuedRead = { false, 0, 0};
|
||||||
|
|
||||||
static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf)
|
static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf)
|
||||||
{
|
{
|
||||||
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
|
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
|
||||||
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
|
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
|
||||||
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
|
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
|
||||||
delay(10);
|
delay(10);
|
||||||
__disable_irq();
|
__disable_irq();
|
||||||
mpu9250ReadRegister(MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C
|
mpuReadRegisterI2C(NULL, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C
|
||||||
__enable_irq();
|
__enable_irq();
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
|
static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
|
||||||
{
|
{
|
||||||
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write
|
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write
|
||||||
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
|
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
|
||||||
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_DO, data); // set I2C salve value
|
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value
|
||||||
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte
|
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -136,9 +136,9 @@ static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_)
|
||||||
|
|
||||||
queuedRead.len = len_;
|
queuedRead.len = len_;
|
||||||
|
|
||||||
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
|
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
|
||||||
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
|
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
|
||||||
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
|
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
|
||||||
|
|
||||||
queuedRead.readStartedAt = micros();
|
queuedRead.readStartedAt = micros();
|
||||||
queuedRead.waiting = true;
|
queuedRead.waiting = true;
|
||||||
|
@ -173,7 +173,7 @@ static bool ak8963SensorCompleteRead(uint8_t *buf)
|
||||||
|
|
||||||
queuedRead.waiting = false;
|
queuedRead.waiting = false;
|
||||||
|
|
||||||
mpu9250ReadRegister(MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer
|
mpuReadRegisterI2C(NULL, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
|
@ -316,13 +316,13 @@ bool ak8963Detect(magDev_t *mag)
|
||||||
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
|
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
|
||||||
// initialze I2C master via SPI bus (MPU9250)
|
// initialze I2C master via SPI bus (MPU9250)
|
||||||
|
|
||||||
verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR
|
verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR
|
||||||
delay(10);
|
delay(10);
|
||||||
|
|
||||||
verifympu9250WriteRegister(MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
|
verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
|
||||||
delay(10);
|
delay(10);
|
||||||
|
|
||||||
verifympu9250WriteRegister(MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
|
verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
|
||||||
delay(10);
|
delay(10);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -20,6 +20,8 @@
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#include "io_types.h"
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
ALIGN_DEFAULT = 0, // driver-provided alignment
|
ALIGN_DEFAULT = 0, // driver-provided alignment
|
||||||
CW0_DEG = 1,
|
CW0_DEG = 1,
|
||||||
|
@ -32,6 +34,12 @@ typedef enum {
|
||||||
CW270_DEG_FLIP = 8
|
CW270_DEG_FLIP = 8
|
||||||
} sensor_align_e;
|
} sensor_align_e;
|
||||||
|
|
||||||
|
typedef union busDevice_t {
|
||||||
|
struct deviceSpi_s {
|
||||||
|
IO_t csnPin;
|
||||||
|
} spi;
|
||||||
|
} busDevice_t;
|
||||||
|
|
||||||
typedef bool (*sensorInitFuncPtr)(void); // sensor init prototype
|
typedef bool (*sensorInitFuncPtr)(void); // sensor init prototype
|
||||||
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
||||||
typedef bool (*sensorInterruptFuncPtr)(void);
|
typedef bool (*sensorInterruptFuncPtr)(void);
|
||||||
|
|
|
@ -519,6 +519,9 @@ static const clivalue_t valueTable[] = {
|
||||||
{ "gyro_isr_update", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_isr_update) },
|
{ "gyro_isr_update", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_isr_update) },
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
#ifdef USE_DUAL_GYRO
|
||||||
|
{ "gyro_to_use", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 1 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
|
||||||
|
#endif
|
||||||
|
|
||||||
// PG_ACCELEROMETER_CONFIG
|
// PG_ACCELEROMETER_CONFIG
|
||||||
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) },
|
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) },
|
||||||
|
|
|
@ -300,6 +300,7 @@ bool accInit(uint32_t gyroSamplingInverval)
|
||||||
{
|
{
|
||||||
memset(&acc, 0, sizeof(acc));
|
memset(&acc, 0, sizeof(acc));
|
||||||
// copy over the common gyro mpu settings
|
// copy over the common gyro mpu settings
|
||||||
|
acc.dev.bus = *gyroSensorBus();
|
||||||
acc.dev.mpuConfiguration = *gyroMpuConfiguration();
|
acc.dev.mpuConfiguration = *gyroMpuConfiguration();
|
||||||
acc.dev.mpuDetectionResult = *gyroMpuDetectionResult();
|
acc.dev.mpuDetectionResult = *gyroMpuDetectionResult();
|
||||||
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
|
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
|
||||||
|
|
|
@ -36,9 +36,10 @@
|
||||||
#include "fc/config.h"
|
#include "fc/config.h"
|
||||||
#include "fc/runtime_config.h"
|
#include "fc/runtime_config.h"
|
||||||
|
|
||||||
#include "sensors/sensors.h"
|
|
||||||
#include "sensors/boardalignment.h"
|
#include "sensors/boardalignment.h"
|
||||||
#include "sensors/compass.h"
|
#include "sensors/compass.h"
|
||||||
|
#include "sensors/gyro.h"
|
||||||
|
#include "sensors/sensors.h"
|
||||||
|
|
||||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||||
#include "hardware_revision.h"
|
#include "hardware_revision.h"
|
||||||
|
@ -141,9 +142,12 @@ bool compassInit(void)
|
||||||
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
|
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
|
||||||
// calculate magnetic declination
|
// calculate magnetic declination
|
||||||
mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
|
mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
|
||||||
|
// copy over SPI bus settings for AK8963 compass
|
||||||
|
magDev.bus = *gyroSensorBus();
|
||||||
if (!compassDetect(&magDev, compassConfig()->mag_hardware)) {
|
if (!compassDetect(&magDev, compassConfig()->mag_hardware)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
const int16_t deg = compassConfig()->mag_declination / 100;
|
const int16_t deg = compassConfig()->mag_declination / 100;
|
||||||
const int16_t min = compassConfig()->mag_declination % 100;
|
const int16_t min = compassConfig()->mag_declination % 100;
|
||||||
mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
|
mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
|
||||||
|
|
|
@ -96,16 +96,19 @@ static void *notchFilter2[3];
|
||||||
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0);
|
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0);
|
||||||
|
|
||||||
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
|
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
|
||||||
.gyro_lpf = GYRO_LPF_256HZ,
|
.gyro_align = ALIGN_DEFAULT,
|
||||||
|
.gyroMovementCalibrationThreshold = 48,
|
||||||
.gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT,
|
.gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT,
|
||||||
|
.gyro_lpf = GYRO_LPF_256HZ,
|
||||||
.gyro_soft_lpf_type = FILTER_PT1,
|
.gyro_soft_lpf_type = FILTER_PT1,
|
||||||
.gyro_soft_lpf_hz = 90,
|
.gyro_soft_lpf_hz = 90,
|
||||||
|
.gyro_isr_update = false,
|
||||||
|
.gyro_use_32khz = false,
|
||||||
|
.gyro_to_use = 0,
|
||||||
.gyro_soft_notch_hz_1 = 400,
|
.gyro_soft_notch_hz_1 = 400,
|
||||||
.gyro_soft_notch_cutoff_1 = 300,
|
.gyro_soft_notch_cutoff_1 = 300,
|
||||||
.gyro_soft_notch_hz_2 = 200,
|
.gyro_soft_notch_hz_2 = 200,
|
||||||
.gyro_soft_notch_cutoff_2 = 100,
|
.gyro_soft_notch_cutoff_2 = 100
|
||||||
.gyro_align = ALIGN_DEFAULT,
|
|
||||||
.gyroMovementCalibrationThreshold = 48
|
|
||||||
);
|
);
|
||||||
|
|
||||||
#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_ICM20689)
|
#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_ICM20689)
|
||||||
|
@ -122,6 +125,11 @@ static const extiConfig_t *selectMPUIntExtiConfig(void)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
const busDevice_t *gyroSensorBus(void)
|
||||||
|
{
|
||||||
|
return &gyroDev0.bus;
|
||||||
|
}
|
||||||
|
|
||||||
const mpuConfiguration_t *gyroMpuConfiguration(void)
|
const mpuConfiguration_t *gyroMpuConfiguration(void)
|
||||||
{
|
{
|
||||||
return &gyroDev0.mpuConfiguration;
|
return &gyroDev0.mpuConfiguration;
|
||||||
|
@ -283,8 +291,14 @@ bool gyroInit(void)
|
||||||
memset(&gyro, 0, sizeof(gyro));
|
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) || defined(USE_GYRO_SPI_ICM20689)
|
#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_ICM20689)
|
||||||
gyroDev0.mpuIntExtiConfig = selectMPUIntExtiConfig();
|
gyroDev0.mpuIntExtiConfig = selectMPUIntExtiConfig();
|
||||||
|
#ifdef USE_DUAL_GYRO
|
||||||
|
// set cnsPin using GYRO_n_CS_PIN defined in target.h
|
||||||
|
gyroDev0.bus.spi.csnPin = gyroConfig()->gyro_to_use == 0 ? IOGetByTag(IO_TAG(GYRO_0_CS_PIN)) : IOGetByTag(IO_TAG(GYRO_1_CS_PIN));
|
||||||
|
#else
|
||||||
|
gyroDev0.bus.spi.csnPin = IO_NONE; // set cnsPin to IO_NONE so mpuDetect will set it according to value defined in target.h
|
||||||
|
#endif // USE_DUAL_GYRO
|
||||||
mpuDetect(&gyroDev0);
|
mpuDetect(&gyroDev0);
|
||||||
mpuResetFn = gyroDev0.mpuConfiguration.resetFn;
|
mpuResetFn = gyroDev0.mpuConfiguration.resetFn; // must be set after mpuDetect
|
||||||
#endif
|
#endif
|
||||||
const gyroSensor_e gyroHardware = gyroDetect(&gyroDev0);
|
const gyroSensor_e gyroHardware = gyroDetect(&gyroDev0);
|
||||||
if (gyroHardware == GYRO_NONE) {
|
if (gyroHardware == GYRO_NONE) {
|
||||||
|
@ -305,7 +319,7 @@ bool gyroInit(void)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Must set gyro sample rate before initialisation
|
// Must set gyro targetLooptime before gyroDev.init and initialisation of filters
|
||||||
gyro.targetLooptime = gyroSetSampleRate(&gyroDev0, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
|
gyro.targetLooptime = gyroSetSampleRate(&gyroDev0, gyroConfig()->gyro_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
|
||||||
gyroDev0.lpf = gyroConfig()->gyro_lpf;
|
gyroDev0.lpf = gyroConfig()->gyro_lpf;
|
||||||
gyroDev0.init(&gyroDev0);
|
gyroDev0.init(&gyroDev0);
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
|
|
||||||
#include "config/parameter_group.h"
|
#include "config/parameter_group.h"
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
|
#include "drivers/io_types.h"
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
@ -54,6 +55,7 @@ typedef struct gyroConfig_s {
|
||||||
uint8_t gyro_soft_lpf_hz;
|
uint8_t gyro_soft_lpf_hz;
|
||||||
bool gyro_isr_update;
|
bool gyro_isr_update;
|
||||||
bool gyro_use_32khz;
|
bool gyro_use_32khz;
|
||||||
|
uint8_t gyro_to_use;
|
||||||
uint16_t gyro_soft_notch_hz_1;
|
uint16_t gyro_soft_notch_hz_1;
|
||||||
uint16_t gyro_soft_notch_cutoff_1;
|
uint16_t gyro_soft_notch_cutoff_1;
|
||||||
uint16_t gyro_soft_notch_hz_2;
|
uint16_t gyro_soft_notch_hz_2;
|
||||||
|
@ -65,6 +67,7 @@ PG_DECLARE(gyroConfig_t, gyroConfig);
|
||||||
bool gyroInit(void);
|
bool gyroInit(void);
|
||||||
void gyroInitFilters(void);
|
void gyroInitFilters(void);
|
||||||
void gyroUpdate(void);
|
void gyroUpdate(void);
|
||||||
|
const busDevice_t *gyroSensorBus(void);
|
||||||
struct mpuConfiguration_s;
|
struct mpuConfiguration_s;
|
||||||
const struct mpuConfiguration_s *gyroMpuConfiguration(void);
|
const struct mpuConfiguration_s *gyroMpuConfiguration(void);
|
||||||
struct mpuDetectionResult_s;
|
struct mpuDetectionResult_s;
|
||||||
|
|
Loading…
Reference in New Issue