From daaf8b61dc4146eee603b47d7c5e006d944bd1dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 12 Jan 2021 14:12:17 +0100 Subject: [PATCH] drivers: remove ism330dlc not used anymore --- src/drivers/drv_sensor.h | 1 - src/drivers/imu/st/CMakeLists.txt | 1 - src/drivers/imu/st/ism330dlc/CMakeLists.txt | 46 --- src/drivers/imu/st/ism330dlc/ISM330DLC.cpp | 342 ------------------ src/drivers/imu/st/ism330dlc/ISM330DLC.hpp | 120 ------ .../st/ism330dlc/ST_ISM330DLC_Registers.hpp | 210 ----------- .../imu/st/ism330dlc/ism330dlc_main.cpp | 117 ------ 7 files changed, 837 deletions(-) delete mode 100644 src/drivers/imu/st/ism330dlc/CMakeLists.txt delete mode 100644 src/drivers/imu/st/ism330dlc/ISM330DLC.cpp delete mode 100644 src/drivers/imu/st/ism330dlc/ISM330DLC.hpp delete mode 100644 src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp delete mode 100644 src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 1bbaf6dc6d..6fd8b3fdf3 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -126,7 +126,6 @@ #define DRV_GYR_DEVTYPE_BMI088 0x66 #define DRV_BARO_DEVTYPE_BMP388 0x67 #define DRV_BARO_DEVTYPE_DPS310 0x68 -#define DRV_IMU_DEVTYPE_ST_ISM330DLC 0x69 #define DRV_ACC_DEVTYPE_BMI088 0x6a #define DRV_OSD_DEVTYPE_ATXXXX 0x6b #define DRV_FLOW_DEVTYPE_PMW3901 0x6c diff --git a/src/drivers/imu/st/CMakeLists.txt b/src/drivers/imu/st/CMakeLists.txt index 8db918a9c0..73a9349572 100644 --- a/src/drivers/imu/st/CMakeLists.txt +++ b/src/drivers/imu/st/CMakeLists.txt @@ -31,5 +31,4 @@ # ############################################################################ -add_subdirectory(ism330dlc) add_subdirectory(lsm9ds1) diff --git a/src/drivers/imu/st/ism330dlc/CMakeLists.txt b/src/drivers/imu/st/ism330dlc/CMakeLists.txt deleted file mode 100644 index 8094a4c3f2..0000000000 --- a/src/drivers/imu/st/ism330dlc/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 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__ism330dlc - MAIN ism330dlc - SRCS - ism330dlc_main.cpp - ISM330DLC.cpp - ISM330DLC.hpp - ST_ISM330DLC_Registers.hpp - DEPENDS - drivers_accelerometer - drivers_gyroscope - px4_work_queue - ) diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp deleted file mode 100644 index 4222a7a0c2..0000000000 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.cpp +++ /dev/null @@ -1,342 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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 "ISM330DLC.hpp" - -using namespace time_literals; - -static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; } - -static constexpr uint32_t FIFO_INTERVAL{1000}; // 1000 us / 1000 Hz interval - -ISM330DLC::ISM330DLC(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio) : - SPI(DRV_IMU_DEVTYPE_ST_ISM330DLC, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _drdy_gpio(drdy_gpio), - _px4_accel(get_device_id(), rotation), - _px4_gyro(get_device_id(), rotation) -{ -} - -ISM330DLC::~ISM330DLC() -{ - perf_free(_interval_perf); - perf_free(_transfer_perf); - perf_free(_fifo_empty_perf); - perf_free(_fifo_overflow_perf); - perf_free(_fifo_reset_perf); - perf_free(_drdy_count_perf); - perf_free(_drdy_interval_perf); -} - -void ISM330DLC::exit_and_cleanup() -{ - if (_drdy_gpio != 0) { - // Disable data ready callback - //px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr); - - RegisterWrite(Register::INT1_CTRL, 0); - } - - I2CSPIDriverBase::exit_and_cleanup(); -} - -int ISM330DLC::probe() -{ - const uint8_t whoami = RegisterRead(Register::WHO_AM_I); - - if (whoami != ISM330DLC_WHO_AM_I) { - DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); - return PX4_ERROR; - } - - return PX4_OK; -} - -int ISM330DLC::init() -{ - int ret = SPI::init(); - - if (ret != PX4_OK) { - DEVICE_DEBUG("SPI::init failed (%i)", ret); - return ret; - } - - if (!Reset()) { - PX4_ERR("reset failed"); - return PX4_ERROR; - } - - Start(); - - return PX4_OK; -} - -bool ISM330DLC::Reset() -{ - // CTRL3_C: SW_RESET - // Note: When the FIFO is used, the IF_INC and BDU bits must be equal to 1. - RegisterSetBits(Register::CTRL3_C, CTRL3_C_BIT::BDU | CTRL3_C_BIT::IF_INC | CTRL3_C_BIT::SW_RESET); - usleep(50); // Wait 50 μs (or wait until the SW_RESET bit of the CTRL3_C register returns to 0). - - // Accelerometer configuration - // CTRL1_XL: Accelerometer 16 G range and ODR 6.66 kHz - RegisterWrite(Register::CTRL1_XL, CTRL1_XL_BIT::ODR_XL_6_66KHZ | CTRL1_XL_BIT::FS_XL_16); - _px4_accel.set_scale(0.488f * (CONSTANTS_ONE_G / 1000.0f)); // 0.488 mg/LSB - _px4_accel.set_range(16.0f * CONSTANTS_ONE_G); - - // Gyroscope configuration - // CTRL2_G: Gyroscope 2000 degrees/second and ODR 6.66 kHz - // CTRL6_C: Gyroscope low-pass filter bandwidth 937 Hz (maximum) - RegisterWrite(Register::CTRL2_G, CTRL2_G_BIT::ODR_G_6_66KHZ | CTRL2_G_BIT::FS_G_2000); - RegisterSetBits(Register::CTRL6_C, CTRL6_C_BIT::FTYPE_GYRO_LPF_BW_937_HZ); - _px4_gyro.set_scale(math::radians(70.0f / 1000.0f)); // 70 mdps/LSB - _px4_gyro.set_range(math::radians(2000.0f)); - - // reset is considered done once data is ready - const bool reset_done = ((RegisterRead(Register::CTRL3_C) & CTRL3_C_BIT::SW_RESET) == 0); - - return reset_done; -} - -void ISM330DLC::ResetFIFO() -{ - perf_count(_fifo_reset_perf); - - // FIFO_CTRL5 - disable FIFO - RegisterWrite(Register::FIFO_CTRL5, 0); - - // CTRL5_C: rounding mode gyro + accel - RegisterWrite(Register::CTRL5_C, CTRL5_C_BIT::ROUNDING_GYRO_ACCEL); - - // FIFO_CTRL3: full gyro and accel data to FIFO - RegisterWrite(Register::FIFO_CTRL3, FIFO_CTRL3_BIT::DEC_FIFO_GYRO | FIFO_CTRL3_BIT::DEC_FIFO_XL); - - // FIFO_CTRL5: FIFO ODR is set to 6.66 kHz, and FIFO continuous mode enabled - RegisterWrite(Register::FIFO_CTRL5, FIFO_CTRL5_BIT::ODR_FIFO_6_66_KHZ | FIFO_CTRL5_BIT::FIFO_MODE_CONTINUOUS); -} - -uint8_t ISM330DLC::RegisterRead(Register reg) -{ - uint8_t cmd[2] {}; - cmd[0] = static_cast(reg) | DIR_READ; - transfer(cmd, cmd, sizeof(cmd)); - return cmd[1]; -} - -void ISM330DLC::RegisterWrite(Register reg, uint8_t value) -{ - uint8_t cmd[2] { (uint8_t)reg, value }; - transfer(cmd, cmd, sizeof(cmd)); -} - -void ISM330DLC::RegisterSetBits(Register reg, uint8_t setbits) -{ - uint8_t val = RegisterRead(reg); - - if (!(val & setbits)) { - val |= setbits; - RegisterWrite(reg, val); - } -} - -void ISM330DLC::RegisterClearBits(Register reg, uint8_t clearbits) -{ - uint8_t val = RegisterRead(reg); - - if (val & clearbits) { - val &= !clearbits; - RegisterWrite(reg, val); - } -} - -int ISM330DLC::DataReadyInterruptCallback(int irq, void *context, void *arg) -{ - ISM330DLC *dev = reinterpret_cast(arg); - dev->DataReady(); - return 0; -} - -void ISM330DLC::DataReady() -{ - // _time_data_ready = hrt_absolute_time(); - - // perf_count(_drdy_count_perf); - // perf_count(_drdy_interval_perf); - - // make another measurement - ScheduleNow(); -} - -void ISM330DLC::Start() -{ - ResetFIFO(); - - if (_drdy_gpio != 0 && false) { // TODO: enable - // Setup data ready on rising edge - //px4_arch_gpiosetevent(_drdy_gpio, true, true, false, &ISM330DLC::DataReadyInterruptCallback, this); - - // FIFO threshold level setting - // FIFO_CTRL1: FTH_[7:0] - // FIFO_CTRL2: FTH_[10:8] - const uint8_t fifo_threshold = 12; - RegisterWrite(Register::FIFO_CTRL1, fifo_threshold); - - // INT1: FIFO full, overrun, or threshold - RegisterWrite(Register::INT1_CTRL, INT1_CTRL_BIT::INT1_FULL_FLAG | INT1_CTRL_BIT::INT1_FIFO_OVR | - INT1_CTRL_BIT::INT1_FTH); - - } else { - ScheduleOnInterval(FIFO_INTERVAL, FIFO_INTERVAL); - } -} - -void ISM330DLC::RunImpl() -{ - perf_count(_interval_perf); - - // use timestamp from the data ready interrupt if available, - // otherwise use the time now roughly corresponding with the last sample we'll pull from the FIFO - const hrt_abstime timestamp_sample = (hrt_elapsed_time(&_time_data_ready) < FIFO_INTERVAL) ? _time_data_ready : - hrt_absolute_time(); - - // Number of unread words (16-bit axes) stored in FIFO. - const uint8_t fifo_words = RegisterRead(Register::FIFO_STATUS1); - - // check for FIFO status - const uint8_t FIFO_STATUS2 = RegisterRead(Register::FIFO_STATUS2); - - if (FIFO_STATUS2 & FIFO_STATUS2_BIT::OVER_RUN) { - // overflow - perf_count(_fifo_overflow_perf); - ResetFIFO(); - return; - - } else if (FIFO_STATUS2 & FIFO_STATUS2_BIT::FIFO_EMPTY) { - // fifo empty could indicate a problem, reset - perf_count(_fifo_empty_perf); - ResetFIFO(); - return; - } - - // FIFO pattern: indicates Next reading from FIFO output registers (Gx, Gy, Gz, XLx, XLy, XLz) - const uint8_t fifo_pattern = RegisterRead(Register::FIFO_STATUS3); - - if (fifo_pattern != 0) { - PX4_DEBUG("check FIFO pattern: %d", fifo_pattern); - } - - // Transfer data - // only transfer out complete sets of gyro + accel - const int samples = (fifo_words / 2) / sizeof(FIFO::DATA); - - if (samples < 1) { - perf_count(_fifo_empty_perf); - return; - - } else if (samples > 16) { - // not technically an overflow, but more samples than we expected - perf_count(_fifo_overflow_perf); - ResetFIFO(); - return; - } - - FIFOTransferBuffer buffer{}; - const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); - - perf_begin(_transfer_perf); - - if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { - perf_end(_transfer_perf); - return; - } - - perf_end(_transfer_perf); - - sensor_accel_fifo_s accel{}; - accel.timestamp_sample = timestamp_sample; - accel.samples = samples; - accel.dt = 1000000 / ST_ISM330DLC::LA_ODR; - - sensor_gyro_fifo_s gyro{}; - gyro.timestamp_sample = timestamp_sample; - gyro.samples = samples; - gyro.dt = 1000000 / ST_ISM330DLC::G_ODR; - - for (int i = 0; i < samples; i++) { - const FIFO::DATA &fifo_sample = buffer.f[i]; - - // sensor Z is up (RHC), flip y & z for publication - gyro.x[i] = combine(fifo_sample.OUTX_L_G, fifo_sample.OUTX_H_G); - gyro.y[i] = -combine(fifo_sample.OUTY_L_G, fifo_sample.OUTY_H_G); - gyro.z[i] = -combine(fifo_sample.OUTZ_L_G, fifo_sample.OUTZ_H_G); - - accel.x[i] = combine(fifo_sample.OUTX_L_XL, fifo_sample.OUTX_H_XL); - accel.y[i] = -combine(fifo_sample.OUTY_L_XL, fifo_sample.OUTY_H_XL); - accel.z[i] = -combine(fifo_sample.OUTZ_L_XL, fifo_sample.OUTZ_H_XL); - } - - // get current temperature at 1 Hz - if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) { - uint8_t temperature_buf[3] {}; - temperature_buf[0] = static_cast(Register::OUT_TEMP_L) | DIR_READ; - - if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) { - return; - } - - // 16 bits in two’s complement format with a sensitivity of 256 LSB/°C. The output zero level corresponds to 25 °C. - const int16_t OUT_TEMP = combine(temperature_buf[1], temperature_buf[2]); - const float temperature = ((float)OUT_TEMP / 256.0f) + 25.0f; - - _px4_accel.set_temperature(temperature); - _px4_gyro.set_temperature(temperature); - } - - _px4_gyro.updateFIFO(gyro); - _px4_accel.updateFIFO(accel); -} - -void ISM330DLC::print_status() -{ - I2CSPIDriverBase::print_status(); - perf_print_counter(_interval_perf); - perf_print_counter(_transfer_perf); - perf_print_counter(_fifo_empty_perf); - perf_print_counter(_fifo_overflow_perf); - perf_print_counter(_fifo_reset_perf); - perf_print_counter(_drdy_count_perf); - perf_print_counter(_drdy_interval_perf); - -} diff --git a/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp b/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp deleted file mode 100644 index d4827a80b7..0000000000 --- a/src/drivers/imu/st/ism330dlc/ISM330DLC.hpp +++ /dev/null @@ -1,120 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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 ISM330DLC.hpp - * - * Driver for the ST ISM330DLC connected via SPI. - * - */ - -#pragma once - -#include "ST_ISM330DLC_Registers.hpp" - -#include -#include -#include -#include -#include -#include -#include - -using namespace ST_ISM330DLC; - -class ISM330DLC : public device::SPI, public I2CSPIDriver -{ -public: - ISM330DLC(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio); - ~ISM330DLC() override; - - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); - static void print_usage(); - - void print_status() override; - - void RunImpl(); - - int init() override; - - void Start(); - bool Reset(); - -protected: - void custom_method(const BusCLIArguments &cli) override; - void exit_and_cleanup() override; -private: - - // Sensor Configuration - static constexpr uint32_t GYRO_RATE{ST_ISM330DLC::G_ODR}; - static constexpr uint32_t ACCEL_RATE{ST_ISM330DLC::LA_ODR}; - static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; - - // Transfer data - struct FIFOTransferBuffer { - uint8_t cmd{static_cast(Register::FIFO_DATA_OUT_L) | DIR_READ}; - FIFO::DATA f[FIFO_MAX_SAMPLES] {}; - }; - // ensure no struct padding - static_assert(sizeof(FIFOTransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); - - int probe() override; - - static int DataReadyInterruptCallback(int irq, void *context, void *arg); - void DataReady(); - - uint8_t RegisterRead(Register reg); - void RegisterWrite(Register reg, uint8_t value); - void RegisterSetBits(Register reg, uint8_t setbits); - void RegisterClearBits(Register reg, uint8_t clearbits); - - void ResetFIFO(); - - const spi_drdy_gpio_t _drdy_gpio; - - PX4Accelerometer _px4_accel; - PX4Gyroscope _px4_gyro; - - perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")}; - perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": 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_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": drdy count")}; - perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")}; - - hrt_abstime _time_data_ready{0}; - hrt_abstime _time_last_temperature_update{0}; -}; diff --git a/src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp b/src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp deleted file mode 100644 index 25705b39cc..0000000000 --- a/src/drivers/imu/st/ism330dlc/ST_ISM330DLC_Registers.hpp +++ /dev/null @@ -1,210 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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_ISM330DLC_registers.hpp - * - * ST ISM330DLC registers. - * - */ - -#pragma once - -// TODO: move to a central header -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_ISM330DLC -{ - -static constexpr uint8_t DIR_READ = 0x80; - -static constexpr uint8_t ISM330DLC_WHO_AM_I = 0b01101010; // Who I am ID - -static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10 MHz SPI clock frequency - -static constexpr uint32_t LA_ODR = 6664; // Linear acceleration output data rate -static constexpr uint32_t G_ODR = 6664; // Angular rate output data rate - -enum class -Register : uint8_t { - FIFO_CTRL1 = 0x06, // FIFO threshold level setting. - - FIFO_CTRL3 = 0x08, // FIFO control register (r/w). - - FIFO_CTRL5 = 0x0A, - - INT1_CTRL = 0x0D, - INT2_CTRL = 0x0E, - WHO_AM_I = 0x0F, - - CTRL1_XL = 0x10, // Linear acceleration sensor control register 1 (r/w). - CTRL2_G = 0x11, // Angular rate sensor control register 2 (r/w). - CTRL3_C = 0x12, // Control register 3 (r/w). - CTRL4_C = 0x13, - CTRL5_C = 0x14, // Control register 5 (r/w). - CTRL6_C = 0x15, // Angular rate sensor control register 6 (r/w). - - OUT_TEMP_L = 0x20, - OUT_TEMP_H = 0x21, - - FIFO_STATUS1 = 0x3A, // FIFO status control register (r) - FIFO_STATUS2 = 0x3B, // FIFO status control register (r) - FIFO_STATUS3 = 0x3C, // FIFO status control register (r) - - FIFO_DATA_OUT_L = 0x3E, // FIFO data output (first byte) - FIFO_DATA_OUT_H = 0x3F, // FIFO data output (second byte) -}; - - -// FIFO_CTRL3 -enum -FIFO_CTRL3_BIT : uint8_t { - DEC_FIFO_GYRO = Bit3, // Gyro no decimation - DEC_FIFO_XL = Bit0, // Accel no decimation -}; - -// FIFO_CTRL5 -enum -FIFO_CTRL5_BIT : uint8_t { - ODR_FIFO_6_66_KHZ = Bit6 | Bit4, // FIFO ODR is set to 6.66 kHz - - FIFO_MODE_CONTINUOUS = Bit2 | Bit1, // Continuous mode. If the FIFO is full, the new sample overwrites the older one. - -}; - -// INT1_CTRL -enum -INT1_CTRL_BIT : uint8_t { - INT1_FULL_FLAG = Bit5, - INT1_FIFO_OVR = Bit4, - INT1_FTH = Bit3, - - INT1_DRDY_G = Bit1, - INT1_DRDY_XL = Bit0, -}; - -// INT2_CTRL -enum -INT2_CTRL_BIT : uint8_t { - INT2_FULL_FLAG = Bit5, - INT2_FIFO_OVR = Bit4, - INT2_FTH = Bit3, - - INT2_DRDY_G = Bit1, - INT2_DRDY_XL = Bit0, -}; - -// CTRL1_XL -enum -CTRL1_XL_BIT : uint8_t { - ODR_XL_6_66KHZ = Bit7 | Bit5, // 6.66 kHz Output data rate and power mode selection - - FS_XL_16 = Bit2, // FS_XL 01: ±16 g -}; - -// CTRL2_G -enum -CTRL2_G_BIT : uint8_t { - ODR_G_6_66KHZ = Bit7 | Bit5, - - FS_G_2000 = Bit3 | Bit2, -}; - -// CTRL3_C -enum -CTRL3_C_BIT : uint8_t { - BDU = Bit7, - - IF_INC = Bit2, - - SW_RESET = Bit0 -}; - -// CTRL4_C -enum -CTRL4_C_BIT : uint8_t { - INT2_on_INT1 = Bit5, -}; - -// CTRL5_C -enum -CTRL5_C_BIT : uint8_t { - ROUNDING_GYRO_ACCEL = Bit1 | Bit0, // ROUNDING[2:0] - 011 Gyroscope + accelerometer -}; - -// CTRL6_C -enum -CTRL6_C_BIT : uint8_t { - FTYPE_GYRO_LPF_BW_937_HZ = Bit1 | Bit0 -}; - -// FIFO_STATUS2 -enum -FIFO_STATUS2_BIT : uint8_t { - OVER_RUN = Bit6, - - FIFO_EMPTY = Bit4, -}; - -namespace FIFO -{ -static constexpr size_t SIZE = 4096; - -// Saving data in the FIFO buffer is organized in four FIFO data sets consisting of 6 bytes each -// each FIFO sample is composed of 16 bits -struct DATA { - uint8_t OUTX_L_G; - uint8_t OUTX_H_G; - uint8_t OUTY_L_G; - uint8_t OUTY_H_G; - uint8_t OUTZ_L_G; - uint8_t OUTZ_H_G; - - uint8_t OUTX_L_XL; - uint8_t OUTX_H_XL; - uint8_t OUTY_L_XL; - uint8_t OUTY_H_XL; - uint8_t OUTZ_L_XL; - uint8_t OUTZ_H_XL; -}; -static_assert(sizeof(DATA) == 12); -} - -} // namespace ST_ISM330DLC diff --git a/src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp b/src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp deleted file mode 100644 index e797b6f815..0000000000 --- a/src/drivers/imu/st/ism330dlc/ism330dlc_main.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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 "ISM330DLC.hpp" - -#include -#include - -void -ISM330DLC::print_usage() -{ - PRINT_MODULE_USAGE_NAME("ism330dlc", "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_COMMAND("reset"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - -I2CSPIDriverBase *ISM330DLC::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - ISM330DLC *instance = new ISM330DLC(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); - - if (!instance) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - -void ISM330DLC::custom_method(const BusCLIArguments &cli) -{ - Reset(); -} - -extern "C" int ism330dlc_main(int argc, char *argv[]) -{ - int ch; - using ThisDriver = ISM330DLC; - BusCLIArguments cli{false, true}; - cli.default_spi_frequency = ST_ISM330DLC::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_ISM330DLC); - - 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); - } - - if (!strcmp(verb, "reset")) { - return ThisDriver::module_custom_method(cli, iterator); - } - - ThisDriver::print_usage(); - return -1; -}