Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 00:35:39 +03:00
Code Issues Wiki Activity

Function renaming as per ledvinap's suggestion

Browse source
This commit is contained in:
Martin Budden 2017-06-29 09:25:59 +01:00
parent bdd876b6aa
commit 5d7f8cdd6e
10 changed files with 116 additions and 103 deletions
Download patch file Download diff file Expand all files Collapse all files

16
src/main/drivers/accgyro/accgyro_mpu.c
View file

@ -241,8 +241,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
if (mpu6000SpiDetect(&gyro->bus)) {
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = spiReadRegister;
gyro->mpuConfiguration.writeFn = spiWriteRegister;
gyro->mpuConfiguration.readFn = spiReadRegBuf;
gyro->mpuConfiguration.writeFn = spiWriteReg;
return true;
}
#endif
@ -255,8 +255,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
if (mpu6500Sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = mpu6500Sensor;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = spiReadRegister;
gyro->mpuConfiguration.writeFn = spiWriteRegister;
gyro->mpuConfiguration.readFn = spiReadRegBuf;
gyro->mpuConfiguration.writeFn = spiWriteReg;
return true;
}
#endif
@ -267,8 +267,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
if (mpu9250SpiDetect(&gyro->bus)) {
gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = spiReadRegister;
gyro->mpuConfiguration.writeFn = mpu9250SpiWriteRegister;
gyro->mpuConfiguration.readFn = spiReadRegBuf;
gyro->mpuConfiguration.writeFn = mpu9250SpiWriteReg;
gyro->mpuConfiguration.resetFn = mpu9250SpiResetGyro;
return true;
}
@ -280,8 +280,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
if (icm20689SpiDetect(&gyro->bus)) {
gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = spiReadRegister;
gyro->mpuConfiguration.writeFn = spiWriteRegister;
gyro->mpuConfiguration.readFn = spiReadRegBuf;
gyro->mpuConfiguration.writeFn = spiWriteReg;
return true;
}
#endif

11
src/main/drivers/accgyro/accgyro_spi_icm20689.c
View file

@ -55,20 +55,17 @@ static void icm20689SpiInit(const busDevice_t *bus)
bool icm20689SpiDetect(const busDevice_t *bus)
{
uint8_t tmp;
uint8_t attemptsRemaining = 20;
icm20689SpiInit(bus);
spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); //low speed
spiWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
spiWriteReg(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
uint8_t attemptsRemaining = 20;
do {
delay(150);
spiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
if (tmp == ICM20689_WHO_AM_I_CONST) {
const uint8_t whoAmI = spiReadReg(bus, MPU_RA_WHO_AM_I);
if (whoAmI == ICM20689_WHO_AM_I_CONST) {
break;
}
if (!attemptsRemaining) {

62
src/main/drivers/accgyro/accgyro_spi_mpu6000.c
View file

@ -108,7 +108,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro)
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON);
// Accel and Gyro DLPF Setting
spiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf);
spiWriteReg(&gyro->bus, MPU6000_CONFIG, gyro->lpf);
delayMicroseconds(1);
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST); // 18 MHz SPI clock
@ -127,22 +127,20 @@ void mpu6000SpiAccInit(accDev_t *acc)
bool mpu6000SpiDetect(const busDevice_t *bus)
{
uint8_t in;
uint8_t attemptsRemaining = 5;
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
IOHi(bus->spi.csnPin);
spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON);
spiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
spiWriteReg(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
uint8_t attemptsRemaining = 5;
do {
delay(150);
spiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
if (in == MPU6000_WHO_AM_I_CONST) {
const uint8_t whoAmI = spiReadReg(bus, MPU_RA_WHO_AM_I);
if (whoAmI == MPU6000_WHO_AM_I_CONST) {
break;
}
if (!attemptsRemaining) {
@ -150,25 +148,25 @@ bool mpu6000SpiDetect(const busDevice_t *bus)
}
} while (attemptsRemaining--);
spiReadRegister(bus, MPU_RA_PRODUCT_ID, 1, &in);
const uint8_t productID = spiReadReg(bus, MPU_RA_PRODUCT_ID);
/* look for a product ID we recognise */
// verify product revision
switch (in) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
case MPU6000ES_REV_D6:
case MPU6000ES_REV_D7:
case MPU6000ES_REV_D8:
case MPU6000_REV_D6:
case MPU6000_REV_D7:
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
return true;
switch (productID) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
case MPU6000ES_REV_D6:
case MPU6000ES_REV_D7:
case MPU6000ES_REV_D8:
case MPU6000_REV_D6:
case MPU6000_REV_D7:
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
return true;
}
return false;
@ -183,41 +181,41 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON);
// Device Reset
spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
spiWriteReg(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
delay(150);
spiWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
spiWriteReg(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
delay(150);
// Clock Source PPL with Z axis gyro reference
spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
spiWriteReg(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
delayMicroseconds(15);
// Disable Primary I2C Interface
spiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
spiWriteReg(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
delayMicroseconds(15);
spiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00);
spiWriteReg(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00);
delayMicroseconds(15);
// Accel Sample Rate 1kHz
// Gyroscope Output Rate = 1kHz when the DLPF is enabled
spiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
spiWriteReg(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
delayMicroseconds(15);
// Gyro +/- 1000 DPS Full Scale
spiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
spiWriteReg(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delayMicroseconds(15);
// Accel +/- 8 G Full Scale
spiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
spiWriteReg(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
delayMicroseconds(15);
spiWriteRegister(&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
spiWriteReg(&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
spiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
spiWriteReg(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
delayMicroseconds(15);
#endif

7
src/main/drivers/accgyro/accgyro_spi_mpu6500.c
View file

@ -57,11 +57,10 @@ uint8_t mpu6500SpiDetect(const busDevice_t *bus)
{
mpu6500SpiInit(bus);
uint8_t tmp;
spiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
const uint8_t whoAmI = spiReadReg(bus, MPU_RA_WHO_AM_I);
uint8_t mpuDetected = MPU_NONE;
switch (tmp) {
switch (whoAmI) {
case MPU6500_WHO_AM_I_CONST:
mpuDetected = MPU_65xx_SPI;
break;
@ -97,7 +96,7 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro)
mpu6500GyroInit(gyro);
// Disable Primary I2C Interface
spiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
spiWriteReg(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
delay(100);
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST);

48
src/main/drivers/accgyro/accgyro_spi_mpu9250.c
View file

@ -50,7 +50,7 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro);
static bool mpuSpi9250InitDone = false;
bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
bool mpu9250SpiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
IOLo(bus->spi.csnPin);
delayMicroseconds(1);
@ -62,7 +62,7 @@ bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
return true;
}
static bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
static bool mpu9250SpiSlowReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
IOLo(bus->spi.csnPin);
delayMicroseconds(1);
@ -79,7 +79,7 @@ void mpu9250SpiResetGyro(void)
// Device 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);
mpu9250SpiWriteReg(&bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
delay(150);
#endif
}
@ -107,21 +107,20 @@ void mpu9250SpiAccInit(accDev_t *acc)
acc->acc_1G = 512 * 8;
}
bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
bool mpu9250SpiWriteRegVerify(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
uint8_t in;
uint8_t attemptsRemaining = 20;
mpu9250SpiWriteRegister(bus, reg, data);
mpu9250SpiWriteReg(bus, reg, data);
delayMicroseconds(100);
uint8_t attemptsRemaining = 20;
do {
mpu9250SpiSlowReadRegister(bus, reg, 1, &in);
uint8_t in;
mpu9250SpiSlowReadRegBuf(bus, reg, 1, &in);
if (in == data) {
return true;
} else {
debug[3]++;
mpu9250SpiWriteRegister(bus, reg, data);
mpu9250SpiWriteReg(bus, reg, data);
delayMicroseconds(100);
}
} while (attemptsRemaining--);
@ -136,30 +135,30 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
mpu9250SpiWriteReg(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
delay(50);
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
mpu9250SpiWriteRegVerify(&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(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
if (gyro->lpf == 4) {
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF
mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF
} else if (gyro->lpf < 4) {
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
} else if (gyro->lpf > 4) {
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF
mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF
}
verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
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
mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
mpu9250SpiWriteRegVerify(&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(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
mpu9250SpiWriteRegVerify(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
#endif
spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_FAST);
@ -169,19 +168,16 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
bool mpu9250SpiDetect(const busDevice_t *bus)
{
uint8_t in;
uint8_t attemptsRemaining = 20;
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); //low speed
mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
mpu9250SpiWriteReg(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
uint8_t attemptsRemaining = 20;
do {
delay(150);
spiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
const uint8_t in = spiReadReg(bus, MPU_RA_WHO_AM_I);
if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) {
break;
}

6
src/main/drivers/accgyro/accgyro_spi_mpu9250.h
View file

@ -33,6 +33,6 @@ bool mpu9250SpiDetect(const busDevice_t *bus);
bool mpu9250SpiAccDetect(accDev_t *acc);
bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
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 mpu9250SpiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu9250SpiWriteRegVerify(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu9250SpiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

15
src/main/drivers/bus_spi.c
View file

@ -353,7 +353,7 @@ void spiResetErrorCounter(SPI_TypeDef *instance)
spiHardwareMap[device].errorCount = 0;
}
bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
bool spiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
IOLo(bus->spi.csnPin);
spiTransferByte(bus->spi.instance, reg);
@ -363,7 +363,7 @@ bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
return true;
}
bool spiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
bool spiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
IOLo(bus->spi.csnPin);
spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction
@ -372,3 +372,14 @@ bool spiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_
return true;
}
uint8_t spiReadReg(const busDevice_t *bus, uint8_t reg)
{
uint8_t data;
IOLo(bus->spi.csnPin);
spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction
spiTransfer(bus->spi.instance, &data, NULL, 1);
IOHi(bus->spi.csnPin);
return data;
}

5
src/main/drivers/bus_spi.h
View file

@ -103,5 +103,6 @@ SPI_HandleTypeDef* spiHandleByInstance(SPI_TypeDef *instance);
DMA_HandleTypeDef* spiSetDMATransmit(DMA_Stream_TypeDef *Stream, uint32_t Channel, SPI_TypeDef *Instance, uint8_t *pData, uint16_t Size);
#endif
bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool spiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
bool spiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool spiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
uint8_t spiReadReg(const busDevice_t *bus, uint8_t reg);

15
src/main/drivers/bus_spi_hal.c
View file

@ -346,7 +346,7 @@ void spiResetErrorCounter(SPI_TypeDef *instance)
spiHardwareMap[device].errorCount = 0;
}
bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
bool spiWriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
IOLo(bus->spi.csnPin);
spiTransferByte(bus->spi.instance, reg);
@ -356,7 +356,7 @@ bool spiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
return true;
}
bool spiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
bool spiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{
IOLo(bus->spi.csnPin);
spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction
@ -366,6 +366,17 @@ bool spiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_
return true;
}
uint8_t spiReadReg(const busDevice_t *bus, uint8_t reg)
{
uint8_t data;
IOLo(bus->spi.csnPin);
spiTransferByte(bus->spi.instance, reg | 0x80); // read transaction
spiTransfer(bus->spi.instance, &data, NULL, 1);
IOHi(bus->spi.csnPin);
return data;
}
void dmaSPIIRQHandler(dmaChannelDescriptor_t* descriptor)
{
SPIDevice device = descriptor->userParam;

34
src/main/drivers/compass/compass_ak8963.c
View file

@ -90,8 +90,8 @@ static busDevice_t *bus = NULL; // HACK
// 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 verifympu9250SpiWriteRegister mpu6500SpiWriteRegisterDelayed
static bool mpu6500SpiWriteRegisterDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data)
#define mpu9250SpiWriteRegVerify mpu6500SpiWriteRegDelayed
static bool mpu6500SpiWriteRegDelayed(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
IOLo(bus->spi.csnPin);
spiTransferByte(bus->spi.instance, reg);
@ -121,22 +121,22 @@ static queuedReadState_t queuedRead = { false, 0, 0};
static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf)
{
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
delay(10);
__disable_irq();
bool ack = spiReadRegister(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C
bool ack = spiReadRegBuf(bus, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C
__enable_irq();
return ack;
}
static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
{
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte
mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write
mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value
mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte
return true;
}
@ -148,9 +148,9 @@ static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_)
queuedRead.len = len_;
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
verifympu9250SpiWriteRegister(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpu9250SpiWriteRegVerify(bus, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
queuedRead.readStartedAt = micros();
queuedRead.waiting = true;
@ -185,7 +185,7 @@ static bool ak8963SensorCompleteRead(uint8_t *buf)
queuedRead.waiting = false;
spiReadRegister(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer
spiReadRegBuf(bus, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer
return true;
}
#else
@ -334,13 +334,13 @@ bool ak8963Detect(magDev_t *mag)
bus->spi.instance = MPU9250_SPI_INSTANCE;
// initialze I2C master via SPI bus (MPU9250)
verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);
mpu9250SpiWriteRegVerify(&mag->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);
delay(10);
verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
mpu9250SpiWriteRegVerify(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
delay(10);
verifympu9250SpiWriteRegister(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
mpu9250SpiWriteRegVerify(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
delay(10);
#endif
#endif