Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

Update to LSM6DSV16X gyro driver (#13724)

Browse source 
* Enable LPF1_G_EN

* Gyro intialisation review input from STMicro

* Update src/main/drivers/accgyro/accgyro_spi_lsm6dsv16x.c

Error in code from STMicro

Co-authored-by: Petr Ledvina <ledvinap@gmail.com>

---------

Co-authored-by: Petr Ledvina <ledvinap@gmail.com>
This commit is contained in:
Steve Evans 2024-06-27 13:15:35 +01:00 • committed by GitHub
parent 0686b74197
commit a1e5292946
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
1 changed files with 33 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

55
src/main/drivers/accgyro/accgyro_spi_lsm6dsv16x.c
View file

@ -860,23 +860,6 @@ uint8_t lsm6dsv16xSpiDetect(const extDevice_t *dev)
void lsm6dsv16xAccInit(accDev_t *acc) void lsm6dsv16xAccInit(accDev_t *acc)
{ {
const extDevice_t *dev = &acc->gyro->dev;
// Enable the accelerometer in high accuracy mode at 1kHz
spiWriteReg(dev, LSM6DSV_CTRL1,
LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL1_OP_MODE_XL_HIGH_ACCURACY,
LSM6DSV_CTRL1_OP_MODE_XL_MASK,
LSM6DSV_CTRL1_OP_MODE_XL_SHIFT) |
LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL1_ODR_XL_1000HZ,
LSM6DSV_CTRL1_ODR_XL_MASK,
LSM6DSV_CTRL1_ODR_XL_SHIFT));
// Enable 16G sensitivity
spiWriteReg(dev, LSM6DSV_CTRL8,
LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL8_FS_XL_16G,
LSM6DSV_CTRL8_FS_XL_MASK,
LSM6DSV_CTRL8_FS_XL_SHIFT));
// ±16G mode // ±16G mode
acc->acc_1G = 512 * 4; acc->acc_1G = 512 * 4;
} }
@ -961,22 +944,50 @@ void lsm6dsv16xGyroInit(gyroDev_t *gyro)
// Perform a software reset // Perform a software reset
spiWriteReg(dev, LSM6DSV_CTRL3, LSM6DSV_CTRL3_SW_RESET); spiWriteReg(dev, LSM6DSV_CTRL3, LSM6DSV_CTRL3_SW_RESET);
// Select high-accuracy ODR mode 1 before leaving power-off mode // Wait for the device to be ready
while (spiReadRegMsk(dev, LSM6DSV_CTRL3) & LSM6DSV_CTRL3_SW_RESET) {}
// Autoincrement register address when doing block SPI reads and update continuously
spiWriteReg(dev, LSM6DSV_CTRL3, LSM6DSV_CTRL3_IF_INC | LSM6DSV_CTRL3_BDU); /*BDU bit need to be set*/
// Select high-accuracy ODR mode 1
spiWriteReg(dev, LSM6DSV_HAODR_CFG, spiWriteReg(dev, LSM6DSV_HAODR_CFG,
LSM6DSV_ENCODE_BITS(LSM6DSV_HAODR_MODE1, LSM6DSV_ENCODE_BITS(LSM6DSV_HAODR_MODE1,
LSM6DSV_HAODR_CFG_HAODR_SEL_MASK, LSM6DSV_HAODR_CFG_HAODR_SEL_MASK,
LSM6DSV_HAODR_CFG_HAODR_SEL_SHIFT)); LSM6DSV_HAODR_CFG_HAODR_SEL_SHIFT));
// Enable the gyro in high accuracy mode at 8kHz // Enable the accelerometer in high accuracy
spiWriteReg(dev, LSM6DSV_CTRL1,
LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL1_OP_MODE_XL_HIGH_ACCURACY,
LSM6DSV_CTRL1_OP_MODE_XL_MASK,
LSM6DSV_CTRL1_OP_MODE_XL_SHIFT));
// Enable the gyro in high accuracy
spiWriteReg(dev, LSM6DSV_CTRL2, spiWriteReg(dev, LSM6DSV_CTRL2,
LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL2_OP_MODE_G_HIGH_ACCURACY, LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL2_OP_MODE_G_HIGH_ACCURACY,
LSM6DSV_CTRL2_OP_MODE_G_MASK, LSM6DSV_CTRL2_OP_MODE_G_MASK,
LSM6DSV_CTRL2_OP_MODE_G_SHIFT) | LSM6DSV_CTRL2_OP_MODE_G_SHIFT));
// Enable 16G sensitivity
spiWriteReg(dev, LSM6DSV_CTRL8,
LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL8_FS_XL_16G,
LSM6DSV_CTRL8_FS_XL_MASK,
LSM6DSV_CTRL8_FS_XL_SHIFT));
// Enable the accelerometer odr at 1kHz
spiWriteReg(dev, LSM6DSV_CTRL1,
LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL1_ODR_XL_1000HZ,
LSM6DSV_CTRL1_ODR_XL_MASK,
LSM6DSV_CTRL1_ODR_XL_SHIFT));
// Enable the gyro odr at 8kHz
spiWriteReg(dev, LSM6DSV_CTRL2,
LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL2_ODR_G_8000HZ, LSM6DSV_ENCODE_BITS(LSM6DSV_CTRL2_ODR_G_8000HZ,
LSM6DSV_CTRL2_ODR_G_MASK, LSM6DSV_CTRL2_ODR_G_MASK,
LSM6DSV_CTRL2_ODR_G_SHIFT)); LSM6DSV_CTRL2_ODR_G_SHIFT));
// Enable 2000 deg/s sensitivity and selected LPF1 filter setting // Enable 2000 deg/s sensitivity and selected LPF1 filter setting
// Set the LPF1 filter bandwidth
spiWriteReg(dev, LSM6DSV_CTRL6, spiWriteReg(dev, LSM6DSV_CTRL6,
LSM6DSV_ENCODE_BITS(lsm6dsv16xLPF1BandwidthOptions[gyroConfig()->gyro_hardware_lpf], LSM6DSV_ENCODE_BITS(lsm6dsv16xLPF1BandwidthOptions[gyroConfig()->gyro_hardware_lpf],
LSM6DSV_CTRL6_LPF1_G_BW_MASK, LSM6DSV_CTRL6_LPF1_G_BW_MASK,
@ -985,8 +996,8 @@ void lsm6dsv16xGyroInit(gyroDev_t *gyro)
LSM6DSV_CTRL6_FS_G_MASK, LSM6DSV_CTRL6_FS_G_MASK,
LSM6DSV_CTRL6_FS_G_SHIFT)); LSM6DSV_CTRL6_FS_G_SHIFT));
// Autoincrement register address when doing block SPI reads and update continuously // Enable the gyro digital LPF1 filter
spiWriteReg(dev, LSM6DSV_CTRL3, LSM6DSV_CTRL3_IF_INC); spiWriteReg(dev, LSM6DSV_CTRL7, LSM6DSV_CTRL7_LPF1_G_EN);
// Generate pulse on interrupt line, not requiring a read to clear // Generate pulse on interrupt line, not requiring a read to clear
spiWriteReg(dev, LSM6DSV_CTRL4, LSM6DSV_CTRL4_DRDY_PULSED); spiWriteReg(dev, LSM6DSV_CTRL4, LSM6DSV_CTRL4_DRDY_PULSED);