diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index e419b1d007..b7f47e5e29 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -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 diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index d9c81875be..54eefdf532 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -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) { diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c index 006f32eebd..5dd924d4b5 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6000.c @@ -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 diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index 947cee0c63..85dc688b28 100755 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c @@ -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); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index 8d4953f1e2..8f96184be5 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -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; } diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h index 71080cd69e..c9a07e9af1 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.h +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.h @@ -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); diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 9022ecde14..ce3fdce1af 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -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; +} diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 9e825abadc..1546de0baa 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -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); diff --git a/src/main/drivers/bus_spi_hal.c b/src/main/drivers/bus_spi_hal.c index 467d713483..2a1711f2e1 100644 --- a/src/main/drivers/bus_spi_hal.c +++ b/src/main/drivers/bus_spi_hal.c @@ -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; diff --git a/src/main/drivers/compass/compass_ak8963.c b/src/main/drivers/compass/compass_ak8963.c index 9c778d40e2..d8f23b9479 100644 --- a/src/main/drivers/compass/compass_ak8963.c +++ b/src/main/drivers/compass/compass_ak8963.c @@ -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