rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add support for lsm6ds3 imu

This commit is contained in:
Hugo Chiang 2022-12-09 10:00:50 +08:00
parent 67a4ce9386
commit b654e758e2
15 changed files with 538 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
make/source.mk
View File

@ -234,6 +234,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
drivers/accgyro/accgyro_mpu3050.c \
drivers/accgyro/accgyro_spi_bmi160.c \
drivers/accgyro/accgyro_spi_bmi270.c \
drivers/accgyro/accgyro_spi_lsm6ds3.c \
drivers/accgyro/accgyro_spi_lsm6dso.c \
drivers/accgyro_legacy/accgyro_adxl345.c \
drivers/accgyro_legacy/accgyro_bma280.c \
@ -372,6 +373,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
drivers/accgyro/accgyro_spi_mpu9250.c \
drivers/accgyro/accgyro_spi_icm20689.c \
drivers/accgyro/accgyro_spi_icm426xx.c \
drivers/accgyro/accgyro_spi_lsm6ds3_init.c \
drivers/accgyro/accgyro_spi_lsm6dso_init.c

4
src/main/cli/settings.c
View File

@ -136,14 +136,14 @@
const char * const lookupTableAccHardware[] = {
"AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC",
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P",
"BMI160", "BMI270", "LSM6DSO", "FAKE"
"BMI160", "BMI270", "LSM6DS3", "LSM6DSO", "FAKE"
};
// sync with gyroHardware_e
const char * const lookupTableGyroHardware[] = {
"AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20",
"MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P",
"BMI160", "BMI270", "LSM6SDO", "FAKE"
"BMI160", "BMI270", "LSM6DS3", "LSM6SDO", "FAKE"
};
#if defined(USE_SENSOR_NAMES) || defined(USE_BARO)

2
src/main/drivers/accgyro/accgyro.h
View File

@ -58,6 +58,7 @@ typedef enum {
GYRO_ICM42688P,
GYRO_BMI160,
GYRO_BMI270,
GYRO_LSM6DS3,
GYRO_LSM6DSO,
GYRO_FAKE
} gyroHardware_e;
@ -75,6 +76,7 @@ typedef enum {
typedef enum {
GYRO_RATE_1_kHz,
GYRO_RATE_1100_Hz,
GYRO_RATE_1667_Hz,
GYRO_RATE_3200_Hz,
GYRO_RATE_6400_Hz,
GYRO_RATE_6664_Hz,

4
src/main/drivers/accgyro/accgyro_mpu.c
View File

@ -51,6 +51,7 @@
#include "drivers/accgyro/accgyro_spi_icm20649.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
#include "drivers/accgyro/accgyro_spi_icm426xx.h"
#include "drivers/accgyro/accgyro_spi_lsm6ds3.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
@ -355,6 +356,9 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
#ifdef USE_GYRO_SPI_ICM20689
icm20689SpiDetect, // icm20689SpiDetect detects ICM20602 and ICM20689
#endif
#ifdef USE_ACCGYRO_LSM6DS3
lsm6ds3Detect,
#endif
#ifdef USE_ACCGYRO_LSM6DSO
lsm6dsoDetect,
#endif

1
src/main/drivers/accgyro/accgyro_mpu.h
View File

@ -202,6 +202,7 @@ typedef enum {
ICM_42688P_SPI,
BMI_160_SPI,
BMI_270_SPI,
LSM6DS3_SPI,
LSM6DSO_SPI,
L3GD20_SPI,
} mpuSensor_e;

206
src/main/drivers/accgyro/accgyro_spi_lsm6ds3.c Normal file
View File

@ -0,0 +1,206 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "platform.h"
#ifdef USE_ACCGYRO_LSM6DS3
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_spi_lsm6ds3.h"
#include "drivers/bus_spi.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/io_impl.h"
#include "drivers/nvic.h"
#include "drivers/sensor.h"
#include "drivers/system.h"
#include "drivers/time.h"
// Need to see at least this many interrupts during initialisation to confirm EXTI connectivity
#define GYRO_EXTI_DETECT_THRESHOLD 1000
#ifdef USE_GYRO_EXTI
// Called in ISR context
// Gyro read has just completed
busStatus_e lsm6ds3Intcallback(uint32_t arg)
{
gyroDev_t *gyro = (gyroDev_t *)arg;
int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI);
if (gyroDmaDuration > gyro->gyroDmaMaxDuration) {
gyro->gyroDmaMaxDuration = gyroDmaDuration;
}
gyro->dataReady = true;
return BUS_READY;
}
void lsm6ds3ExtiHandler(extiCallbackRec_t *cb)
{
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
// Ideally we'd use a timer to capture such information, but unfortunately the port used for EXTI interrupt does
// not have an associated timer
uint32_t nowCycles = getCycleCounter();
gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration;
gyro->gyroLastEXTI = nowCycles;
if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) {
spiSequence(&gyro->dev, gyro->segments);
}
gyro->detectedEXTI++;
}
#else
void lsm6ds3ExtiHandler(extiCallbackRec_t *cb)
{
gyroDev_t *gyro = container_of(cb, gyroDev_t, exti);
gyro->dataReady = true;
}
#endif
bool lsm6ds3AccRead(accDev_t *acc)
{
switch (acc->gyro->gyroModeSPI) {
case GYRO_EXTI_INT:
case GYRO_EXTI_NO_INT:
{
acc->gyro->dev.txBuf[1] = LSM6DS3_REG_OUTX_L_A | 0x80;
busSegment_t segments[] = {
{.u.buffers = {NULL, NULL}, 8, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
segments[0].u.buffers.txData = &acc->gyro->dev.txBuf[1];
segments[0].u.buffers.rxData = &acc->gyro->dev.rxBuf[1];
spiSequence(&acc->gyro->dev, &segments[0]);
// Wait for completion
spiWait(&acc->gyro->dev);
uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf;
acc->ADCRaw[X] = accData[1];
acc->ADCRaw[Y] = accData[2];
acc->ADCRaw[Z] = accData[3];
break;
}
case GYRO_EXTI_INT_DMA:
{
// If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
// up an old value.
// This data was read from the gyro, which is the same SPI device as the acc
uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf;
acc->ADCRaw[X] = accData[4];
acc->ADCRaw[Y] = accData[5];
acc->ADCRaw[Z] = accData[6];
break;
}
case GYRO_EXTI_INIT:
default:
break;
}
return true;
}
bool lsm6ds3GyroRead(gyroDev_t *gyro)
{
uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf;
switch (gyro->gyroModeSPI) {
case GYRO_EXTI_INIT:
{
// Initialise the tx buffer to all 0x00
memset(gyro->dev.txBuf, 0x00, 14);
#ifdef USE_GYRO_EXTI
// Check that minimum number of interrupts have been detected
// We need some offset from the gyro interrupts to ensure sampling after the interrupt
gyro->gyroDmaMaxDuration = 5;
// Using DMA for gyro access upsets the scheduler on the F4
if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) {
if (spiUseDMA(&gyro->dev)) {
gyro->dev.callbackArg = (uint32_t)gyro;
gyro->dev.txBuf[1] = LSM6DS3_REG_OUTX_L_G | 0x80;
gyro->segments[0].len = 13;
gyro->segments[0].callback = lsm6ds3Intcallback;
gyro->segments[0].u.buffers.txData = &gyro->dev.txBuf[1];
gyro->segments[0].u.buffers.rxData = &gyro->dev.rxBuf[1];
gyro->segments[0].negateCS = true;
gyro->gyroModeSPI = GYRO_EXTI_INT_DMA;
} else {
// Interrupts are present, but no DMA
gyro->gyroModeSPI = GYRO_EXTI_INT;
}
} else
#endif
{
gyro->gyroModeSPI = GYRO_EXTI_NO_INT;
}
break;
}
case GYRO_EXTI_INT:
case GYRO_EXTI_NO_INT:
{
gyro->dev.txBuf[1] = LSM6DS3_REG_OUTX_L_G | 0x80;
busSegment_t segments[] = {
{.u.buffers = {NULL, NULL}, 7, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
segments[0].u.buffers.txData = &gyro->dev.txBuf[1];
segments[0].u.buffers.rxData = &gyro->dev.rxBuf[1];
spiSequence(&gyro->dev, &segments[0]);
// Wait for completion
spiWait(&gyro->dev);
// Fall through
FALLTHROUGH;
}
case GYRO_EXTI_INT_DMA:
{
// If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick
// up an old value.
gyro->gyroADCRaw[X] = gyroData[1];
gyro->gyroADCRaw[Y] = gyroData[2];
gyro->gyroADCRaw[Z] = gyroData[3];
break;
}
default:
break;
}
return true;
}
#endif

66
src/main/drivers/accgyro/accgyro_spi_lsm6ds3.h Normal file
View File

@ -0,0 +1,66 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "drivers/bus.h"
#include "drivers/exti.h"
// LSM6DS3 registers (not the complete list)
typedef enum {
LSM6DS3_REG_INT1_CTRL = 0x0D, // int pin 1 control
LSM6DS3_REG_INT2_CTRL = 0x0E, // int pin 2 control
LSM6DS3_REG_WHO_AM_I = 0x0F, // chip ID
LSM6DS3_REG_CTRL1_XL = 0x10, // accelerometer control
LSM6DS3_REG_CTRL2_G = 0x11, // gyro control
LSM6DS3_REG_CTRL3_C = 0x12, // control register 3
LSM6DS3_REG_CTRL4_C = 0x13, // control register 4
LSM6DS3_REG_CTRL5_C = 0x14, // control register 5
LSM6DS3_REG_CTRL6_C = 0x15, // control register 6
LSM6DS3_REG_CTRL7_G = 0x16, // control register 7
LSM6DS3_REG_CTRL8_XL = 0x17, // control register 8
LSM6DS3_REG_CTRL9_XL = 0x18, // control register 9
LSM6DS3_REG_CTRL10_C = 0x19, // control register 10
LSM6DS3_REG_STATUS = 0x1E, // status register
LSM6DS3_REG_OUT_TEMP_L = 0x20, // temperature LSB
LSM6DS3_REG_OUT_TEMP_H = 0x21, // temperature MSB
LSM6DS3_REG_OUTX_L_G = 0x22, // gyro X axis LSB
LSM6DS3_REG_OUTX_H_G = 0x23, // gyro X axis MSB
LSM6DS3_REG_OUTY_L_G = 0x24, // gyro Y axis LSB
LSM6DS3_REG_OUTY_H_G = 0x25, // gyro Y axis MSB
LSM6DS3_REG_OUTZ_L_G = 0x26, // gyro Z axis LSB
LSM6DS3_REG_OUTZ_H_G = 0x27, // gyro Z axis MSB
LSM6DS3_REG_OUTX_L_A = 0x28, // acc X axis LSB
LSM6DS3_REG_OUTX_H_A = 0x29, // acc X axis MSB
LSM6DS3_REG_OUTY_L_A = 0x2A, // acc Y axis LSB
LSM6DS3_REG_OUTY_H_A = 0x2B, // acc Y axis MSB
LSM6DS3_REG_OUTZ_L_A = 0x2C, // acc Z axis LSB
LSM6DS3_REG_OUTZ_H_A = 0x2D, // acc Z axis MSB
} lsm6ds3Register_e;
// Contained in accgyro_spi_lsm6ds3_init.c which is size-optimized
uint8_t lsm6ds3Detect(const extDevice_t *dev);
bool lsm6ds3SpiAccDetect(accDev_t *acc);
bool lsm6ds3SpiGyroDetect(gyroDev_t *gyro);
// Contained in accgyro_spi_lsm6ds3.c which is speed-optimized
void lsm6ds3ExtiHandler(extiCallbackRec_t *cb);
bool lsm6ds3AccRead(accDev_t *acc);
bool lsm6ds3GyroRead(gyroDev_t *gyro);

219
src/main/drivers/accgyro/accgyro_spi_lsm6ds3_init.c Normal file
View File

@ -0,0 +1,219 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include "platform.h"
#ifdef USE_ACCGYRO_LSM6DS3
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_spi_lsm6ds3.h"
#include "drivers/bus_spi.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/io_impl.h"
#include "drivers/nvic.h"
#include "drivers/sensor.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "sensors/gyro.h"
// 10 MHz max SPI frequency
#define LSM6DS3_MAX_SPI_CLK_HZ 10000000
#define LSM6DS3_CHIP_ID 0x69
// LSM6DS3 register configuration values
typedef enum {
LSM6DS3_VAL_INT1_CTRL = 0x02, // enable gyro data ready interrupt pin 1
LSM6DS3_VAL_INT2_CTRL = 0x00, // disable gyro data ready interrupt pin 2
LSM6DS3_VAL_CTRL1_XL_ODR833 = 0x07, // accelerometer 833hz output data rate (gyro/8)
LSM6DS3_VAL_CTRL1_XL_ODR1667 = 0x08, // accelerometer 1666hz output data rate (gyro/4)
LSM6DS3_VAL_CTRL1_XL_ODR3332 = 0x09, // accelerometer 3332hz output data rate (gyro/2)
LSM6DS3_VAL_CTRL1_XL_ODR6664 = 0x0A, // accelerometer 6664hz output data rate (gyro/1)
LSM6DS3_VAL_CTRL1_XL_8G = 0x03, // accelerometer 8G scale
LSM6DS3_VAL_CTRL1_XL_16G = 0x01, // accelerometer 16G scale
LSM6DS3_VAL_CTRL1_XL_BW_XL = 0x01, // accelerometer filter bandwidth 200Hz
LSM6DS3_VAL_CTRL2_G_ODR1667 = 0x08, // gyro 1667hz output data rate
LSM6DS3_VAL_CTRL2_G_2000DPS = 0x03, // gyro 2000dps scale
LSM6DS3_VAL_CTRL3_C_H_LACTIVE = 0, // (bit 5) interrupt pins active high
LSM6DS3_VAL_CTRL3_C_PP_OD = 0, // (bit 4) interrupt pins push/pull
LSM6DS3_VAL_CTRL3_C_SIM = 0, // (bit 3) SPI 4-wire interface mode
LSM6DS3_VAL_CTRL3_C_IF_INC = BIT(2), // (bit 2) auto-increment address for burst reads
LSM6DS3_VAL_CTRL4_C_XL_BW_SCAL_ODR = BIT(7),// (bit 7) bandwidth determined by BW_XL
LSM6DS3_VAL_CTRL4_C_DRDY_MASK = BIT(3), // (bit 3) data ready interrupt mask
LSM6DS3_VAL_CTRL4_C_I2C_DISABLE = BIT(2), // (bit 2) disable I2C interface
LSM6DS3_VAL_CTRL6_C_XL_HM_MODE = 0, // (bit 4) enable accelerometer high performance mode
LSM6DS3_VAL_CTRL7_G_HP_EN_G = BIT(6), // (bit 6) enable gyro high-pass filter
LSM6DS3_VAL_CTRL7_G_HPM_G_8 = 0x00, // (bits 1:0) gyro HPF cutoff 8.1mHz
LSM6DS3_VAL_CTRL7_G_HPM_G_32 = 0x01, // (bits 1:0) gyro HPF cutoff 32.4mHz
LSM6DS3_VAL_CTRL7_G_HPM_G_2070 = 0x02, // (bits 1:0) gyro HPF cutoff 2.07Hz
LSM6DS3_VAL_CTRL7_G_HPM_G_16320 = 0x03, // (bits 1:0) gyro HPF cutoff 16.32Hz
} lsm6ds3ConfigValues_e;
// LSM6DS3 register configuration bit masks
typedef enum {
LSM6DS3_MASK_DRDY_PULSE_CFG_G = 0x80,// 0b10000000
LSM6DS3_MASK_CTRL3_C = 0x3C, // 0b00111100
LSM6DS3_MASK_CTRL3_C_RESET = BIT(0), // 0b00000001
LSM6DS3_MASK_CTRL4_C = 0x8C, // 0b10001100
LSM6DS3_MASK_CTRL6_C = 0x10, // 0b00010000
LSM6DS3_MASK_CTRL7_G = 0x70, // 0b01000011
LSM6DS3_MASK_CTRL9_XL = 0x02, // 0b00000010
} lsm6ds3ConfigMasks_e;
uint8_t lsm6ds3Detect(const extDevice_t *dev)
{
uint8_t chipID = 0;
if (busReadRegisterBuffer(dev, LSM6DS3_REG_WHO_AM_I, &chipID, 1)) {
if (chipID == LSM6DS3_CHIP_ID) {
return LSM6DS3_SPI;
}
}
return MPU_NONE;
}
static void lsm6ds3WriteRegister(const extDevice_t *dev, lsm6ds3Register_e registerID, uint8_t value, unsigned delayMs)
{
busWriteRegister(dev, registerID, value);
if (delayMs) {
delay(delayMs);
}
}
static void lsm6ds3WriteRegisterBits(const extDevice_t *dev, lsm6ds3Register_e registerID, lsm6ds3ConfigMasks_e mask, uint8_t value, unsigned delayMs)
{
uint8_t newValue;
if (busReadRegisterBuffer(dev, registerID, &newValue, 1)) {
delayMicroseconds(2);
newValue = (newValue & ~mask) | value;
lsm6ds3WriteRegister(dev, registerID, newValue, delayMs);
}
}
static void lsm6ds3Config(gyroDev_t *gyro)
{
extDevice_t *dev = &gyro->dev;
// Reset the device (wait 100ms before continuing config)
lsm6ds3WriteRegisterBits(dev, LSM6DS3_REG_CTRL3_C, LSM6DS3_MASK_CTRL3_C_RESET, BIT(0), 100);
// // Configure data ready pulsed mode
// lsm6ds3WriteRegisterBits(dev, LSM6DS3_REG_DRDY_PULSE_CFG_G, LSM6DS3_MASK_DRDY_PULSE_CFG_G, LSM6DS3_VAL_DRDY_PULSE_CFG_G_DDRY, 0);
// Configure interrupt pin 1 for gyro data ready only
lsm6ds3WriteRegister(dev, LSM6DS3_REG_INT1_CTRL, LSM6DS3_VAL_INT1_CTRL, 1);
// Disable interrupt pin 2
lsm6ds3WriteRegister(dev, LSM6DS3_REG_INT2_CTRL, LSM6DS3_VAL_INT2_CTRL, 1);
// Configure the accelerometer
// 833hz ODR, 16G scale, use LPF2 output (default with ODR/4 cutoff)
lsm6ds3WriteRegister(dev, LSM6DS3_REG_CTRL1_XL, (LSM6DS3_VAL_CTRL1_XL_ODR833 << 4) | (LSM6DS3_VAL_CTRL1_XL_16G << 2) | LSM6DS3_VAL_CTRL1_XL_BW_XL, 1);
// Configure the gyro
// 6664hz ODR, 2000dps scale
lsm6ds3WriteRegister(dev, LSM6DS3_REG_CTRL2_G, (LSM6DS3_VAL_CTRL2_G_ODR1667 << 4) | (LSM6DS3_VAL_CTRL2_G_2000DPS << 2), 1);
// Configure control register 3
// latch LSB/MSB during reads; set interrupt pins active high; set interrupt pins push/pull; set 4-wire SPI; enable auto-increment burst reads
lsm6ds3WriteRegisterBits(dev, LSM6DS3_REG_CTRL3_C, LSM6DS3_MASK_CTRL3_C, (LSM6DS3_VAL_CTRL3_C_H_LACTIVE | LSM6DS3_VAL_CTRL3_C_PP_OD | LSM6DS3_VAL_CTRL3_C_SIM | LSM6DS3_VAL_CTRL3_C_IF_INC), 1);
// Configure control register 4
// enable accelerometer high performane mode; enable gyro LPF1
lsm6ds3WriteRegisterBits(dev, LSM6DS3_REG_CTRL4_C, LSM6DS3_MASK_CTRL4_C, (LSM6DS3_VAL_CTRL4_C_XL_BW_SCAL_ODR | LSM6DS3_VAL_CTRL4_C_DRDY_MASK | LSM6DS3_VAL_CTRL4_C_I2C_DISABLE ), 1);
// Configure control register 6
// disable I2C interface; set gyro LPF1 cutoff according to gyro_hardware_lpf setting
lsm6ds3WriteRegisterBits(dev, LSM6DS3_REG_CTRL6_C, LSM6DS3_MASK_CTRL6_C, LSM6DS3_VAL_CTRL6_C_XL_HM_MODE, 1);
// Configure control register 7
lsm6ds3WriteRegisterBits(dev, LSM6DS3_REG_CTRL7_G, LSM6DS3_MASK_CTRL7_G, (LSM6DS3_VAL_CTRL7_G_HP_EN_G | LSM6DS3_VAL_CTRL7_G_HPM_G_32), 1);
}
#ifdef USE_GYRO_EXTI
static void lsm6ds3IntExtiInit(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, lsm6ds3ExtiHandler);
EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING);
EXTIEnable(mpuIntIO);
}
#endif
static void lsm6ds3SpiGyroInit(gyroDev_t *gyro)
{
extDevice_t *dev = &gyro->dev;
lsm6ds3Config(gyro);
#ifdef USE_GYRO_EXTI
lsm6ds3IntExtiInit(gyro);
#endif
spiSetClkDivisor(dev, spiCalculateDivider(LSM6DS3_MAX_SPI_CLK_HZ));
}
static void lsm6ds3SpiAccInit(accDev_t *acc)
{
// sensor is configured during gyro init
acc->acc_1G = 512 * 4; // 16G sensor scale
}
bool lsm6ds3SpiAccDetect(accDev_t *acc)
{
if (acc->mpuDetectionResult.sensor != LSM6DS3_SPI) {
return false;
}
acc->initFn = lsm6ds3SpiAccInit;
acc->readFn = lsm6ds3AccRead;
return true;
}
bool lsm6ds3SpiGyroDetect(gyroDev_t *gyro)
{
if (gyro->mpuDetectionResult.sensor != LSM6DS3_SPI) {
return false;
}
gyro->initFn = lsm6ds3SpiGyroInit;
gyro->readFn = lsm6ds3GyroRead;
gyro->scale = GYRO_SCALE_2000DPS;
return true;
}
#endif // USE_ACCGYRO_LSM6DS3

2
src/main/drivers/accgyro/accgyro_spi_lsm6dso_init.c
View File

@ -53,7 +53,7 @@ typedef enum {
LSM6DSO_VAL_CTRL1_XL_ODR833 = 0x07, // accelerometer 833hz output data rate (gyro/8)
LSM6DSO_VAL_CTRL1_XL_ODR1667 = 0x08, // accelerometer 1666hz output data rate (gyro/4)
LSM6DSO_VAL_CTRL1_XL_ODR3332 = 0x09, // accelerometer 3332hz output data rate (gyro/2)
LSM6DSO_VAL_CTRL1_XL_ODR3333 = 0x0A, // accelerometer 6664hz output data rate (gyro/1)
LSM6DSO_VAL_CTRL1_XL_ODR6664 = 0x0A, // accelerometer 6664hz output data rate (gyro/1)
LSM6DSO_VAL_CTRL1_XL_8G = 0x03, // accelerometer 8G scale
LSM6DSO_VAL_CTRL1_XL_16G = 0x01, // accelerometer 16G scale
LSM6DSO_VAL_CTRL1_XL_LPF1 = 0x00, // accelerometer output from LPF1

7
src/main/drivers/accgyro/gyro_sync.c
View File

@ -77,10 +77,15 @@ uint16_t gyroSetSampleRate(gyroDev_t *gyro)
gyroSampleRateHz = 9000;
accSampleRateHz = 1125;
break;
case LSM6DS3_SPI:
gyro->gyroRateKHz = GYRO_RATE_1667_Hz;
gyroSampleRateHz = 1667;
accSampleRateHz = 833;
break;
#ifdef USE_ACCGYRO_LSM6DSO
case LSM6DSO_SPI:
gyro->gyroRateKHz = GYRO_RATE_6664_Hz;
gyroSampleRateHz = 6664; // Yes, this is correct per the datasheet. Will effectively round to 150us and 6.67KHz.
gyroSampleRateHz = 1666; // Yes, this is correct per the datasheet. Will effectively round to 150us and 6.67KHz.
accSampleRateHz = 833;
break;
#endif

1
src/main/sensors/acceleration.h
View File

@ -46,6 +46,7 @@ typedef enum {
ACC_ICM42688P,
ACC_BMI160,
ACC_BMI270,
ACC_LSM6DS3,
ACC_LSM6DSO,
ACC_FAKE
} accelerationSensor_e;

11
src/main/sensors/acceleration_init.c
View File

@ -47,6 +47,7 @@
#include "drivers/accgyro/accgyro_spi_icm20649.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
#include "drivers/accgyro/accgyro_spi_icm426xx.h"
#include "drivers/accgyro/accgyro_spi_lsm6ds3.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
@ -306,6 +307,16 @@ retry:
FALLTHROUGH;
#endif
#ifdef USE_ACCGYRO_LSM6DS3
case ACC_LSM6DS3:
if (lsm6ds3SpiAccDetect(dev)) {
accHardware = ACC_LSM6DS3;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_ACCGYRO_LSM6DSO
case ACC_LSM6DSO:
if (lsm6dsoSpiAccDetect(dev)) {

16
src/main/sensors/gyro_init.c
View File

@ -46,6 +46,7 @@
#include "drivers/accgyro/accgyro_spi_icm20689.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
#include "drivers/accgyro/accgyro_spi_icm426xx.h"
#include "drivers/accgyro/accgyro_spi_lsm6ds3.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
@ -313,6 +314,7 @@ void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
case GYRO_MPU6000:
case GYRO_MPU6500:
case GYRO_MPU9250:
case GYRO_LSM6DS3:
case GYRO_LSM6DSO:
gyroSensor->gyroDev.gyroHasOverflowProtection = true;
break;
@ -482,6 +484,15 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
FALLTHROUGH;
#endif
#ifdef USE_ACCGYRO_LSM6DS3
case GYRO_LSM6DS3:
if (lsm6ds3SpiGyroDetect(dev)) {
gyroHardware = GYRO_LSM6DS3;
break;
}
FALLTHROUGH;
#endif
#ifdef USE_ACCGYRO_LSM6DSO
case GYRO_LSM6DSO:
if (lsm6dsoSpiGyroDetect(dev)) {
@ -516,7 +527,8 @@ static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t
{
#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_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
|| defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) \
|| defined(USE_ACCGYRO_LSM6DS3) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config);
@ -541,7 +553,7 @@ static void gyroPreInitSensor(const gyroDeviceConfig_t *config)
{
#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_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGRYO_LSM6DSO)
|| defined(USE_GYRO_SPI_ICM20689) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGRYO_LSM6DS3) || defined(USE_ACCGRYO_LSM6DSO)
mpuPreInit(config);
#else
UNUSED(config);

1
src/main/target/AT32F437DEV/target.h
View File

@ -109,6 +109,7 @@
#define USE_GYRO
#define USE_GYRO_SPI_MPU6500
#define USE_GYRO_SPI_ICM42688P
#define USE_ACCGYRO_LSM6DS3
#define USE_ACCGYRO_LSM6DSO
#define USE_ACC

2
src/main/target/AT32F437DEV/target.mk
View File

@ -11,6 +11,8 @@ TARGET_SRC = \
drivers/compass/compass_qmc5883l.c\
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/accgyro/accgyro_spi_mpu6500.c\
drivers/accgyro/accgyro_spi_lsm6ds3_init.c \
drivers/accgyro/accgyro_spi_lsm6ds3.c \
drivers/accgyro/accgyro_spi_lsm6dso_init.c \
drivers/accgyro/accgyro_spi_lsm6dso.c \
drivers/max7456.c \