Update L3GD20 gyro to modern gyro API.

This commit is contained in:
mikeller 2018-12-01 22:13:45 +13:00
parent dd7a77e46c
commit fdd5869054
7 changed files with 61 additions and 70 deletions

View File

@ -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
};

View File

@ -205,6 +205,7 @@ typedef enum {
ICM_20649_SPI,
ICM_20689_SPI,
BMI_160_SPI,
L3GD20_SPI,
} mpuSensor_e;
typedef enum {

View File

@ -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;

View File

@ -20,4 +20,5 @@
#pragma once
bool l3gd20Detect(gyroDev_t *gyro);
uint8_t l3gd20Detect(const busDevice_t *bus);
bool l3gd20GyroDetect(gyroDev_t *gyro);

View File

@ -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;

View File

@ -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

View File

@ -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