Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00
Code Issues Wiki Activity

Added runtime setting of gyro SPI pin

Browse source
This commit is contained in:
Martin Budden 2017-03-13 16:27:58 +00:00
parent a4e345ca54
commit b3ee1409e8
20 changed files with 205 additions and 162 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -48,6 +48,7 @@ typedef struct gyroDev_s {
sensorGyroInterruptStatusFuncPtr intStatus;
sensorGyroUpdateFuncPtr update;
extiCallbackRec_t exti;
busDevice_t bus;
float scale; // scalefactor
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int16_t gyroZero[XYZ_AXIS_COUNT];
@ -64,6 +65,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

50
src/main/drivers/accgyro_mpu.c
View file

@ -75,7 +75,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 +89,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);
@ -158,14 +158,16 @@ static void mpuIntExtiInit(gyroDev_t *gyro)
#endif
}
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;
}
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;
}
@ -174,7 +176,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;
}
@ -197,7 +199,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;
}
@ -224,8 +226,12 @@ 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 = mpu6000SpiReadRegister;
@ -235,8 +241,11 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif
#ifdef USE_GYRO_SPI_MPU6500
if (mpu6500SpiDetect()) {
gyro->mpuDetectionResult.sensor = MPU_65xx_SPI;
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 = mpu6500SpiReadRegister;
gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister;
@ -245,7 +254,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#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 = mpu9250SpiReadRegister;
@ -258,7 +268,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif
#ifdef USE_GYRO_SPI_ICM20608
if (icm20608SpiDetect()) {
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20608_CS_PIN)) : gyro->bus.spi.csnPin;
if (icm20608SpiDetect(&gyro->bus)) {
mpuDetectionResult.sensor = ICM_20608_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.readFn = icm20608SpiReadRegister;
@ -268,7 +279,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#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 = icm20689SpiReadRegister;
@ -287,20 +299,18 @@ 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;
}
@ -309,7 +319,7 @@ void mpuDetect(gyroDev_t *gyro)
// 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;
@ -318,14 +328,12 @@ void mpuDetect(gyroDev_t *gyro)
}
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;
}
void mpuGyroInit(gyroDev_t *gyro)

14
src/main/drivers/accgyro_mpu.h
View file

@ -20,6 +20,12 @@
#include "exti.h"
#include "sensor.h"
//#define DEBUG_MPU_DATA_READY_INTERRUPT
#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
#define GYRO_USES_SPI
#endif
// MPU6050
#define MPU_RA_WHO_AM_I 0x75
#define MPU_RA_WHO_AM_I_LEGACY 0x00
@ -118,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;
@ -187,8 +193,8 @@ typedef struct mpuDetectionResult_s {
mpu6050Resolution_e resolution;
} mpuDetectionResult_t;
bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data);
bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
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);

12
src/main/drivers/accgyro_mpu3050.c
View file

@ -53,20 +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)
{
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);
bool ack = gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
bool ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
ack = 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)
ack = gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
ack = 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)
ack = 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
ack = 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)
ack = gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
ack = 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)
ack = 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)
ack = gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
ack = gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG,
ack = 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
ack = gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif
UNUSED(ack);
}

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

@ -69,34 +69,34 @@ void mpu6500GyroInit(gyroDev_t *gyro)
}
#endif
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_8G << 3);
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 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
gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider
delay(100);
// Data ready interrupt configuration
#ifdef USE_MPU9250_MAG
gyro->mpuConfiguration.writeFn(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
gyro->mpuConfiguration.writeFn(&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
#else
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
#endif
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
delay(15);
}

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

@ -100,29 +100,27 @@ 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 mpu6000SpiWriteRegister(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);
delayMicroseconds(1);
spiTransferByte(MPU6000_SPI_INSTANCE, reg);
spiTransferByte(MPU6000_SPI_INSTANCE, data);
DISABLE_MPU6000;
DISABLE_MPU6000(bus->spi.csnPin);
delayMicroseconds(1);
return true;
}
bool mpu6000SpiReadRegister(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;
}
@ -136,7 +134,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro)
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
// Accel and Gyro DLPF Setting
mpu6000SpiWriteRegister(MPU6000_CONFIG, gyro->lpf);
mpu6000SpiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf);
delayMicroseconds(1);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock
@ -153,25 +151,22 @@ void mpu6000SpiAccInit(accDev_t *acc)
acc->acc_1G = 512 * 8;
}
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, RESOURCE_SPI_CS, 0);
IOConfigGPIO(mpuSpi6000CsPin, SPI_IO_CS_CFG);
IOInit(bus->spi.csnPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
mpu6000SpiWriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET);
mpu6000SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
do {
delay(150);
mpu6000SpiReadRegister(MPU_RA_WHO_AM_I, 1, &in);
mpu6000SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
if (in == MPU6000_WHO_AM_I_CONST) {
break;
}
@ -180,7 +175,7 @@ bool mpu6000SpiDetect(void)
}
} while (attemptsRemaining--);
mpu6000SpiReadRegister(MPU_RA_PRODUCT_ID, 1, &in);
mpu6000SpiReadRegister(bus, MPU_RA_PRODUCT_ID, 1, &in);
/* look for a product ID we recognise */
@ -213,41 +208,41 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
// Device Reset
mpu6000SpiWriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET);
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
delay(150);
mpu6000SpiWriteRegister(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
mpu6000SpiWriteRegister(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
mpu6000SpiWriteRegister(MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
delayMicroseconds(15);
mpu6000SpiWriteRegister(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
mpu6000SpiWriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
delayMicroseconds(15);
// Gyro +/- 1000 DPS Full Scale
mpu6000SpiWriteRegister(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
mpu6000SpiWriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
delayMicroseconds(15);
mpu6000SpiWriteRegister(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
mpu6000SpiWriteRegister(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 mpu6000SpiWriteRegister(uint8_t reg, uint8_t data);
bool mpu6000SpiReadRegister(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);

39
src/main/drivers/accgyro_spi_mpu6500.c
View file

@ -20,6 +20,8 @@
#include "platform.h"
#if defined(USE_ACC_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
#include "common/axis.h"
#include "common/maths.h"
@ -35,35 +37,34 @@
#include "accgyro_mpu6500.h"
#include "accgyro_spi_mpu6500.h"
#if defined(USE_ACC_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
#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 mpu6500SpiWriteRegister(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);
delayMicroseconds(1);
spiTransferByte(MPU6500_SPI_INSTANCE, reg);
spiTransferByte(MPU6500_SPI_INSTANCE, data);
DISABLE_MPU6500;
DISABLE_MPU6500(bus->spi.csnPin);
delayMicroseconds(1);
return true;
}
bool mpu6500SpiReadRegister(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;
@ -71,22 +72,24 @@ static void mpu6500SpiInit(void)
return;
}
mpuSpi6500CsPin = IOGetByTag(IO_TAG(MPU6500_CS_PIN));
IOInit(mpuSpi6500CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
IOConfigGPIO(mpuSpi6500CsPin, SPI_IO_CS_CFG);
IOInit(bus->spi.csnPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
hardwareInitialised = true;
}
bool mpu6500SpiDetect(void)
uint8_t mpu6500SpiDetect(const busDevice_t *bus)
{
mpu6500SpiInit(bus);
uint8_t tmp;
mpu6500SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
mpu6500SpiInit();
mpu6500SpiInit(bus);
mpu6500SpiReadRegister(MPU_RA_WHO_AM_I, 1, &tmp);
mpu6500SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
if (tmp == MPU6500_WHO_AM_I_CONST ||
tmp == MPU9250_WHO_AM_I_CONST ||
@ -123,7 +126,7 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro)
mpu6500SpiGyroInit(gyro);
// Disable Primary I2C Interface
mpu6500SpiWriteRegister(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);

8
src/main/drivers/accgyro_spi_mpu6500.h
View file

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

70
src/main/drivers/accgyro_spi_mpu9250.c
View file

@ -49,59 +49,57 @@
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)
bool mpu9250SpiReadRegister(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 mpu9250SpiSlowReadRegister(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;
}
bool mpu9250SpiWriteRegister(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 verifympu9250SpiWriteRegister(uint8_t reg, uint8_t data)
bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
mpu9250SpiWriteRegister(reg, data);
mpu9250SpiWriteRegister(bus, reg, data);
delayMicroseconds(100);
uint8_t attemptsRemaining = 20;
do {
uint8_t in;
mpu9250SpiSlowReadRegister(reg, 1, &in);
mpu9250SpiSlowReadRegister(bus, reg, 1, &in);
if (in == data) {
return true;
} else {
debug[3]++;
mpu9250SpiWriteRegister(reg, data);
mpu9250SpiWriteRegister(bus, reg, data);
delayMicroseconds(100);
}
} while (attemptsRemaining--);
@ -111,8 +109,11 @@ bool verifympu9250SpiWriteRegister(uint8_t reg, uint8_t data)
void mpu9250SpiResetGyro(void)
{
// Device Reset
mpu9250SpiWriteRegister(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
}
static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
@ -123,30 +124,30 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
mpu9250SpiWriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
delay(50);
verifympu9250SpiWriteRegister(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);
verifympu9250SpiWriteRegister(MPU_RA_GYRO_CONFIG, raGyroConfigData);
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
if (gyro->lpf == 4) {
verifympu9250SpiWriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF
} else if (gyro->lpf < 4) {
verifympu9250SpiWriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
} else if (gyro->lpf > 4) {
verifympu9250SpiWriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF
}
verifympu9250SpiWriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
verifympu9250SpiWriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
verifympu9250SpiWriteRegister(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)
verifympu9250SpiWriteRegister(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);
@ -154,23 +155,20 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
mpuSpi9250InitDone = true; //init done
}
bool mpu9250SpiDetect(void)
bool mpu9250SpiDetect(const busDevice_t *bus)
{
/* not the best place for this - should really have an init method */
#ifdef MPU9250_CS_PIN
mpuSpi9250CsPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN));
#endif
IOInit(mpuSpi9250CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG);
IOInit(bus->spi.csnPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
mpu9250SpiWriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
uint8_t attemptsRemaining = 20;
do {
delay(150);
uint8_t in;
mpu9250SpiReadRegister(MPU_RA_WHO_AM_I, 1, &in);
mpu9250SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
if (in == MPU9250_WHO_AM_I_CONST) {
break;
}

13
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. :)
@ -17,6 +19,7 @@
#define GYRO_SCALE_FACTOR 0.00053292f // (4/131) * pi/180 (32.75 LSB = 1 DPS)
#define MPU9250_WHO_AM_I_CONST (0x71)
#define MPU9255_WHO_AM_I_CONST (0x73)
#define MPU9250_BIT_RESET (0x80)
@ -25,12 +28,12 @@
void mpu9250SpiResetGyro(void);
bool mpu9250SpiDetect(void);
bool mpu9250SpiDetect(const busDevice_t *bus);
bool mpu9250SpiAccDetect(accDev_t *acc);
bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
bool mpu9250SpiWriteRegister(uint8_t reg, uint8_t data);
bool verifympu9250SpiWriteRegister(uint8_t reg, uint8_t data);
bool mpu9250SpiReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
bool mpu9250SpiSlowReadRegister(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;

30
src/main/drivers/compass_ak8963.c
View file

@ -101,22 +101,22 @@ static queuedReadState_t queuedRead = { false, 0, 0};
static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf)
{
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpuWriteRegisterI2C(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();
mpuReadRegisterI2C(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)
{
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_DO, data); // set I2C salve value
mpuWriteRegisterI2C(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;
}
@ -128,9 +128,9 @@ static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_)
queuedRead.len = len_;
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpuWriteRegisterI2C(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;
@ -165,7 +165,7 @@ static bool ak8963SensorCompleteRead(uint8_t *buf)
queuedRead.waiting = false;
mpuReadRegisterI2C(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
@ -327,13 +327,13 @@ bool ak8963Detect(magDev_t *mag)
#if defined(USE_SPI) && defined(MPU6500_SPI_INSTANCE)
// initialze I2C master via SPI bus (MPU9250)
ack = mpuWriteRegisterI2C(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR
ack = mpuWriteRegisterI2C(&mag->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR
delay(10);
ack = mpuWriteRegisterI2C(MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
ack = mpuWriteRegisterI2C(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
delay(10);
ack = mpuWriteRegisterI2C(MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
ack = mpuWriteRegisterI2C(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
delay(10);
#endif

11
src/main/drivers/sensor.h
View file

@ -17,6 +17,11 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include "io_types.h"
typedef enum {
ALIGN_DEFAULT = 0, // driver-provided alignment
CW0_DEG = 1,
@ -29,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);

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

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

5
src/main/sensors/compass.c
View file

@ -46,8 +46,9 @@
#include "io/gps.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#ifdef NAZE
#include "hardware_revision.h"
@ -233,6 +234,8 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
bool compassInit(void)
{
// copy over SPI bus settings for AK8963 compass
mag.dev.bus = *gyroSensorBus();
if (!compassDetect(&mag.dev, compassConfig()->mag_hardware)) {
return false;
}

5
src/main/sensors/gyro.c
View file

@ -128,6 +128,11 @@ static const extiConfig_t *selectMPUIntExtiConfig(void)
#endif
}
const busDevice_t *gyroSensorBus(void)
{
return &gyro.dev.bus;
}
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
{
dev->gyroAlign = ALIGN_DEFAULT;

2
src/main/sensors/gyro.h
View file

@ -20,6 +20,7 @@
#include "drivers/accgyro.h"
#include "common/axis.h"
#include "config/parameter_group.h"
#include "drivers/sensor.h"
typedef enum {
GYRO_NONE = 0,
@ -61,5 +62,6 @@ PG_DECLARE(gyroConfig_t, gyroConfig);
bool gyroInit(void);
void gyroInitFilters(void);
void gyroUpdate(void);
const busDevice_t *gyroSensorBus(void);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
bool gyroIsCalibrationComplete(void);

1
src/main/target/PIXRACER/target.h
View file

@ -46,6 +46,7 @@
#ifdef PIXRACER_ICM20608
// Variant that uses ICM20608 gyro/acc
#define ICM20608_CS_PIN PC15
#define MPU6500_CS_PIN PC15
#define MPU6500_SPI_INSTANCE SPI1