mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-20 11:23:06 +08:00
feat(boards/px4_fmu-v6c): add support for LSM6DSV16X (#26951)
* feat(drivers): add ST LSM6DSV16X IMU driver Add a new SPI driver for the ST LSM6DSV16X 6-axis IMU with FIFO-based accel, gyro, and temperature acquisition. The driver supports continuous FIFO mode with tagged sample parsing, DRDY interrupt triggering via FIFO threshold, and dynamic watermark configuration based on the selected sample rate. It also falls back to scheduled polling when a GPIO interrupt is not available. To improve robustness, the driver periodically validates critical register settings and limits FIFO draining per cycle. Tested on Pixhawk6C with ground testing and flight validation. * feat(boards/px4_fmu-v6c): add LSM6DSV16X support for Pixhawk6C Enable the ST LSM6DSV16X as an alternative probe-based IMU on Pixhawk6C. The sensor is configured on SPI1 with chip-select PC13 and shares the same CS and DRDY lines as the ICM42688P across all supported hardware revisions. Board startup first probes the ICM42688P and then the LSM6DSV16X, so only the IMU populated on a given board is started. This change enables the LSM6DSV16X driver in the board configuration, registers the device type in `spi.cpp`, and adds board sensor probing for the new IMU. Use rotation `-R 26` (`ROTATION_PITCH_180_YAW_90`) to match the sensor orientation on the Pixhawk6C PCB. * fix(drivers/lsm6dsv): address review comments - add missing #include <cstddef> for size_t - check FIFO status transfer() return value - fix FIFO word count to sample period conversion (DIFF_FIFO reports words, not sample periods; each sample period is 2 words) - fix drv_sensor.h macro alignment (spaces to tab) * drv_sensor: move LSM6DSV devtype into IMU range * fmu-v6c: use quiet LSM6DSV fallback probe
This commit is contained in:
zebulon-86
GitHub
@@ -22,6 +22,7 @@ CONFIG_DRIVERS_HEATER=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI055=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
|
||||
CONFIG_DRIVERS_IMU_ST_LSM6DSV=y
|
||||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
|
||||
@@ -15,8 +15,11 @@ else
|
||||
bmi055 -G -R 4 -s start
|
||||
fi
|
||||
|
||||
# Internal SPI bus ICM42688p
|
||||
icm42688p -R 6 -s start
|
||||
# Internal SPI bus IMU (probe-based: LSM6DSV or ICM42688P on same CS)
|
||||
if ! icm42688p -R 6 -s -q start
|
||||
then
|
||||
lsm6dsv -R 26 -s start
|
||||
fi
|
||||
|
||||
# Internal barometer on I2C4 (The same bus is also exposed externally, and therefore marked as external)
|
||||
ms5611 -X -b 4 -a 0x77 start
|
||||
|
||||
@@ -41,6 +41,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
|
||||
initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ST_LSM6DSV, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
|
||||
}, {GPIO::PortB, GPIO::Pin2}),
|
||||
initSPIBus(SPI::Bus::SPI2, {
|
||||
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
|
||||
@@ -51,6 +52,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
|
||||
initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ST_LSM6DSV, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
|
||||
}, {GPIO::PortB, GPIO::Pin2}),
|
||||
initSPIBus(SPI::Bus::SPI2, {
|
||||
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
|
||||
@@ -61,6 +63,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
|
||||
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ST_LSM6DSV, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
|
||||
}, {GPIO::PortB, GPIO::Pin2}),
|
||||
initSPIBus(SPI::Bus::SPI2, {
|
||||
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
|
||||
@@ -71,6 +74,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
|
||||
initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ST_LSM6DSV, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
|
||||
}, {GPIO::PortB, GPIO::Pin2}),
|
||||
initSPIBus(SPI::Bus::SPI2, {
|
||||
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
|
||||
@@ -81,6 +85,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
|
||||
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ST_LSM6DSV, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
|
||||
}, {GPIO::PortB, GPIO::Pin2}),
|
||||
initSPIBus(SPI::Bus::SPI2, {
|
||||
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
|
||||
|
||||
@@ -68,6 +68,7 @@
|
||||
#define DRV_MAG_DEVTYPE_QMC5883P 0x0F
|
||||
|
||||
#define DRV_IMU_DEVTYPE_LSM303D 0x11
|
||||
#define DRV_IMU_DEVTYPE_ST_LSM6DSV 0x12
|
||||
|
||||
#define DRV_IMU_DEVTYPE_SIM 0x14
|
||||
#define DRV_DIFF_PRESS_DEVTYPE_SIM 0x15
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2024-2026 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE drivers__imu__st__lsm6dsv
|
||||
MAIN lsm6dsv
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
LSM6DSV.cpp
|
||||
LSM6DSV.hpp
|
||||
lsm6dsv_main.cpp
|
||||
ST_LSM6DSV_Registers.hpp
|
||||
DEPENDS
|
||||
drivers_accelerometer
|
||||
drivers_gyroscope
|
||||
px4_work_queue
|
||||
)
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig DRIVERS_IMU_ST_LSM6DSV
|
||||
bool "lsm6dsv"
|
||||
default n
|
||||
---help---
|
||||
Enable support for lsm6dsv
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,159 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024-2026 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file LSM6DSV.hpp
|
||||
*
|
||||
* Driver for the ST LSM6DSV connected via SPI.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ST_LSM6DSV_Registers.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/device/spi.h>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/atomic.h>
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
|
||||
using namespace ST_LSM6DSV;
|
||||
|
||||
class LSM6DSV : public device::SPI, public I2CSPIDriver<LSM6DSV>
|
||||
{
|
||||
public:
|
||||
LSM6DSV(const I2CSPIDriverConfig &config);
|
||||
~LSM6DSV() override;
|
||||
|
||||
static void print_usage();
|
||||
|
||||
void RunImpl();
|
||||
|
||||
int init() override;
|
||||
void print_status() override;
|
||||
|
||||
private:
|
||||
void exit_and_cleanup() override;
|
||||
|
||||
// Sensor Configuration
|
||||
static constexpr float FIFO_SAMPLE_DT{1e6f / GYRO_ODR};
|
||||
static constexpr float GYRO_RATE{static_cast<float>(GYRO_ODR)};
|
||||
static constexpr float ACCEL_RATE{static_cast<float>(ACCEL_ODR)};
|
||||
|
||||
static constexpr int32_t FIFO_MAX_SAMPLES{static_cast<int32_t>(FIFO::MAX_DRAIN_SAMPLES)};
|
||||
|
||||
struct register_config_t {
|
||||
Register reg;
|
||||
uint8_t set_bits{0};
|
||||
uint8_t clear_bits{0};
|
||||
};
|
||||
|
||||
int probe() override;
|
||||
|
||||
bool Reset();
|
||||
|
||||
bool Configure();
|
||||
void ConfigureSampleRate(int sample_rate);
|
||||
|
||||
bool RegisterCheck(const register_config_t ®_cfg);
|
||||
|
||||
uint8_t RegisterRead(Register reg);
|
||||
void RegisterWrite(Register reg, uint8_t value);
|
||||
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
|
||||
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples);
|
||||
void FIFOReset();
|
||||
|
||||
void UpdateTemperature();
|
||||
|
||||
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
|
||||
void DataReady();
|
||||
bool DataReadyInterruptConfigure();
|
||||
bool DataReadyInterruptDisable();
|
||||
void ConfigureFIFOWatermark(uint8_t samples);
|
||||
|
||||
const spi_drdy_gpio_t _drdy_gpio;
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
|
||||
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
|
||||
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
|
||||
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
|
||||
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
|
||||
perf_counter_t _drdy_missed_perf{nullptr};
|
||||
|
||||
hrt_abstime _reset_timestamp{0};
|
||||
hrt_abstime _last_config_check_timestamp{0};
|
||||
hrt_abstime _temperature_update_timestamp{0};
|
||||
int _failure_count{0};
|
||||
|
||||
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
|
||||
bool _data_ready_interrupt_enabled{false};
|
||||
|
||||
enum class STATE : uint8_t {
|
||||
RESET,
|
||||
WAIT_FOR_RESET,
|
||||
CONFIGURE,
|
||||
FIFO_RESET,
|
||||
FIFO_READ,
|
||||
} _state{STATE::RESET};
|
||||
|
||||
uint16_t _fifo_empty_interval_us{500}; // default 500 us / 2000 Hz
|
||||
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_ODR))};
|
||||
|
||||
uint8_t _checked_register{0};
|
||||
static constexpr uint8_t size_register_cfg{12};
|
||||
register_config_t _register_cfg[size_register_cfg] {
|
||||
// Register | Set bits | Clear bits
|
||||
{ Register::CTRL3, CTRL3_BIT::BDU | CTRL3_BIT::IF_INC, CTRL3_BIT::SW_RESET },
|
||||
{ Register::HAODR_CFG, HAODR_CFG_BIT::HAODR_MODE1, 0 },
|
||||
{ Register::CTRL1, HAODR_MODE1_ODR_2000HZ | CTRL1_BIT::CTRL1_MODE_HAODR, 0 },
|
||||
{ Register::CTRL2, HAODR_MODE1_ODR_2000HZ | CTRL2_BIT::CTRL2_MODE_HAODR, 0 },
|
||||
{ Register::CTRL6, CTRL6_BIT::FS_G_2000DPS, 0 },
|
||||
{ Register::CTRL8, CTRL8_BIT::FS_XL_16G | CTRL8_BIT::LPF2_BW_ODR_DIV_10, 0 },
|
||||
{ Register::CTRL9, CTRL9_BIT::LPF2_XL_EN, 0 },
|
||||
{
|
||||
Register::FIFO_CTRL3, static_cast<uint8_t>(FIFO_CTRL3_BIT::BDR_GY_HAODR) |
|
||||
static_cast<uint8_t>(FIFO_CTRL3_BIT::BDR_XL_HAODR), 0
|
||||
},
|
||||
{ Register::FIFO_CTRL4, FIFO_CTRL4_BIT::FIFO_MODE_CONTINUOUS, 0 },
|
||||
{ Register::INT1_CTRL, INT1_CTRL_BIT::INT1_FIFO_TH, 0 },
|
||||
{ Register::CTRL4, CTRL4_BIT::DRDY_PULSED, 0 },
|
||||
{ Register::FIFO_CTRL1, 0, 0 }, // WTM[7:0] set at runtime by ConfigureFIFOWatermark()
|
||||
};
|
||||
};
|
||||
@@ -0,0 +1,217 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024-2026 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ST_LSM6DSV_Registers.hpp
|
||||
*
|
||||
* ST LSM6DSV registers.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
|
||||
static constexpr uint8_t Bit0 = (1 << 0);
|
||||
static constexpr uint8_t Bit1 = (1 << 1);
|
||||
static constexpr uint8_t Bit2 = (1 << 2);
|
||||
static constexpr uint8_t Bit3 = (1 << 3);
|
||||
static constexpr uint8_t Bit4 = (1 << 4);
|
||||
static constexpr uint8_t Bit5 = (1 << 5);
|
||||
static constexpr uint8_t Bit6 = (1 << 6);
|
||||
static constexpr uint8_t Bit7 = (1 << 7);
|
||||
|
||||
namespace ST_LSM6DSV
|
||||
{
|
||||
|
||||
static constexpr uint32_t SPI_SPEED = 8 * 1000 * 1000; // 8 MHz SPI data clock
|
||||
|
||||
static constexpr uint8_t DIR_READ = 0x80;
|
||||
|
||||
static constexpr uint8_t WHO_AM_I_ID = 0x70; // LSM6DSV16X
|
||||
|
||||
// HAODR mode-1 ODR: 2000 Hz
|
||||
static constexpr uint32_t GYRO_ODR = 2000;
|
||||
static constexpr uint32_t ACCEL_ODR = 2000;
|
||||
|
||||
// HAODR mode-1 ODR codes (written to CTRL1/CTRL2 [3:0])
|
||||
static constexpr uint8_t HAODR_MODE1_ODR_2000HZ = 0x0A;
|
||||
|
||||
enum class Register : uint8_t {
|
||||
IF_CFG = 0x03, // Interrupt polarity and output mode
|
||||
|
||||
FIFO_CTRL1 = 0x07,
|
||||
FIFO_CTRL2 = 0x08,
|
||||
FIFO_CTRL3 = 0x09,
|
||||
FIFO_CTRL4 = 0x0A,
|
||||
|
||||
INT1_CTRL = 0x0D, // INT1 pin control
|
||||
|
||||
WHO_AM_I = 0x0F,
|
||||
|
||||
CTRL1 = 0x10, // Accel ODR + FS
|
||||
CTRL2 = 0x11, // Gyro ODR + FS
|
||||
CTRL3 = 0x12, // BDU, IF_INC, SW_RESET
|
||||
CTRL4 = 0x13,
|
||||
CTRL6 = 0x15, // Gyro FS
|
||||
CTRL8 = 0x17, // Accel FS + LPF2 BW
|
||||
CTRL9 = 0x18, // LPF2 enable
|
||||
|
||||
FIFO_STATUS1 = 0x1B,
|
||||
FIFO_STATUS2 = 0x1C,
|
||||
|
||||
STATUS_REG = 0x1E,
|
||||
|
||||
OUT_TEMP_L = 0x20,
|
||||
OUT_TEMP_H = 0x21,
|
||||
|
||||
OUTX_L_G = 0x22,
|
||||
|
||||
OUTX_L_A = 0x28,
|
||||
|
||||
FIFO_DATA_OUT_TAG = 0x78,
|
||||
FIFO_DATA_OUT_X_L = 0x79,
|
||||
|
||||
HAODR_CFG = 0x62,
|
||||
};
|
||||
|
||||
// CTRL1 — Accelerometer control (ODR + operating mode + HAODR flag)
|
||||
enum CTRL1_BIT : uint8_t {
|
||||
// ODR_XL [3:0] — set via HAODR mode-1 code 0x0A for 2000 Hz
|
||||
// OP_MODE_XL [2:0] in bits [6:4]: 0b000 = high-performance mode
|
||||
CTRL1_MODE_HAODR = Bit4, // bit 4 must be set for HAODR selection
|
||||
};
|
||||
|
||||
// CTRL2 — Gyroscope control (ODR + operating mode + HAODR flag)
|
||||
enum CTRL2_BIT : uint8_t {
|
||||
CTRL2_MODE_HAODR = Bit4,
|
||||
};
|
||||
|
||||
// IF_CFG (0x03) — Interrupt polarity and output mode
|
||||
enum IF_CFG_BIT : uint8_t {
|
||||
H_LACTIVE = Bit4, // Interrupt active-low
|
||||
PP_OD = Bit3, // Push-pull (0) / Open-drain (1)
|
||||
};
|
||||
|
||||
// INT1_CTRL (0x0D) — INT1 pin control
|
||||
enum INT1_CTRL_BIT : uint8_t {
|
||||
INT1_FIFO_TH = Bit3, // FIFO threshold interrupt on INT1
|
||||
};
|
||||
|
||||
// CTRL3
|
||||
enum CTRL3_BIT : uint8_t {
|
||||
BDU = Bit6, // Block Data Update
|
||||
IF_INC = Bit2, // Register address auto-increment
|
||||
SW_RESET = Bit0, // Software reset
|
||||
};
|
||||
|
||||
// CTRL4
|
||||
enum CTRL4_BIT : uint8_t {
|
||||
INT2_on_INT1 = Bit4, // Route INT2 signals to INT1
|
||||
DRDY_PULSED = Bit1, // Pulsed interrupt mode (~65 µs)
|
||||
};
|
||||
|
||||
// CTRL6 — Gyroscope full-scale
|
||||
enum CTRL6_BIT : uint8_t {
|
||||
// FS_G [3:0]
|
||||
FS_G_2000DPS = 0x04, // ±2000 dps
|
||||
};
|
||||
|
||||
// CTRL8 — Accelerometer full-scale + LPF2 bandwidth
|
||||
enum CTRL8_BIT : uint8_t {
|
||||
// FS_XL [1:0] in bits [1:0]
|
||||
FS_XL_16G = 0x03, // ±16 g
|
||||
|
||||
// HP_LPF2_XL_BW [2:0] in bits [7:5] — when LPF2 enabled via CTRL9
|
||||
LPF2_BW_ODR_DIV_10 = Bit5, // 0x20 → ODR/10
|
||||
};
|
||||
|
||||
// CTRL9
|
||||
enum CTRL9_BIT : uint8_t {
|
||||
LPF2_XL_EN = Bit3, // Enable accelerometer LPF2
|
||||
};
|
||||
|
||||
// STATUS_REG
|
||||
enum STATUS_REG_BIT : uint8_t {
|
||||
XLDA = Bit0, // Accelerometer new data available
|
||||
GDA = Bit1, // Gyroscope new data available
|
||||
TDA = Bit2, // Temperature new data available
|
||||
};
|
||||
|
||||
// FIFO_CTRL3 — Batch Data Rate for accel and gyro
|
||||
enum FIFO_CTRL3_BIT : uint8_t {
|
||||
// BDR_GY [3:0] in bits [7:4], BDR_XL [3:0] in bits [3:0]
|
||||
// Set both to HAODR mode-1 code = 0x0A
|
||||
BDR_XL_HAODR = HAODR_MODE1_ODR_2000HZ,
|
||||
BDR_GY_HAODR = HAODR_MODE1_ODR_2000HZ << 4,
|
||||
};
|
||||
|
||||
// FIFO_CTRL4 — FIFO mode
|
||||
enum FIFO_CTRL4_BIT : uint8_t {
|
||||
FIFO_MODE_BYPASS = 0x00,
|
||||
FIFO_MODE_CONTINUOUS = 0x06, // Continuous mode
|
||||
};
|
||||
|
||||
// FIFO_STATUS2 (bit layout: [7]WTM_IA [6]OVR_IA [5]FULL_IA [4]CNT_BDR [3]OVR_LATCHED [2:1]0 [0]DIFF8)
|
||||
enum FIFO_STATUS2_BIT : uint8_t {
|
||||
FIFO_OVR_LATCHED = Bit3, // FIFO overrun latched (cleared on read)
|
||||
COUNTER_BDR_IA = Bit4, // Counter BDR reached
|
||||
FIFO_OVR_IA = Bit6, // FIFO overrun status (non-latched)
|
||||
DIFF_FIFO_8 = Bit0, // bit 8 of FIFO diff count
|
||||
};
|
||||
|
||||
// HAODR_CFG
|
||||
enum HAODR_CFG_BIT : uint8_t {
|
||||
HAODR_MODE1 = 0x01, // Enable HAODR mode-1
|
||||
};
|
||||
|
||||
// FIFO tag IDs (upper 5 bits of FIFO_DATA_OUT_TAG >> 3)
|
||||
enum class FifoTag : uint8_t {
|
||||
GYRO_NC = 0x01,
|
||||
ACCEL_NC = 0x02,
|
||||
TEMPERATURE = 0x03,
|
||||
TIMESTAMP = 0x04,
|
||||
};
|
||||
|
||||
namespace FIFO
|
||||
{
|
||||
// FIFO word: 1-byte tag + 6-byte data = 7 bytes
|
||||
static constexpr size_t WORD_SIZE = 7;
|
||||
// Max samples to drain per poll (avoid blocking scheduler)
|
||||
static constexpr size_t MAX_DRAIN_SAMPLES = 32;
|
||||
// FIFO depth: 512 words max on LSM6DSV
|
||||
static constexpr size_t DEPTH = 512;
|
||||
}
|
||||
|
||||
} // namespace ST_LSM6DSV
|
||||
@@ -0,0 +1,87 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024-2026 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "LSM6DSV.hpp"
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
void LSM6DSV::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("lsm6dsv", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
extern "C" int lsm6dsv_main(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
using ThisDriver = LSM6DSV;
|
||||
BusCLIArguments cli{false, true};
|
||||
cli.default_spi_frequency = SPI_SPEED;
|
||||
|
||||
while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'R':
|
||||
cli.rotation = (enum Rotation)atoi(cli.optArg());
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
const char *verb = cli.optArg();
|
||||
|
||||
if (!verb) {
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ST_LSM6DSV);
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
return ThisDriver::module_start(cli, iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
return ThisDriver::module_stop(iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "status")) {
|
||||
return ThisDriver::module_status(iterator);
|
||||
}
|
||||
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
||||
Reference in New Issue
Block a user