Merge pull request #3502 from martinbudden/bf_gyro_spi_detect2
Gyro SPI detect function now returns gyro type
This commit is contained in:
commit
43d8657e5e
|
@ -240,6 +240,9 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI
|
UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI
|
||||||
|
|
||||||
|
uint8_t sensor = MPU_NONE;
|
||||||
|
UNUSED(sensor);
|
||||||
|
|
||||||
#ifdef USE_GYRO_SPI_MPU6000
|
#ifdef USE_GYRO_SPI_MPU6000
|
||||||
#ifdef MPU6000_SPI_INSTANCE
|
#ifdef MPU6000_SPI_INSTANCE
|
||||||
spiBusSetInstance(&gyro->bus, MPU6000_SPI_INSTANCE);
|
spiBusSetInstance(&gyro->bus, MPU6000_SPI_INSTANCE);
|
||||||
|
@ -247,8 +250,9 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
||||||
#ifdef MPU6000_CS_PIN
|
#ifdef MPU6000_CS_PIN
|
||||||
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.spi.csnPin;
|
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.spi.csnPin;
|
||||||
#endif
|
#endif
|
||||||
if (mpu6000SpiDetect(&gyro->bus)) {
|
sensor = mpu6000SpiDetect(&gyro->bus);
|
||||||
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
|
if (sensor != MPU_NONE) {
|
||||||
|
gyro->mpuDetectionResult.sensor = sensor;
|
||||||
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||||
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
|
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
|
||||||
gyro->mpuConfiguration.writeFn = spiWriteRegister;
|
gyro->mpuConfiguration.writeFn = spiWriteRegister;
|
||||||
|
@ -263,10 +267,10 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
||||||
#ifdef MPU6500_CS_PIN
|
#ifdef MPU6500_CS_PIN
|
||||||
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.spi.csnPin;
|
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.spi.csnPin;
|
||||||
#endif
|
#endif
|
||||||
const uint8_t mpu6500Sensor = mpu6500SpiDetect(&gyro->bus);
|
sensor = mpu6500SpiDetect(&gyro->bus);
|
||||||
// some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
|
// some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
|
||||||
if (mpu6500Sensor != MPU_NONE) {
|
if (sensor != MPU_NONE) {
|
||||||
gyro->mpuDetectionResult.sensor = mpu6500Sensor;
|
gyro->mpuDetectionResult.sensor = sensor;
|
||||||
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||||
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
|
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
|
||||||
gyro->mpuConfiguration.writeFn = spiWriteRegister;
|
gyro->mpuConfiguration.writeFn = spiWriteRegister;
|
||||||
|
@ -281,8 +285,9 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
||||||
#ifdef MPU9250_CS_PIN
|
#ifdef MPU9250_CS_PIN
|
||||||
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.spi.csnPin;
|
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.spi.csnPin;
|
||||||
#endif
|
#endif
|
||||||
if (mpu9250SpiDetect(&gyro->bus)) {
|
sensor = mpu9250SpiDetect(&gyro->bus);
|
||||||
gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
|
if (sensor != MPU_NONE) {
|
||||||
|
gyro->mpuDetectionResult.sensor = sensor;
|
||||||
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||||
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
|
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
|
||||||
gyro->mpuConfiguration.writeFn = spiWriteRegister;
|
gyro->mpuConfiguration.writeFn = spiWriteRegister;
|
||||||
|
@ -298,8 +303,9 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
||||||
#ifdef ICM20689_CS_PIN
|
#ifdef ICM20689_CS_PIN
|
||||||
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.spi.csnPin;
|
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.spi.csnPin;
|
||||||
#endif
|
#endif
|
||||||
if (icm20689SpiDetect(&gyro->bus)) {
|
sensor = icm20689SpiDetect(&gyro->bus);
|
||||||
gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
|
if (sensor != MPU_NONE) {
|
||||||
|
gyro->mpuDetectionResult.sensor = sensor;
|
||||||
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||||
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
|
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
|
||||||
gyro->mpuConfiguration.writeFn = spiWriteRegister;
|
gyro->mpuConfiguration.writeFn = spiWriteRegister;
|
||||||
|
@ -314,8 +320,9 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
|
||||||
#ifdef BMI160_CS_PIN
|
#ifdef BMI160_CS_PIN
|
||||||
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI160_CS_PIN)) : gyro->bus.spi.csnPin;
|
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI160_CS_PIN)) : gyro->bus.spi.csnPin;
|
||||||
#endif
|
#endif
|
||||||
if (bmi160Detect(&gyro->bus)) {
|
sensor = bmi160Detect(&gyro->bus);
|
||||||
gyro->mpuDetectionResult.sensor = BMI_160_SPI;
|
if (sensor != MPU_NONE) {
|
||||||
|
gyro->mpuDetectionResult.sensor = sensor;
|
||||||
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
|
gyro->mpuConfiguration.readFn = spiReadRegisterBuffer;
|
||||||
gyro->mpuConfiguration.writeFn = spiWriteRegister;
|
gyro->mpuConfiguration.writeFn = spiWriteRegister;
|
||||||
return true;
|
return true;
|
||||||
|
|
|
@ -93,10 +93,10 @@ static int32_t BMI160_do_foc(const busDevice_t *bus);
|
||||||
static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data);
|
static int32_t BMI160_WriteReg(const busDevice_t *bus, uint8_t reg, uint8_t data);
|
||||||
|
|
||||||
|
|
||||||
bool bmi160Detect(const busDevice_t *bus)
|
uint8_t bmi160Detect(const busDevice_t *bus)
|
||||||
{
|
{
|
||||||
if (BMI160Detected) {
|
if (BMI160Detected) {
|
||||||
return true;
|
return BMI_160_SPI;
|
||||||
}
|
}
|
||||||
|
|
||||||
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
||||||
|
@ -111,11 +111,11 @@ bool bmi160Detect(const busDevice_t *bus)
|
||||||
|
|
||||||
/* Check the chip ID */
|
/* Check the chip ID */
|
||||||
if (spiReadRegister(bus, BMI160_REG_CHIPID) != 0xd1) {
|
if (spiReadRegister(bus, BMI160_REG_CHIPID) != 0xd1) {
|
||||||
return false;
|
return MPU_NONE;
|
||||||
}
|
}
|
||||||
|
|
||||||
BMI160Detected = true;
|
BMI160Detected = true;
|
||||||
return true;
|
return BMI_160_SPI;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -381,7 +381,7 @@ void bmi160SpiAccInit(accDev_t *acc)
|
||||||
|
|
||||||
bool bmi160SpiAccDetect(accDev_t *acc)
|
bool bmi160SpiAccDetect(accDev_t *acc)
|
||||||
{
|
{
|
||||||
if (!bmi160Detect(acc->bus.spi.csnPin)) {
|
if (bmi160Detect(acc->bus.spi.csnPin) == MPU_NONE) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -394,7 +394,7 @@ bool bmi160SpiAccDetect(accDev_t *acc)
|
||||||
|
|
||||||
bool bmi160SpiGyroDetect(gyroDev_t *gyro)
|
bool bmi160SpiGyroDetect(gyroDev_t *gyro)
|
||||||
{
|
{
|
||||||
if (!bmi160Detect(gyro->bus.spi.csnPin)) {
|
if (bmi160Detect(gyro->bus.spi.csnPin) == MPU_NONE) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -68,6 +68,6 @@ enum bmi160_gyro_range {
|
||||||
BMI160_RANGE_2000DPS = 0x00,
|
BMI160_RANGE_2000DPS = 0x00,
|
||||||
};
|
};
|
||||||
|
|
||||||
bool bmi160Detect(const busDevice_t *bus);
|
uint8_t bmi160Detect(const busDevice_t *bus);
|
||||||
bool bmi160SpiAccDetect(accDev_t *acc);
|
bool bmi160SpiAccDetect(accDev_t *acc);
|
||||||
bool bmi160SpiGyroDetect(gyroDev_t *gyro);
|
bool bmi160SpiGyroDetect(gyroDev_t *gyro);
|
||||||
|
|
|
@ -53,7 +53,7 @@ static void icm20689SpiInit(const busDevice_t *bus)
|
||||||
hardwareInitialised = true;
|
hardwareInitialised = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool icm20689SpiDetect(const busDevice_t *bus)
|
uint8_t icm20689SpiDetect(const busDevice_t *bus)
|
||||||
{
|
{
|
||||||
icm20689SpiInit(bus);
|
icm20689SpiInit(bus);
|
||||||
|
|
||||||
|
@ -69,13 +69,13 @@ bool icm20689SpiDetect(const busDevice_t *bus)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if (!attemptsRemaining) {
|
if (!attemptsRemaining) {
|
||||||
return false;
|
return MPU_NONE;
|
||||||
}
|
}
|
||||||
} while (attemptsRemaining--);
|
} while (attemptsRemaining--);
|
||||||
|
|
||||||
spiSetDivisor(bus->spi.instance, SPI_CLOCK_STANDARD);
|
spiSetDivisor(bus->spi.instance, SPI_CLOCK_STANDARD);
|
||||||
|
|
||||||
return true;
|
return ICM_20689_SPI;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -27,7 +27,7 @@ bool icm20689GyroDetect(gyroDev_t *gyro);
|
||||||
void icm20689AccInit(accDev_t *acc);
|
void icm20689AccInit(accDev_t *acc);
|
||||||
void icm20689GyroInit(gyroDev_t *gyro);
|
void icm20689GyroInit(gyroDev_t *gyro);
|
||||||
|
|
||||||
bool icm20689SpiDetect(const busDevice_t *bus);
|
uint8_t icm20689SpiDetect(const busDevice_t *bus);
|
||||||
|
|
||||||
bool icm20689SpiAccDetect(accDev_t *acc);
|
bool icm20689SpiAccDetect(accDev_t *acc);
|
||||||
bool icm20689SpiGyroDetect(gyroDev_t *gyro);
|
bool icm20689SpiGyroDetect(gyroDev_t *gyro);
|
||||||
|
|
|
@ -125,7 +125,7 @@ void mpu6000SpiAccInit(accDev_t *acc)
|
||||||
acc->acc_1G = 512 * 8;
|
acc->acc_1G = 512 * 8;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool mpu6000SpiDetect(const busDevice_t *bus)
|
uint8_t mpu6000SpiDetect(const busDevice_t *bus)
|
||||||
{
|
{
|
||||||
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
||||||
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
|
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
|
||||||
|
@ -144,7 +144,7 @@ bool mpu6000SpiDetect(const busDevice_t *bus)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if (!attemptsRemaining) {
|
if (!attemptsRemaining) {
|
||||||
return false;
|
return MPU_NONE;
|
||||||
}
|
}
|
||||||
} while (attemptsRemaining--);
|
} while (attemptsRemaining--);
|
||||||
|
|
||||||
|
@ -166,10 +166,10 @@ bool mpu6000SpiDetect(const busDevice_t *bus)
|
||||||
case MPU6000_REV_D8:
|
case MPU6000_REV_D8:
|
||||||
case MPU6000_REV_D9:
|
case MPU6000_REV_D9:
|
||||||
case MPU6000_REV_D10:
|
case MPU6000_REV_D10:
|
||||||
return true;
|
return MPU_60x0_SPI;
|
||||||
}
|
}
|
||||||
|
|
||||||
return false;
|
return MPU_NONE;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
|
static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
|
||||||
|
|
|
@ -17,7 +17,7 @@
|
||||||
// RF = Register Flag
|
// RF = Register Flag
|
||||||
#define MPU_RF_DATA_RDY_EN (1 << 0)
|
#define MPU_RF_DATA_RDY_EN (1 << 0)
|
||||||
|
|
||||||
bool mpu6000SpiDetect(const busDevice_t *bus);
|
uint8_t mpu6000SpiDetect(const busDevice_t *bus);
|
||||||
|
|
||||||
bool mpu6000SpiAccDetect(accDev_t *acc);
|
bool mpu6000SpiAccDetect(accDev_t *acc);
|
||||||
bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
|
bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
|
||||||
|
|
|
@ -96,8 +96,8 @@ void mpu9250SpiGyroInit(gyroDev_t *gyro)
|
||||||
|
|
||||||
mpuGyroRead(gyro);
|
mpuGyroRead(gyro);
|
||||||
|
|
||||||
if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) {
|
if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(gyro->bus.spi.instance) != 0) {
|
||||||
spiResetErrorCounter(MPU9250_SPI_INSTANCE);
|
spiResetErrorCounter(gyro->bus.spi.instance);
|
||||||
failureMode(FAILURE_GYRO_INIT_FAILED);
|
failureMode(FAILURE_GYRO_INIT_FAILED);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -166,7 +166,7 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
|
||||||
mpuSpi9250InitDone = true; //init done
|
mpuSpi9250InitDone = true; //init done
|
||||||
}
|
}
|
||||||
|
|
||||||
bool mpu9250SpiDetect(const busDevice_t *bus)
|
uint8_t mpu9250SpiDetect(const busDevice_t *bus)
|
||||||
{
|
{
|
||||||
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
||||||
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
|
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
|
||||||
|
@ -182,13 +182,13 @@ bool mpu9250SpiDetect(const busDevice_t *bus)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if (!attemptsRemaining) {
|
if (!attemptsRemaining) {
|
||||||
return false;
|
return MPU_NONE;
|
||||||
}
|
}
|
||||||
} while (attemptsRemaining--);
|
} while (attemptsRemaining--);
|
||||||
|
|
||||||
spiSetDivisor(bus->spi.instance, SPI_CLOCK_FAST);
|
spiSetDivisor(bus->spi.instance, SPI_CLOCK_FAST);
|
||||||
|
|
||||||
return true;
|
return MPU_9250_SPI;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool mpu9250SpiAccDetect(accDev_t *acc)
|
bool mpu9250SpiAccDetect(accDev_t *acc)
|
||||||
|
|
|
@ -28,7 +28,7 @@
|
||||||
|
|
||||||
void mpu9250SpiResetGyro(void);
|
void mpu9250SpiResetGyro(void);
|
||||||
|
|
||||||
bool mpu9250SpiDetect(const busDevice_t *bus);
|
uint8_t mpu9250SpiDetect(const busDevice_t *bus);
|
||||||
|
|
||||||
bool mpu9250SpiAccDetect(accDev_t *acc);
|
bool mpu9250SpiAccDetect(accDev_t *acc);
|
||||||
bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
|
bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
|
||||||
|
|
Loading…
Reference in New Issue