rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #2586 from martinbudden/bf_gyro_spi_selection 

Added runtime setting of gyro SPI pin
This commit is contained in:
Martin Budden 2017-03-26 10:07:34 +01:00 committed by GitHub
parent 1281d30eba 60c2b812d1
commit 3545775fdf
24 changed files with 360 additions and 292 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/drivers/accgyro.h
View File

@ -49,6 +49,7 @@ typedef struct gyroDev_s {
sensorGyroInterruptStatusFuncPtr intStatus;
sensorGyroUpdateFuncPtr update;
extiCallbackRec_t exti;
busDevice_t bus;
float scale; // scalefactor
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int32_t gyroZero[XYZ_AXIS_COUNT];
@ -66,6 +67,7 @@ typedef struct gyroDev_s {
typedef struct accDev_s {
sensorAccInitFuncPtr init; // initialize function
sensorAccReadFuncPtr read; // read 3 axis data function
busDevice_t bus;
uint16_t acc_1G;
int16_t ADCRaw[XYZ_AXIS_COUNT];
char revisionCode; // a revision code for the sensor, if known

91
src/main/drivers/accgyro_mpu.c
View File

@ -41,9 +41,10 @@
#include "accgyro_mpu3050.h"
#include "accgyro_mpu6050.h"
#include "accgyro_mpu6500.h"
#include "accgyro_spi_bmi160.h"
#include "accgyro_spi_icm20689.h"
#include "accgyro_spi_mpu6000.h"
#include "accgyro_spi_mpu6500.h"
#include "accgyro_spi_icm20689.h"
#include "accgyro_spi_mpu9250.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
// 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);
if (revision) {
/* Congrats, these parts are better. */
@ -89,7 +90,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
failureMode(FAILURE_ACC_INCOMPATIBLE);
}
} 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;
if (!revision) {
failureMode(FAILURE_ACC_INCOMPATIBLE);
@ -160,14 +161,16 @@ static void mpuIntExtiInit(gyroDev_t *gyro)
#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);
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);
return ack;
}
@ -176,7 +179,7 @@ bool mpuAccRead(accDev_t *acc)
{
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) {
return false;
}
@ -199,7 +202,7 @@ bool mpuGyroRead(gyroDev_t *gyro)
{
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) {
return false;
}
@ -226,104 +229,112 @@ bool mpuCheckDataReady(gyroDev_t* gyro)
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
{
UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI
#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->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6000ReadRegister;
gyro->mpuConfiguration.writeFn = mpu6000WriteRegister;
gyro->mpuConfiguration.readFn = mpu6000SpiReadRegister;
gyro->mpuConfiguration.writeFn = mpu6000SpiWriteRegister;
return true;
}
#endif
#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) {
gyro->mpuDetectionResult.sensor = mpu6500Sensor;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6500ReadRegister;
gyro->mpuConfiguration.writeFn = mpu6500WriteRegister;
gyro->mpuConfiguration.readFn = mpu6500SpiReadRegister;
gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister;
return true;
}
#endif
#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->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu9250ReadRegister;
gyro->mpuConfiguration.slowreadFn = mpu9250SlowReadRegister;
gyro->mpuConfiguration.verifywriteFn = verifympu9250WriteRegister;
gyro->mpuConfiguration.writeFn = mpu9250WriteRegister;
gyro->mpuConfiguration.resetFn = mpu9250ResetGyro;
gyro->mpuConfiguration.readFn = mpu9250SpiReadRegister;
gyro->mpuConfiguration.slowreadFn = mpu9250SpiSlowReadRegister;
gyro->mpuConfiguration.verifywriteFn = verifympu9250SpiWriteRegister;
gyro->mpuConfiguration.writeFn = mpu9250SpiWriteRegister;
gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro;
return true;
}
#endif
#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->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = icm20689ReadRegister;
gyro->mpuConfiguration.writeFn = icm20689WriteRegister;
gyro->mpuConfiguration.readFn = icm20689SpiReadRegister;
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;
}
#endif
UNUSED(gyro);
return false;
}
#endif
mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
void mpuDetect(gyroDev_t *gyro)
{
// MPU datasheet specifies 30ms.
delay(35);
#ifndef USE_I2C
uint8_t sig = 0;
bool ack = false;
#ifdef USE_I2C
bool ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I, 1, &sig);
#else
uint8_t sig;
bool ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
bool ack = false;
#endif
if (ack) {
gyro->mpuConfiguration.readFn = mpuReadRegisterI2C;
gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C;
} else {
#ifdef USE_SPI
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro);
UNUSED(detectedSpiSensor);
detectSPISensorsAndUpdateDetectionResult(gyro);
#endif
return &gyro->mpuDetectionResult;
return;
}
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
// If an MPU3050 is connected sig will contain 0.
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;
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_3050;
gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
return &gyro->mpuDetectionResult;
return;
}
sig &= MPU_INQUIRY_MASK;
if (sig == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_60x0;
mpu6050FindRevision(gyro);
} else if (sig == MPU6500_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
}
return &gyro->mpuDetectionResult;
}
void mpuGyroInit(gyroDev_t *gyro)

12
src/main/drivers/accgyro_mpu.h
View File

@ -124,8 +124,8 @@
// RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0)
typedef bool (*mpuReadRegisterFnPtr)(uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFnPtr)(uint8_t reg, uint8_t data);
typedef bool (*mpuReadRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data);
typedef void(*mpuResetFnPtr)(void);
extern mpuResetFnPtr mpuResetFn;
@ -177,7 +177,8 @@ typedef enum {
MPU_9250_SPI,
ICM_20689_SPI,
ICM_20608_SPI,
ICM_20602_SPI
ICM_20602_SPI,
BMI_160_SPI,
} mpuSensor_e;
typedef enum {
@ -190,12 +191,15 @@ typedef struct mpuDetectionResult_s {
mpu6050Resolution_e resolution;
} 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;
void mpuGyroInit(struct gyroDev_s *gyro);
struct accDev_s;
bool mpuAccRead(struct accDev_s *acc);
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);
void mpuGyroSetIsrUpdate(struct gyroDev_s *gyro, sensorGyroUpdateFuncPtr updateFn);

13
src/main/drivers/accgyro_mpu3050.c
View File

@ -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
ack = gyro->mpuConfiguration.writeFn(MPU3050_SMPLRT_DIV, 0);
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_SMPLRT_DIV, 0);
if (!ack)
failureMode(FAILURE_ACC_INIT);
gyro->mpuConfiguration.writeFn(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
gyro->mpuConfiguration.writeFn(MPU3050_INT_CFG, 0);
gyro->mpuConfiguration.writeFn(MPU3050_USER_CTRL, MPU3050_USER_RESET);
gyro->mpuConfiguration.writeFn(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_INT_CFG, 0);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
}
static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
{
UNUSED(gyro);
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;
}

16
src/main/drivers/accgyro_mpu6050.c
View File

@ -79,23 +79,23 @@ static void mpu6050GyroInit(gyroDev_t *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);
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(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_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_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
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(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_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_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff.
// 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
#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
}

22
src/main/drivers/accgyro_mpu6500.c
View File

@ -55,34 +55,34 @@ void mpu6500GyroInit(gyroDev_t *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);
gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x07);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07);
delay(100);
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
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);
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);
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);
gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
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);
// Data ready interrupt configuration
#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
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
delay(15);
#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
delay(15);
}

125
src/main/drivers/accgyro_spi_bmi160.c
View File

@ -37,24 +37,16 @@
#include "platform.h"
#include "common/axis.h"
#include "common/maths.h"
#include "system.h"
#include "io.h"
#include "exti.h"
#include "nvic.h"
#include "bus_spi.h"
#include "gyro_sync.h"
#include "sensor.h"
#include "accgyro.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
@ -90,38 +82,36 @@
#define BMI160_REG_CONF_NVM_PROG_EN 0x02
///* Global Variables */
static volatile bool BMI160InitDone = false;
static volatile bool BMI160Detected = false;
static volatile bool BMI160InitDone = false;
static volatile bool BMI160Detected = false;
static volatile bool bmi160DataReady = false;
static volatile bool bmi160ExtiInitDone = false;
//! Private functions
static int32_t BMI160_Config();
static int32_t BMI160_do_foc();
static uint8_t BMI160_ReadReg(uint8_t reg);
static int32_t BMI160_WriteReg(uint8_t reg, uint8_t data);
static int32_t BMI160_Config(const busDevice_t *bus);
static int32_t BMI160_do_foc(const busDevice_t *bus);
static uint8_t BMI160_ReadReg(const busDevice_t *bus, uint8_t reg);
static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data);
static IO_t bmi160CsPin = IO_NONE;
#define DISABLE_BMI160 IOHi(bmi160CsPin)
#define ENABLE_BMI160 IOLo(bmi160CsPin)
#define DISABLE_BMI160(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_BMI160(spiCsnPin) IOLo(spiCsnPin)
bool BMI160_Detect()
bool bmi160Detect(const busDevice_t *bus)
{
if (BMI160Detected)
return true;
bmi160CsPin = IOGetByTag(IO_TAG(BMI160_CS_PIN));
IOInit(bmi160CsPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bmi160CsPin, SPI_IO_CS_CFG);
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
spiSetDivisor(BMI160_SPI_INSTANCE, BMI160_SPI_DIVISOR);
/* 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
/* Check the chip ID */
if (BMI160_ReadReg(BMI160_REG_CHIPID) != 0xd1){
if (BMI160_ReadReg(bus, BMI160_REG_CHIPID) != 0xd1){
return false;
}
@ -134,13 +124,13 @@ bool BMI160_Detect()
* @brief Initialize the BMI160 6-axis sensor.
* @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)
return;
/* Configure the BMI160 Sensor */
if (BMI160_Config() != 0){
if (BMI160_Config(bus) != 0){
return;
}
@ -148,7 +138,7 @@ static void BMI160_Init()
/* Perform fast offset compensation if requested */
if (do_foc) {
BMI160_do_foc();
BMI160_do_foc(bus);
}
BMI160InitDone = true;
@ -158,69 +148,69 @@ static void BMI160_Init()
/**
* @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
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;
}
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;
}
delay(5); // can take up to 3.8ms
// 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){
return -3;
}
// Set odr and ranges
// 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;
}
delay(1);
// 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;
}
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;
}
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;
}
delay(1);
// Enable offset compensation
uint8_t val = BMI160_ReadReg(BMI160_REG_OFFSET_0);
if (BMI160_WriteReg(BMI160_REG_OFFSET_0, val | 0xC0) != 0){
uint8_t val = BMI160_ReadReg(bus, BMI160_REG_OFFSET_0);
if (BMI160_WriteReg(bus, BMI160_REG_OFFSET_0, val | 0xC0) != 0){
return -7;
}
// 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;
}
delay(1);
// 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;
}
delay(1);
// 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;
}
delay(1);
@ -228,22 +218,22 @@ static int32_t BMI160_Config()
return 0;
}
static int32_t BMI160_do_foc()
static int32_t BMI160_do_foc(const busDevice_t *bus)
{
// assume sensor is mounted on top
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;
}
// 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;
}
// Wait for FOC to complete
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) {
break;
}
@ -254,18 +244,18 @@ static int32_t BMI160_do_foc()
}
// Program NVM
val = BMI160_ReadReg(BMI160_REG_CONF);
if (BMI160_WriteReg(BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN) != 0) {
val = BMI160_ReadReg(bus, BMI160_REG_CONF);
if (BMI160_WriteReg(bus, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN) != 0) {
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;
}
// Wait for NVM programming to complete
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) {
break;
}
@ -283,20 +273,29 @@ static int32_t BMI160_do_foc()
* @returns The register value
* @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;
ENABLE_BMI160;
ENABLE_BMI160(bus->spi.csnPin);
spiTransferByte(BMI160_SPI_INSTANCE, 0x80 | reg); // request byte
spiTransfer(BMI160_SPI_INSTANCE, &data, NULL, 1); // receive response
DISABLE_BMI160;
DISABLE_BMI160(bus->spi.csnPin);
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
@ -304,18 +303,22 @@ static uint8_t BMI160_ReadReg(uint8_t reg)
* \param[in] data Byte to write
* @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, data);
DISABLE_BMI160;
DISABLE_BMI160(bus->spi.csnPin);
return 0;
}
bool bmi160SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
return BMI160_WriteReg(bus, reg, data);
}
extiCallbackRec_t bmi160IntCallbackRec;
@ -362,9 +365,9 @@ bool bmi160AccRead(accDev_t *acc)
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};
ENABLE_BMI160;
ENABLE_BMI160(acc->bus.spi.csnPin);
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[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_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
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[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)
{
BMI160_Init();
BMI160_Init(gyro->bus.spi.csnPin);
bmi160IntExtiInit(gyro);
}
void bmi160SpiAccInit(accDev_t *acc)
{
BMI160_Init();
BMI160_Init(acc->bus.spi.csnPin);
acc->acc_1G = 512 * 8;
}
@ -430,7 +433,7 @@ void bmi160SpiAccInit(accDev_t *acc)
bool bmi160SpiAccDetect(accDev_t *acc)
{
if (!BMI160_Detect()) {
if (!bmi160Detect(acc->bus.spi.csnPin)) {
return false;
}
@ -443,7 +446,7 @@ bool bmi160SpiAccDetect(accDev_t *acc)
bool bmi160SpiGyroDetect(gyroDev_t *gyro)
{
if (!BMI160_Detect()) {
if (!bmi160Detect(gyro->bus.spi.csnPin)) {
return false;
}

6
src/main/drivers/accgyro_spi_bmi160.h
View File

@ -34,6 +34,8 @@
#pragma once
#include "sensor.h"
enum pios_bmi160_orientation { // clockwise rotation from board forward
PIOS_BMI160_TOP_0DEG,
PIOS_BMI160_TOP_90DEG,
@ -66,5 +68,9 @@ enum bmi160_gyro_range {
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 bmi160SpiGyroDetect(gyroDev_t *gyro);

55
src/main/drivers/accgyro_spi_icm20689.c
View File

@ -36,32 +36,30 @@
#include "accgyro_mpu.h"
#include "accgyro_spi_icm20689.h"
#define DISABLE_ICM20689 IOHi(icmSpi20689CsPin)
#define ENABLE_ICM20689 IOLo(icmSpi20689CsPin)
#define DISABLE_ICM20689(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_ICM20689(spiCsnPin) IOLo(spiCsnPin)
static IO_t icmSpi20689CsPin = IO_NONE;
bool icm20689WriteRegister(uint8_t reg, uint8_t data)
bool icm20689SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
ENABLE_ICM20689;
ENABLE_ICM20689(bus->spi.csnPin);
spiTransferByte(ICM20689_SPI_INSTANCE, reg);
spiTransferByte(ICM20689_SPI_INSTANCE, data);
DISABLE_ICM20689;
DISABLE_ICM20689(bus->spi.csnPin);
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
spiTransfer(ICM20689_SPI_INSTANCE, data, NULL, length);
DISABLE_ICM20689;
DISABLE_ICM20689(bus->spi.csnPin);
return true;
}
static void icm20689SpiInit(void)
static void icm20689SpiInit(const busDevice_t *bus)
{
static bool hardwareInitialised = false;
@ -69,30 +67,29 @@ static void icm20689SpiInit(void)
return;
}
icmSpi20689CsPin = IOGetByTag(IO_TAG(ICM20689_CS_PIN));
IOInit(icmSpi20689CsPin, OWNER_MPU_CS, 0);
IOConfigGPIO(icmSpi20689CsPin, SPI_IO_CS_CFG);
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
hardwareInitialised = true;
}
bool icm20689SpiDetect(void)
bool icm20689SpiDetect(const busDevice_t *bus)
{
uint8_t tmp;
uint8_t attemptsRemaining = 20;
icm20689SpiInit();
icm20689SpiInit(bus);
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 {
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) {
break;
}
@ -130,32 +127,32 @@ void icm20689GyroInit(gyroDev_t *gyro)
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);
gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x03);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
delay(100);
// gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0);
// gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
// 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);
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);
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);
gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
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);
// 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(MPU_RA_INT_PIN_CFG, 0x10); // 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(&gyro->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
delay(15);
#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
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);

8
src/main/drivers/accgyro_spi_icm20689.h
View File

@ -16,6 +16,8 @@
*/
#pragma once
#include "sensor.h"
#define ICM20689_WHO_AM_I_CONST (0x98)
#define ICM20689_BIT_RESET (0x80)
@ -25,10 +27,10 @@ bool icm20689GyroDetect(gyroDev_t *gyro);
void icm20689AccInit(accDev_t *acc);
void icm20689GyroInit(gyroDev_t *gyro);
bool icm20689SpiDetect(void);
bool icm20689SpiDetect(const busDevice_t *bus);
bool icm20689SpiAccDetect(accDev_t *acc);
bool icm20689SpiGyroDetect(gyroDev_t *gyro);
bool icm20689WriteRegister(uint8_t reg, uint8_t data);
bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
bool icm20689SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool icm20689SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

55
src/main/drivers/accgyro_spi_mpu6000.c
View File

@ -99,27 +99,25 @@ static bool mpuSpi6000InitDone = false;
#define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A
#define DISABLE_MPU6000 IOHi(mpuSpi6000CsPin)
#define ENABLE_MPU6000 IOLo(mpuSpi6000CsPin)
#define DISABLE_MPU6000(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_MPU6000(spiCsnPin) IOLo(spiCsnPin)
static IO_t mpuSpi6000CsPin = IO_NONE;
bool mpu6000WriteRegister(uint8_t reg, uint8_t data)
bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
ENABLE_MPU6000;
ENABLE_MPU6000(bus->spi.csnPin);
spiTransferByte(MPU6000_SPI_INSTANCE, reg);
spiTransferByte(MPU6000_SPI_INSTANCE, data);
DISABLE_MPU6000;
DISABLE_MPU6000(bus->spi.csnPin);
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
spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU6000;
DISABLE_MPU6000(bus->spi.csnPin);
return true;
}
@ -133,7 +131,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro)
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, gyro->lpf);
mpu6000SpiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf);
delayMicroseconds(1);
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;
}
bool mpu6000SpiDetect(void)
bool mpu6000SpiDetect(const busDevice_t *bus)
{
uint8_t in;
uint8_t attemptsRemaining = 5;
#ifdef MPU6000_CS_PIN
mpuSpi6000CsPin = IOGetByTag(IO_TAG(MPU6000_CS_PIN));
#endif
IOInit(mpuSpi6000CsPin, OWNER_MPU_CS, 0);
IOConfigGPIO(mpuSpi6000CsPin, SPI_IO_CS_CFG);
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
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 {
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) {
break;
}
@ -177,7 +172,7 @@ bool mpu6000SpiDetect(void)
}
} while (attemptsRemaining--);
mpu6000ReadRegister(MPU_RA_PRODUCT_ID, 1, &in);
mpu6000SpiReadRegister(bus, MPU_RA_PRODUCT_ID, 1, &in);
/* look for a product ID we recognise */
@ -210,41 +205,41 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
// Device Reset
mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET);
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
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);
// 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);
// 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);
mpu6000WriteRegister(MPU_RA_PWR_MGMT_2, 0x00);
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00);
delayMicroseconds(15);
// Accel Sample Rate 1kHz
// 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);
// 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);
// 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);
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);
#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);
#endif

8
src/main/drivers/accgyro_spi_mpu6000.h
View File

@ -1,6 +1,8 @@
#pragma once
#include "sensor.h"
#define MPU6000_CONFIG 0x1A
#define BITS_DLPF_CFG_256HZ 0x00
@ -15,10 +17,10 @@
// RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0)
bool mpu6000SpiDetect(void);
bool mpu6000SpiDetect(const busDevice_t *bus);
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);
bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

56
src/main/drivers/accgyro_spi_mpu6500.c
View File

@ -34,32 +34,32 @@
#include "accgyro_mpu6500.h"
#include "accgyro_spi_mpu6500.h"
#define DISABLE_MPU6500 IOHi(mpuSpi6500CsPin)
#define ENABLE_MPU6500 IOLo(mpuSpi6500CsPin)
#define DISABLE_MPU6500(spiCsnPin) IOHi(spiCsnPin)
#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, data);
DISABLE_MPU6500;
DISABLE_MPU6500(bus->spi.csnPin);
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
spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU6500;
DISABLE_MPU6500(bus->spi.csnPin);
return true;
}
static void mpu6500SpiInit(void)
static void mpu6500SpiInit(const busDevice_t *bus)
{
static bool hardwareInitialised = false;
@ -67,24 +67,22 @@ static void mpu6500SpiInit(void)
return;
}
mpuSpi6500CsPin = IOGetByTag(IO_TAG(MPU6500_CS_PIN));
IOInit(mpuSpi6500CsPin, OWNER_MPU_CS, 0);
IOConfigGPIO(mpuSpi6500CsPin, SPI_IO_CS_CFG);
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
hardwareInitialised = true;
}
static uint8_t mpuDetected = MPU_NONE;
uint8_t mpu6500SpiDetect(void)
uint8_t mpu6500SpiDetect(const busDevice_t *bus)
{
mpu6500SpiInit(bus);
uint8_t tmp;
mpu6500SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
mpu6500SpiInit();
mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp);
uint8_t mpuDetected = MPU_NONE;
switch (tmp) {
case MPU6500_WHO_AM_I_CONST:
mpuDetected = MPU_65xx_SPI;
@ -118,7 +116,7 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro)
mpu6500GyroInit(gyro);
// 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);
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
@ -127,7 +125,14 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro)
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;
}
@ -139,7 +144,14 @@ bool mpu6500SpiAccDetect(accDev_t *acc)
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;
}

12
src/main/drivers/accgyro_spi_mpu6500.h
View File

@ -17,13 +17,15 @@
#pragma once
uint8_t mpu6500SpiDetect(void);
#include "sensor.h"
void mpu6500SpiAccInit(accDev_t *acc);
void mpu6500SpiGyroInit(gyroDev_t *gyro);
uint8_t mpu6500SpiDetect(const busDevice_t *bus);
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);
bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, 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);

73
src/main/drivers/accgyro_spi_mpu9250.c
View File

@ -50,47 +50,48 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro);
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)
#define ENABLE_MPU9250 IOLo(mpuSpi9250CsPin)
void mpu9250ResetGyro(void)
void mpu9250SpiResetGyro(void)
{
// 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);
#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);
spiTransferByte(MPU9250_SPI_INSTANCE, reg);
spiTransferByte(MPU9250_SPI_INSTANCE, data);
DISABLE_MPU9250;
DISABLE_MPU9250(bus->spi.csnPin);
delayMicroseconds(1);
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
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU9250;
DISABLE_MPU9250(bus->spi.csnPin);
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);
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU9250;
DISABLE_MPU9250(bus->spi.csnPin);
delayMicroseconds(1);
return true;
@ -119,21 +120,21 @@ void mpu9250SpiAccInit(accDev_t *acc)
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 attemptsRemaining = 20;
mpu9250WriteRegister(reg, data);
mpu9250SpiWriteRegister(bus, reg, data);
delayMicroseconds(100);
do {
mpu9250SlowReadRegister(reg, 1, &in);
mpu9250SpiSlowReadRegister(bus, reg, 1, &in);
if (in == data) {
return true;
} else {
debug[3]++;
mpu9250WriteRegister(reg, data);
mpu9250SpiWriteRegister(bus, reg, data);
delayMicroseconds(100);
}
} 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
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
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
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) {
verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF
} 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) {
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);
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_ACCEL_CONFIG, INV_FSR_8G << 3);
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)
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
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
@ -179,25 +180,21 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
mpuSpi9250InitDone = true; //init done
}
bool mpu9250SpiDetect(void)
bool mpu9250SpiDetect(const busDevice_t *bus)
{
uint8_t in;
uint8_t attemptsRemaining = 20;
/* not the best place for this - should really have an init method */
#ifdef MPU9250_CS_PIN
mpuSpi9250CsPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN));
#endif
IOInit(mpuSpi9250CsPin, OWNER_MPU_CS, 0);
IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG);
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
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 {
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) {
break;
}

14
src/main/drivers/accgyro_spi_mpu9250.h
View File

@ -1,6 +1,8 @@
#pragma once
#include "sensor.h"
#define mpu9250_CONFIG 0x1A
/* We should probably use these. :)
@ -24,14 +26,14 @@
// RF = Register Flag
#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 mpu9250SpiGyroDetect(gyroDev_t *gyro);
bool mpu9250WriteRegister(uint8_t reg, uint8_t data);
bool verifympu9250WriteRegister(uint8_t reg, uint8_t data);
bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

1
src/main/drivers/compass.h
View File

@ -22,6 +22,7 @@
typedef struct magDev_s {
sensorInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
busDevice_t bus;
sensor_align_e magAlign;
} magDev_t;

36
src/main/drivers/compass_ak8963.c
View File

@ -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.
#if defined(MPU6500_SPI_INSTANCE) && !defined(MPU9250_SPI_INSTANCE)
#define MPU9250_SPI_INSTANCE
#define verifympu9250WriteRegister mpu6500WriteRegister
#define mpu9250WriteRegister mpu6500WriteRegister
#define mpu9250ReadRegister mpu6500ReadRegister
#define verifympu9250SpiWriteRegister mpu6500SpiWriteRegister
#define mpu9250SpiWriteRegister mpu6500SpiWriteRegister
#define mpu9250SpiReadRegister mpu6500SpiReadRegister
#endif
#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)
{
verifympu9250WriteRegister(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
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
delay(10);
__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();
return true;
}
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
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
verifympu9250WriteRegister(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_ADDR, addr_); // set I2C slave address for write
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte
return true;
}
@ -136,9 +136,9 @@ static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_)
queuedRead.len = len_;
verifympu9250WriteRegister(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
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
queuedRead.readStartedAt = micros();
queuedRead.waiting = true;
@ -173,7 +173,7 @@ static bool ak8963SensorCompleteRead(uint8_t *buf)
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;
}
#else
@ -316,13 +316,13 @@ bool ak8963Detect(magDev_t *mag)
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
// 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);
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);
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);
#endif

8
src/main/drivers/sensor.h
View File

@ -20,6 +20,8 @@
#include <stdbool.h>
#include <stdint.h>
#include "io_types.h"
typedef enum {
ALIGN_DEFAULT = 0, // driver-provided alignment
CW0_DEG = 1,
@ -32,6 +34,12 @@ typedef enum {
CW270_DEG_FLIP = 8
} 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 (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef bool (*sensorInterruptFuncPtr)(void);

3
src/main/fc/cli.c
View File

@ -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) },
#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
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) },

1
src/main/sensors/acceleration.c
View File

@ -300,6 +300,7 @@ bool accInit(uint32_t gyroSamplingInverval)
{
memset(&acc, 0, sizeof(acc));
// copy over the common gyro mpu settings
acc.dev.bus = *gyroSensorBus();
acc.dev.mpuConfiguration = *gyroMpuConfiguration();
acc.dev.mpuDetectionResult = *gyroMpuDetectionResult();
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {

6
src/main/sensors/compass.c
View File

@ -36,9 +36,10 @@
#include "fc/config.h"
#include "fc/runtime_config.h"
#include "sensors/sensors.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
#include "hardware_revision.h"
@ -141,9 +142,12 @@ bool compassInit(void)
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
// 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.
// copy over SPI bus settings for AK8963 compass
magDev.bus = *gyroSensorBus();
if (!compassDetect(&magDev, compassConfig()->mag_hardware)) {
return false;
}
const int16_t deg = 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

26
src/main/sensors/gyro.c
View File

@ -96,16 +96,19 @@ static void *notchFilter2[3];
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0);
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_lpf = GYRO_LPF_256HZ,
.gyro_soft_lpf_type = FILTER_PT1,
.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_cutoff_1 = 300,
.gyro_soft_notch_hz_2 = 200,
.gyro_soft_notch_cutoff_2 = 100,
.gyro_align = ALIGN_DEFAULT,
.gyroMovementCalibrationThreshold = 48
.gyro_soft_notch_cutoff_2 = 100
);
#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
const busDevice_t *gyroSensorBus(void)
{
return &gyroDev0.bus;
}
const mpuConfiguration_t *gyroMpuConfiguration(void)
{
return &gyroDev0.mpuConfiguration;
@ -283,8 +291,14 @@ bool gyroInit(void)
memset(&gyro, 0, sizeof(gyro));
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
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);
mpuResetFn = gyroDev0.mpuConfiguration.resetFn;
mpuResetFn = gyroDev0.mpuConfiguration.resetFn; // must be set after mpuDetect
#endif
const gyroSensor_e gyroHardware = gyroDetect(&gyroDev0);
if (gyroHardware == GYRO_NONE) {
@ -305,7 +319,7 @@ bool gyroInit(void)
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);
gyroDev0.lpf = gyroConfig()->gyro_lpf;
gyroDev0.init(&gyroDev0);

3
src/main/sensors/gyro.h
View File

@ -19,6 +19,7 @@
#include "config/parameter_group.h"
#include "common/axis.h"
#include "drivers/io_types.h"
#include "drivers/sensor.h"
typedef enum {
@ -54,6 +55,7 @@ typedef struct gyroConfig_s {
uint8_t gyro_soft_lpf_hz;
bool gyro_isr_update;
bool gyro_use_32khz;
uint8_t gyro_to_use;
uint16_t gyro_soft_notch_hz_1;
uint16_t gyro_soft_notch_cutoff_1;
uint16_t gyro_soft_notch_hz_2;
@ -65,6 +67,7 @@ PG_DECLARE(gyroConfig_t, gyroConfig);
bool gyroInit(void);
void gyroInitFilters(void);
void gyroUpdate(void);
const busDevice_t *gyroSensorBus(void);
struct mpuConfiguration_s;
const struct mpuConfiguration_s *gyroMpuConfiguration(void);
struct mpuDetectionResult_s;