diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index 547f086974..c13d6478b0 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -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 diff --git a/boards/px4/fmu-v6c/init/rc.board_sensors b/boards/px4/fmu-v6c/init/rc.board_sensors index 634afade5f..34afb338dc 100644 --- a/boards/px4/fmu-v6c/init/rc.board_sensors +++ b/boards/px4/fmu-v6c/init/rc.board_sensors @@ -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 diff --git a/boards/px4/fmu-v6c/src/spi.cpp b/boards/px4/fmu-v6c/src/spi.cpp index 4f51807057..af3d1c9f6b 100644 --- a/boards/px4/fmu-v6c/src/spi.cpp +++ b/boards/px4/fmu-v6c/src/spi.cpp @@ -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}) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 204abbfc35..bff7cfd7cd 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -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 diff --git a/src/drivers/imu/st/lsm6dsv/CMakeLists.txt b/src/drivers/imu/st/lsm6dsv/CMakeLists.txt new file mode 100644 index 0000000000..d28b39b19c --- /dev/null +++ b/src/drivers/imu/st/lsm6dsv/CMakeLists.txt @@ -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 + ) diff --git a/src/drivers/imu/st/lsm6dsv/Kconfig b/src/drivers/imu/st/lsm6dsv/Kconfig new file mode 100644 index 0000000000..ca691463e4 --- /dev/null +++ b/src/drivers/imu/st/lsm6dsv/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_ST_LSM6DSV + bool "lsm6dsv" + default n + ---help--- + Enable support for lsm6dsv diff --git a/src/drivers/imu/st/lsm6dsv/LSM6DSV.cpp b/src/drivers/imu/st/lsm6dsv/LSM6DSV.cpp new file mode 100644 index 0000000000..37f2d9a3c6 --- /dev/null +++ b/src/drivers/imu/st/lsm6dsv/LSM6DSV.cpp @@ -0,0 +1,539 @@ +/**************************************************************************** + * + * 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" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +LSM6DSV::LSM6DSV(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) +{ + if (config.drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); + } + + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); +} + +LSM6DSV::~LSM6DSV() +{ + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_fifo_empty_perf); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); + perf_free(_drdy_missed_perf); +} + +int LSM6DSV::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool LSM6DSV::Reset() +{ + DataReadyInterruptDisable(); + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void LSM6DSV::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void LSM6DSV::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.1f Hz), %ld samples per cycle", + _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us, (long)_fifo_gyro_samples); + PX4_INFO("Sensor ODR: %u Hz (HAODR mode1), FIFO sample dt: %.0f us", + (unsigned)GYRO_ODR, (double)FIFO_SAMPLE_DT); + + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_missed_perf); +} + +int LSM6DSV::probe() +{ + const uint8_t whoami = RegisterRead(Register::WHO_AM_I); + + if (whoami != WHO_AM_I_ID) { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; + } + + return PX4_OK; +} + +void LSM6DSV::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // Software reset + RegisterWrite(Register::CTRL3, CTRL3_BIT::SW_RESET); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(100_ms); + break; + + case STATE::WAIT_FOR_RESET: + if (RegisterRead(Register::WHO_AM_I) == WHO_AM_I_ID) { + // Set IF_INC immediately to enable multi-byte reads + RegisterWrite(Register::CTRL3, CTRL3_BIT::IF_INC | CTRL3_BIT::BDU); + + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } else { + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + _state = STATE::FIFO_RESET; + ScheduleDelayed(1_ms); + + } else { + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::FIFO_RESET: + _state = STATE::FIFO_READ; + FIFOReset(); + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + ScheduleDelayed(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + break; + + case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + bool success = false; + + if (_data_ready_interrupt_enabled) { + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { + perf_count(_drdy_missed_perf); + } + + ScheduleDelayed(_fifo_empty_interval_us * 2); + } + + // Read FIFO status (atomic multi-byte read to avoid race between STATUS1 and STATUS2) + struct FIFOStatusTransfer { + uint8_t cmd{static_cast(Register::FIFO_STATUS1) | DIR_READ}; + uint8_t STATUS1{0}; + uint8_t STATUS2{0}; + } fifo_status{}; + + if (transfer((uint8_t *)&fifo_status, (uint8_t *)&fifo_status, sizeof(fifo_status)) != PX4_OK) { + perf_count(_bad_transfer_perf); + + } else if (fifo_status.STATUS2 & static_cast(FIFO_STATUS2_BIT::FIFO_OVR_LATCHED)) { + FIFOReset(); + perf_count(_fifo_overflow_perf); + + } else { + // FIFO unread word count: 9-bit field (FIFO_STATUS2 bit0 is bit8) + // Each sample period produces 2 words (1 gyro word + 1 accel word) + uint16_t fifo_words = fifo_status.STATUS1; + + if (fifo_status.STATUS2 & static_cast(FIFO_STATUS2_BIT::DIFF_FIFO_8)) { + fifo_words |= (1u << 8); + } + + // Convert word count to sample periods for comparisons against _fifo_gyro_samples / FIFO_MAX_SAMPLES + const uint16_t sample_periods = fifo_words / 2; + + if (sample_periods == 0) { + perf_count(_fifo_empty_perf); + + } else if (sample_periods > static_cast(FIFO_MAX_SAMPLES)) { + // not technically an overflow, but more samples than we expected or can publish + FIFOReset(); + perf_count(_fifo_overflow_perf); + + } else { + + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (sample_periods == static_cast(_fifo_gyro_samples) + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + fifo_words -= 2; + } + + if (FIFORead(timestamp_sample, fifo_words)) { + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + } + } + + if (!success) { + _failure_count++; + + if (_failure_count > 10) { + Reset(); + return; + } + } + + // periodically check configuration registers + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + perf_count(_bad_register_perf); + Reset(); + } + + } else { + // periodically update temperature (~1 Hz) + if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + UpdateTemperature(); + _temperature_update_timestamp = now; + } + } + } + + break; + } +} + +void LSM6DSV::ConfigureSampleRate(int sample_rate) +{ + const float min_interval = FIFO_SAMPLE_DT; + _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); + + _fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES)); + + _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); + + ConfigureFIFOWatermark(_fifo_gyro_samples); +} + +bool LSM6DSV::Configure() +{ + // First enable HAODR mode, then configure ODR registers + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // Verify all registers + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + // Gyroscope: ±2000 dps, 70 mdps/LSB (ST datasheet) + _px4_gyro.set_scale(math::radians(70.f / 1000.f)); + _px4_gyro.set_range(math::radians(2000.f)); + + // Accelerometer: ±16g, 0.488 mg/LSB (ST datasheet) + _px4_accel.set_scale(0.488f * (CONSTANTS_ONE_G / 1000.f)); + _px4_accel.set_range(16.f * CONSTANTS_ONE_G); + + return success; +} + +bool LSM6DSV::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint8_t LSM6DSV::RegisterRead(Register reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +void LSM6DSV::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +void LSM6DSV::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} + +bool LSM6DSV::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) +{ + sensor_gyro_fifo_s gyro{}; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = 0; + gyro.dt = FIFO_SAMPLE_DT; + + sensor_accel_fifo_s accel{}; + accel.timestamp_sample = timestamp_sample; + accel.samples = 0; + accel.dt = FIFO_SAMPLE_DT; + + // Read FIFO word by word: each word is 7 bytes (tag + 6 data) + for (uint16_t i = 0; i < samples; i++) { + // Read tag + data in one transfer (1 cmd byte + 7 data bytes) + struct FIFOWordTransfer { + uint8_t cmd{static_cast(Register::FIFO_DATA_OUT_TAG) | DIR_READ}; + uint8_t TAG{0}; + uint8_t DATA_X_L{0}; + uint8_t DATA_X_H{0}; + uint8_t DATA_Y_L{0}; + uint8_t DATA_Y_H{0}; + uint8_t DATA_Z_L{0}; + uint8_t DATA_Z_H{0}; + } buffer{}; + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) { + perf_count(_bad_transfer_perf); + continue; + } + + // Decode tag from upper 5 bits + const uint8_t tag_id = buffer.TAG >> 3; + + const int16_t data_x = combine(buffer.DATA_X_H, buffer.DATA_X_L); + const int16_t data_y = combine(buffer.DATA_Y_H, buffer.DATA_Y_L); + const int16_t data_z = combine(buffer.DATA_Z_H, buffer.DATA_Z_L); + + if (tag_id == static_cast(FifoTag::GYRO_NC)) { + if (gyro.samples < (sizeof(gyro.x) / sizeof(gyro.x[0]))) { + gyro.x[gyro.samples] = data_x; + gyro.y[gyro.samples] = data_y; + gyro.z[gyro.samples] = data_z; + gyro.samples++; + } + + } else if (tag_id == static_cast(FifoTag::ACCEL_NC)) { + if (accel.samples < (sizeof(accel.x) / sizeof(accel.x[0]))) { + accel.x[accel.samples] = data_x; + accel.y[accel.samples] = data_y; + accel.z[accel.samples] = data_z; + accel.samples++; + } + + } else if (tag_id == static_cast(FifoTag::TEMPERATURE)) { + const int16_t temp_raw = combine(buffer.DATA_X_H, buffer.DATA_X_L); + const float temperature = (temp_raw / 256.0f) + 25.0f; + + if (PX4_ISFINITE(temperature)) { + _px4_accel.set_temperature(temperature); + _px4_gyro.set_temperature(temperature); + } + } + + // Other tags (TIMESTAMP, etc.) are silently ignored + } + + // Publish + if (gyro.samples > 0) { + _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + _px4_gyro.updateFIFO(gyro); + } + + if (accel.samples > 0) { + _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + _px4_accel.updateFIFO(accel); + } + + return (accel.samples > 0) && (gyro.samples > 0); +} + +void LSM6DSV::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // Switch to Bypass mode to flush FIFO + RegisterWrite(Register::FIFO_CTRL4, FIFO_CTRL4_BIT::FIFO_MODE_BYPASS); + + // Re-enable Continuous mode + RegisterWrite(Register::FIFO_CTRL4, FIFO_CTRL4_BIT::FIFO_MODE_CONTINUOUS); + + _drdy_timestamp_sample.store(0); +} + +void LSM6DSV::UpdateTemperature() +{ + struct TransferBuffer { + uint8_t cmd{static_cast(Register::OUT_TEMP_L) | DIR_READ}; + uint8_t OUT_TEMP_L{0}; + uint8_t OUT_TEMP_H{0}; + } buffer{}; + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return; + } + + // 256 LSB/°C, zero = 25°C + const int16_t OUT_TEMP = combine(buffer.OUT_TEMP_H, buffer.OUT_TEMP_L); + const float temperature = (OUT_TEMP / 256.0f) + 25.0f; + + if (PX4_ISFINITE(temperature)) { + _px4_accel.set_temperature(temperature); + _px4_gyro.set_temperature(temperature); + } +} + +int LSM6DSV::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void LSM6DSV::DataReady() +{ + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); +} + +bool LSM6DSV::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // INT1 defaults to active-high (H_LACTIVE=0), use rising edge + return px4_arch_gpiosetevent(_drdy_gpio, true, false, true, &DataReadyInterruptCallback, this) == 0; +} + +bool LSM6DSV::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +void LSM6DSV::ConfigureFIFOWatermark(uint8_t samples) +{ + // accel + gyro = 2 FIFO words per sample period + const uint8_t fifo_watermark = samples * 2; + + for (auto &r : _register_cfg) { + if (r.reg == Register::FIFO_CTRL1) { + r.set_bits = fifo_watermark; + r.clear_bits = static_cast(~fifo_watermark & 0xFF); + } + } +} diff --git a/src/drivers/imu/st/lsm6dsv/LSM6DSV.hpp b/src/drivers/imu/st/lsm6dsv/LSM6DSV.hpp new file mode 100644 index 0000000000..4b783f8bb4 --- /dev/null +++ b/src/drivers/imu/st/lsm6dsv/LSM6DSV.hpp @@ -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 +#include +#include +#include +#include +#include +#include +#include + +using namespace ST_LSM6DSV; + +class LSM6DSV : public device::SPI, public I2CSPIDriver +{ +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(GYRO_ODR)}; + static constexpr float ACCEL_RATE{static_cast(ACCEL_ODR)}; + + static constexpr int32_t FIFO_MAX_SAMPLES{static_cast(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 _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(_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(FIFO_CTRL3_BIT::BDR_GY_HAODR) | + static_cast(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() + }; +}; diff --git a/src/drivers/imu/st/lsm6dsv/ST_LSM6DSV_Registers.hpp b/src/drivers/imu/st/lsm6dsv/ST_LSM6DSV_Registers.hpp new file mode 100644 index 0000000000..ffd6bd4f10 --- /dev/null +++ b/src/drivers/imu/st/lsm6dsv/ST_LSM6DSV_Registers.hpp @@ -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 +#include + +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 diff --git a/src/drivers/imu/st/lsm6dsv/lsm6dsv_main.cpp b/src/drivers/imu/st/lsm6dsv/lsm6dsv_main.cpp new file mode 100644 index 0000000000..f0b4cbe469 --- /dev/null +++ b/src/drivers/imu/st/lsm6dsv/lsm6dsv_main.cpp @@ -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 +#include + +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; +}