Update L3GD20 gyro to modern gyro API.
This commit is contained in:
parent
dd7a77e46c
commit
fdd5869054
|
@ -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
|
||||
};
|
||||
|
|
|
@ -205,6 +205,7 @@ typedef enum {
|
|||
ICM_20649_SPI,
|
||||
ICM_20689_SPI,
|
||||
BMI_160_SPI,
|
||||
L3GD20_SPI,
|
||||
} mpuSensor_e;
|
||||
|
||||
typedef enum {
|
||||
|
|
|
@ -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;
|
|
@ -20,4 +20,5 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
bool l3gd20Detect(gyroDev_t *gyro);
|
||||
uint8_t l3gd20Detect(const busDevice_t *bus);
|
||||
bool l3gd20GyroDetect(gyroDev_t *gyro);
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
Loading…
Reference in New Issue