diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 92cf657891..dfc3bedcfc 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -52,6 +52,7 @@ #include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" #include "drivers/accgyro/accgyro_spi_mpu9250.h" +#include "drivers/accgyro/accgyro_spi_l3gd20.h" #include "drivers/accgyro/accgyro_mpu.h" #include "pg/pg.h" @@ -211,6 +212,9 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = { #endif #ifdef USE_ACCGYRO_BMI160 bmi160Detect, +#endif +#ifdef USE_GYRO_L3GD20 + l3gd20Detect, #endif NULL // Avoid an empty array }; diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 77da677317..f5db434c12 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -205,6 +205,7 @@ typedef enum { ICM_20649_SPI, ICM_20689_SPI, BMI_160_SPI, + L3GD20_SPI, } mpuSensor_e; typedef enum { diff --git a/src/main/drivers/accgyro_legacy/accgyro_l3gd20.c b/src/main/drivers/accgyro/accgyro_spi_l3gd20.c similarity index 67% rename from src/main/drivers/accgyro_legacy/accgyro_l3gd20.c rename to src/main/drivers/accgyro/accgyro_spi_l3gd20.c index b1ea9b168d..0b805a7175 100644 --- a/src/main/drivers/accgyro_legacy/accgyro_l3gd20.c +++ b/src/main/drivers/accgyro/accgyro_spi_l3gd20.c @@ -30,13 +30,15 @@ #include "common/maths.h" -#include "drivers/time.h" -#include "drivers/io.h" #include "drivers/bus_spi.h" - +#include "drivers/io.h" +#include "drivers/nvic.h" #include "drivers/sensor.h" +#include "drivers/time.h" + #include "drivers/accgyro/accgyro.h" -#include "accgyro_l3gd20.h" + +#include "accgyro_spi_l3gd20.h" #define READ_CMD ((uint8_t)0x80) #define MULTIPLEBYTE_CMD ((uint8_t)0x40) @@ -72,87 +74,75 @@ #define BOOT ((uint8_t)0x80) -#define ENABLE_L3GD20 IOLo(mpul3gd20CsPin) -#define DISABLE_L3GD20 IOHi(mpul3gd20CsPin) - -static IO_t mpul3gd20CsPin = IO_NONE; - -static void l3gd20SpiInit(SPI_TypeDef *SPIx) +#if defined(USE_MPU_DATA_READY_SIGNAL) +static void l3gd20ExtiHandler(extiCallbackRec_t *cb) { - UNUSED(SPIx); // FIXME - - mpul3gd20CsPin = IOGetByTag(IO_TAG(L3GD20_CS_PIN)); - - spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD); + gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); + gyro->dataReady = true; } +static void l3gd20IntExtiInit(gyroDev_t *gyro) +{ + if (gyro->mpuIntExtiTag == IO_TAG_NONE) { + return; + } + + IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag); + + IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); + EXTIHandlerInit(&gyro->exti, l3gd20ExtiHandler); + EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, EXTI_TRIGGER_RISING); + EXTIEnable(mpuIntIO, true); +} +#endif + void l3gd20GyroInit(gyroDev_t *gyro) { - UNUSED(gyro); // FIXME use it! + spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); - l3gd20SpiInit(L3GD20_SPI); - - ENABLE_L3GD20; - - spiTransferByte(L3GD20_SPI, CTRL_REG5_ADDR); - spiTransferByte(L3GD20_SPI, BOOT); - - DISABLE_L3GD20; + spiBusWriteRegister(&gyro->bus, CTRL_REG5_ADDR, BOOT); delayMicroseconds(100); - ENABLE_L3GD20; - - spiTransferByte(L3GD20_SPI, CTRL_REG1_ADDR); - - spiTransferByte(L3GD20_SPI, MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_3); - //spiTransferByte(L3GD20_SPI, MODE_ACTIVE | OUTPUT_DATARATE_4 | AXES_ENABLE | BANDWIDTH_4); - - DISABLE_L3GD20; + spiBusWriteRegister(&gyro->bus, CTRL_REG1_ADDR, MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_3); + //spiBusWriteRegister(&gyro->bus, CTRL_REG1_ADDR. MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_4); delayMicroseconds(1); - ENABLE_L3GD20; - - spiTransferByte(L3GD20_SPI, CTRL_REG4_ADDR); - spiTransferByte(L3GD20_SPI, BLOCK_DATA_UPDATE_CONTINUOUS | BLE_MSB | FULLSCALE_2000); - - DISABLE_L3GD20; + spiBusWriteRegister(&gyro->bus, CTRL_REG4_ADDR, BLOCK_DATA_UPDATE_CONTINUOUS | BLE_MSB | FULLSCALE_2000); delay(100); + +#if defined(USE_MPU_DATA_READY_SIGNAL) + l3gd20IntExtiInit(gyro); +#endif + } static bool l3gd20GyroRead(gyroDev_t *gyro) { uint8_t buf[6]; - ENABLE_L3GD20; - spiTransferByte(L3GD20_SPI, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD); - - uint8_t index; - for (index = 0; index < sizeof(buf); index++) { - buf[index] = spiTransferByte(L3GD20_SPI, DUMMY_BYTE); - } - - DISABLE_L3GD20; + spiBusReadRegisterBuffer(&gyro->bus, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD,buf, sizeof(buf)); gyro->gyroADCRaw[0] = (int16_t)((buf[0] << 8) | buf[1]); gyro->gyroADCRaw[1] = (int16_t)((buf[2] << 8) | buf[3]); gyro->gyroADCRaw[2] = (int16_t)((buf[4] << 8) | buf[5]); -#if 0 - debug[0] = (int16_t)((buf[1] << 8) | buf[0]); - debug[1] = (int16_t)((buf[3] << 8) | buf[2]); - debug[2] = (int16_t)((buf[5] << 8) | buf[4]); -#endif - return true; } // Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit #define L3GD20_GYRO_SCALE_FACTOR 0.07f -bool l3gd20Detect(gyroDev_t *gyro) +uint8_t l3gd20Detect(const busDevice_t *bus) +{ + UNUSED(bus); + + return L3GD20_SPI; // blindly assume it's present, for now. +} + +bool l3gd20GyroDetect(gyroDev_t *gyro) { gyro->initFn = l3gd20GyroInit; gyro->readFn = l3gd20GyroRead; diff --git a/src/main/drivers/accgyro_legacy/accgyro_l3gd20.h b/src/main/drivers/accgyro/accgyro_spi_l3gd20.h similarity index 90% rename from src/main/drivers/accgyro_legacy/accgyro_l3gd20.h rename to src/main/drivers/accgyro/accgyro_spi_l3gd20.h index cc9dedf260..39d452619f 100644 --- a/src/main/drivers/accgyro_legacy/accgyro_l3gd20.h +++ b/src/main/drivers/accgyro/accgyro_spi_l3gd20.h @@ -20,4 +20,5 @@ #pragma once -bool l3gd20Detect(gyroDev_t *gyro); +uint8_t l3gd20Detect(const busDevice_t *bus); +bool l3gd20GyroDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro_legacy/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro_legacy/accgyro_lsm303dlhc.c index 0c7504d191..21a8831244 100644 --- a/src/main/drivers/accgyro_legacy/accgyro_lsm303dlhc.c +++ b/src/main/drivers/accgyro_legacy/accgyro_lsm303dlhc.c @@ -151,12 +151,6 @@ static bool lsm303dlhcAccRead(accDev_t *acc) acc->ADCRaw[Y] = (int16_t)((buf[3] << 8) | buf[2]) / 2; acc->ADCRaw[Z] = (int16_t)((buf[5] << 8) | buf[4]) / 2; -#if 0 - debug[0] = (int16_t)((buf[1] << 8) | buf[0]); - debug[1] = (int16_t)((buf[3] << 8) | buf[2]); - debug[2] = (int16_t)((buf[5] << 8) | buf[4]); -#endif - return true; } @@ -166,8 +160,9 @@ bool lsm303dlhcAccDetect(accDev_t *acc) uint8_t status; ack = i2cRead(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, LSM303DLHC_STATUS_REG_A, 1, &status); - if (!ack) + if (!ack) { return false; + } acc->initFn = lsm303dlhcAccInit; acc->readFn = lsm303dlhcAccRead; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c8210726f8..ecd542df70 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -51,12 +51,12 @@ #include "drivers/accgyro/accgyro_spi_mpu6500.h" #include "drivers/accgyro/accgyro_spi_mpu9250.h" -#ifdef USE_GYRO_L3G4200D -#include "drivers/accgyro_legacy/accgyro_l3g4200d.h" +#ifdef USE_GYRO_L3GD20 +#include "drivers/accgyro/accgyro_spi_l3gd20.h" #endif -#ifdef USE_GYRO_L3GD20 -#include "drivers/accgyro_legacy/accgyro_l3gd20.h" +#ifdef USE_GYRO_L3G4200D +#include "drivers/accgyro_legacy/accgyro_l3g4200d.h" #endif #include "drivers/accgyro/gyro_sync.h" @@ -296,7 +296,7 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev) #ifdef USE_GYRO_L3GD20 case GYRO_L3GD20: - if (l3gd20Detect(dev)) { + if (l3gd20GyroDetect(dev)) { gyroHardware = GYRO_L3GD20; break; } @@ -418,7 +418,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *c gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag; #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ - || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) + || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) mpuDetect(&gyroSensor->gyroDev, config); #endif diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 726ac8c6ab..8f51fb6211 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -192,11 +192,11 @@ #endif // Generate USE_SPI_GYRO or USE_I2C_GYRO -#if defined(USE_GYRO_L3G4200D) || defined(USE_GYRO_L3GD20) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6000) || defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU6500) +#if defined(USE_GYRO_L3G4200D) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6000) || defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU6500) #define USE_I2C_GYRO #endif -#if defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) +#if defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_L3GD20) #define USE_SPI_GYRO #endif