diff --git a/boards/airmind/mindpx-v2/default.px4board b/boards/airmind/mindpx-v2/default.px4board index 7b52a99b64..b00b612d6b 100644 --- a/boards/airmind/mindpx-v2/default.px4board +++ b/boards/airmind/mindpx-v2/default.px4board @@ -16,8 +16,9 @@ CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU6500=y -CONFIG_DRIVERS_IMU_L3GD20=y -CONFIG_DRIVERS_IMU_LSM303D=y +CONFIG_DRIVERS_IMU_ST_L3GD20=y +CONFIG_DRIVERS_IMU_ST_L3GD20H=y +CONFIG_DRIVERS_IMU_ST_LSM303D=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y @@ -42,6 +43,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y @@ -50,6 +52,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y @@ -65,7 +68,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y -CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y @@ -74,10 +76,9 @@ CONFIG_MODULES_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y -CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_VTOL_ATT_CONTROL=y -CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_GPIO=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y diff --git a/boards/airmind/mindpx-v2/init/rc.board_sensors b/boards/airmind/mindpx-v2/init/rc.board_sensors index 30fca7a7ed..cd37d41481 100644 --- a/boards/airmind/mindpx-v2/init/rc.board_sensors +++ b/boards/airmind/mindpx-v2/init/rc.board_sensors @@ -11,8 +11,12 @@ qmc5883l -I -R 6 start mpu6000 -s -R 2 start mpu6500 -s -R 2 start -lsm303d -s -R 10 start -l3gd20 -s -R 14 start + +lsm303d -s -R 2 start +if ! l3gd20 -s -R 2 start +then + l3gd20h -s -R 2 start +fi # Internal SPI ms5611 -s start diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index d188db8ec7..752b9430ab 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -11,8 +11,8 @@ CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y -CONFIG_DRIVERS_IMU_L3GD20=y -CONFIG_DRIVERS_IMU_LSM303D=y +CONFIG_DRIVERS_IMU_ST_L3GD20=y +CONFIG_DRIVERS_IMU_ST_LSM303D=y CONFIG_DRIVERS_LIGHTS_RGBLED=y CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/px4/fmu-v2/init/rc.board_sensors b/boards/px4/fmu-v2/init/rc.board_sensors index 8aecc96c34..5943c02e4b 100644 --- a/boards/px4/fmu-v2/init/rc.board_sensors +++ b/boards/px4/fmu-v2/init/rc.board_sensors @@ -11,70 +11,65 @@ board_adc start # Internal I2C bus hmc5883 -T -I -R 4 start -# Internal SPI (auto detect ms5611 or ms5607) -if ! ms5611 -T 5607 -s -b 1 start +# V2 +if ver hwtypecmp V2 then + # SPI1 + mpu6000 -s -b 1 -R 14 start + + lsm303d -s -b 1 -R 8 start + + if ! l3gd20 -s -b 1 -R 14 start + then + l3gd20h -s -b 1 -R 12 start + fi + + # SPI1 (auto detect ms5611 or ms5607) + if ! ms5611 -T 5607 -s -b 1 start + then + ms5611 -s -b 1 start + fi +fi + + +# V2M Pixhawk mini +if ver hwtypecmp V2M +then + # SPI1 + icm20608g -s -b 1 -R 14 start + + #mpu9250 -s -b 1 -R 14 start # unused mpu9250 + ms5611 -s -b 1 start fi -set BOARD_FMUV3 0 # V3 build hwtypecmp supports V2|V2M|V30 if ver hwtypecmp V30 -then - # Check for Pixhawk 2.0 cube (isolated IMU on SPI4) - # mpu6000 on v2.0, mpu9250 on v2.1 - if mpu6000 -s -b 4 -R 10 start - then - set BOARD_FMUV3 20 - else - # Check for Pixhawk 2.1 cube - # isolated/external mpu9250 (SPI4) - if mpu9250 -s -b 4 -R 10 start - then - set BOARD_FMUV3 21 - fi - fi -fi - -# Check if a Pixhack (which reports as V2M) is present -if ver hwtypecmp V2M -then - # Internal SPI bus ICM-20608-G - icm20608g -s -b 1 -R 14 start - - # Pixhawk Mini doesn't have these sensors, - # so if they are found we know its a Pixhack - - # mppu6000 internal (SPI1) - if mpu6000 -s -b 1 -R 10 start - then - set BOARD_FMUV3 20 - else - # Check for Pixhack 3.1 - # mpu9250 external (SPI4) - if mpu9250 -s -b 1 -R 10 start - then - set BOARD_FMUV3 21 - fi - fi -fi - -if [ $BOARD_FMUV3 != 0 ] then # sensor heating is available, but we disable it for now param set-default SENS_EN_THERMAL 0 - # l3gd20 (external/isolated SPI4) - l3gd20 -s -b 4 -R 4 start + # SPI 4 (external/isolated) + if mpu9250 -s -b 4 -R 10 start + then + mpu6000 -s -b 4 -R 10 start + fi - # lsm303d (external/isolated SPI4) - lsm303d -s -b 4 -R 6 start - - # ms5611 (external/isolated SPI4) ms5611 -s -b 4 start - if [ $BOARD_FMUV3 = 20 ] + # l3gd20 (external/isolated SPI4) + if ! l3gd20 -s -b 4 -R 14 start + then + l3gd20h -s -b 4 -R 14 start + fi + + # lsm303d (external/isolated SPI4) + lsm303d -s -b 4 -R 14 start + + + # SPI1 internal mpu9250 (v2.1) or mpu6000 (v2.0) + if ! mpu9250 -s -b 1 start then # v2.0 internal mpu6000 mpu6000 -s -b 1 start @@ -83,28 +78,10 @@ then hmc5883 -T -s -b 1 -R 8 start fi - if [ $BOARD_FMUV3 = 21 ] + # SPI1 Internal SPI (auto detect ms5611 or ms5607) + if ! ms5611 -T 5607 -s -b 1 start then - # v2.1 mpu9250 on SPI1 - mpu9250 -s -b 1 start + ms5611 -s -b 1 start fi - -else - # $BOARD_FMUV3 = 0 -> FMUv2 - - mpu6000 -s -b 1 -R 14 start - - # As we will use the external mag and the ICM-20608-G - # V2 build hwtypecmp is always false - # V3 build hwtypecmp supports V2|V2M|V30 - if ! ver hwtypecmp V2M - then - mpu9250 -s -b 1 -R 14 start - # else: On the PixhawkMini the mpu9250 has been disabled due to HW errata - fi - - l3gd20 -s -b 1 start - lsm303d -s -b 1 start fi -unset BOARD_FMUV3 diff --git a/boards/px4/fmu-v3/default.px4board b/boards/px4/fmu-v3/default.px4board index 7d2c17bea3..fae18e9fd1 100644 --- a/boards/px4/fmu-v3/default.px4board +++ b/boards/px4/fmu-v3/default.px4board @@ -21,8 +21,9 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y -CONFIG_DRIVERS_IMU_L3GD20=y -CONFIG_DRIVERS_IMU_LSM303D=y +CONFIG_DRIVERS_IMU_ST_L3GD20=y +CONFIG_DRIVERS_IMU_ST_L3GD20H=y +CONFIG_DRIVERS_IMU_ST_LSM303D=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y @@ -45,6 +46,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y @@ -53,6 +55,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y @@ -68,7 +71,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y -CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y @@ -77,10 +79,9 @@ CONFIG_MODULES_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y -CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_VTOL_ATT_CONTROL=y -CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_GPIO=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y diff --git a/boards/px4/fmu-v3/init/rc.board_sensors b/boards/px4/fmu-v3/init/rc.board_sensors index 33e5698d76..a807ab7fa7 100644 --- a/boards/px4/fmu-v3/init/rc.board_sensors +++ b/boards/px4/fmu-v3/init/rc.board_sensors @@ -11,70 +11,56 @@ board_adc start # Internal I2C bus hmc5883 -T -I -R 4 start -# Internal SPI (auto detect ms5611 or ms5607) -if ! ms5611 -T 5607 -s -b 1 start +# V2 +if ver hwtypecmp V2 then + # SPI1 + mpu6000 -s -b 1 -R 14 start + + lsm303d -s -b 1 -R 8 start + + if ! l3gd20 -s -b 1 -R 14 start + then + l3gd20h -s -b 1 -R 12 start + fi + + # SPI1 (auto detect ms5611 or ms5607) + if ! ms5611 -T 5607 -s -b 1 -q start + then + ms5611 -s -b 1 start + fi +fi + + +# V2M Pixhawk mini +if ver hwtypecmp V2M +then + # SPI1 + icm20608g -s -b 1 -R 14 start + + #mpu9250 -s -b 1 -R 14 start # unused mpu9250 + ms5611 -s -b 1 start fi -set BOARD_FMUV3 0 # V3 build hwtypecmp supports V2|V2M|V30 if ver hwtypecmp V30 -then - # Check for Pixhawk 2.0 cube (isolated IMU on SPI4) - # mpu6000 on v2.0, mpu9250 on v2.1 - if mpu6000 -s -b 4 -R 10 start - then - set BOARD_FMUV3 20 - else - # Check for Pixhawk 2.1 cube - # isolated/external mpu9250 (SPI4) - if mpu9250 -s -b 4 -R 10 start - then - set BOARD_FMUV3 21 - fi - fi -fi - -# Check if a Pixhack (which reports as V2M) is present -if ver hwtypecmp V2M -then - # Internal SPI bus ICM-20608-G - icm20608g -s -b 1 -R 14 start - - # Pixhawk Mini doesn't have these sensors, - # so if they are found we know its a Pixhack - - # mppu6000 internal (SPI1) - if mpu6000 -s -b 1 -R 10 start - then - set BOARD_FMUV3 20 - else - # Check for Pixhack 3.1 - # mpu9250 external (SPI4) - if mpu9250 -s -b 1 -R 10 start - then - set BOARD_FMUV3 21 - fi - fi -fi - -if [ $BOARD_FMUV3 != 0 ] then # sensor heating is available, but we disable it for now param set-default SENS_EN_THERMAL 0 - # l3gd20 (external/isolated SPI4) - l3gd20 -s -b 4 -R 4 start + # SPI4 (external/isolated) + if ! mpu9250 -s -b 4 -R 10 start + then + mpu6000 -s -b 4 -R 10 start + fi - # lsm303d (external/isolated SPI4) - lsm303d -s -b 4 -R 6 start - - # ms5611 (external/isolated SPI4) ms5611 -s -b 4 start - if [ $BOARD_FMUV3 = 20 ] + + # SPI1 internal mpu9250 (v2.1) or mpu6000 (v2.0) + if ! mpu9250 -s -b 1 start then # v2.0 internal mpu6000 mpu6000 -s -b 1 start @@ -83,28 +69,20 @@ then hmc5883 -T -s -b 1 -R 8 start fi - if [ $BOARD_FMUV3 = 21 ] + # SPI1 Internal SPI (auto detect ms5611 or ms5607) + if ! ms5611 -T 5607 -s -b 1 start then - # v2.1 mpu9250 on SPI1 - mpu9250 -s -b 1 start + ms5611 -s -b 1 start fi -else - # $BOARD_FMUV3 = 0 -> FMUv2 - mpu6000 -s -b 1 -R 14 start - - # As we will use the external mag and the ICM-20608-G - # V2 build hwtypecmp is always false - # V3 build hwtypecmp supports V2|V2M|V30 - if ! ver hwtypecmp V2M + # SPI4 l3gd20 (external/isolated SPI4) + if ! l3gd20 -s -b 4 -R 10 start then - mpu9250 -s -b 1 -R 14 start - # else: On the PixhawkMini the mpu9250 has been disabled due to HW errata + l3gd20h -s -b 4 -R 14 start fi - l3gd20 -s -b 1 start - lsm303d -s -b 1 start + # SPI4 lsm303d (external/isolated SPI4) + lsm303d -s -b 4 -R 14 start fi -unset BOARD_FMUV3 diff --git a/boards/px4/fmu-v3/src/spi.cpp b/boards/px4/fmu-v3/src/spi.cpp index e28958d3ba..a4ba34bb6a 100644 --- a/boards/px4/fmu-v3/src/spi.cpp +++ b/boards/px4/fmu-v3/src/spi.cpp @@ -150,7 +150,8 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), - initSPIDevice(DRV_GYR_DEVTYPE_L3GD20, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}), + initSPIDevice(DRV_GYR_DEVTYPE_L3GD20, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}), + initSPIDevice(DRV_GYR_DEVTYPE_L3GD20H, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortB, GPIO::Pin0}), initSPIDevice(DRV_IMU_DEVTYPE_LSM303D, SPI::CS{GPIO::PortC, GPIO::Pin15}), initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortD, GPIO::Pin7}), initSPIDevice(DRV_BARO_DEVTYPE_MS5607, SPI::CS{GPIO::PortD, GPIO::Pin7}), @@ -178,7 +179,8 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIBus(SPI::Bus::SPI4, { initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin4}), initSPIDevice(DRV_IMU_DEVTYPE_MPU9250, SPI::CS{GPIO::PortE, GPIO::Pin4}), - initSPIDevice(DRV_GYR_DEVTYPE_L3GD20, SPI::CS{GPIO::PortC, GPIO::Pin13}), + initSPIDevice(DRV_GYR_DEVTYPE_L3GD20, SPI::CS{GPIO::PortC, GPIO::Pin13}), + initSPIDevice(DRV_GYR_DEVTYPE_L3GD20H, SPI::CS{GPIO::PortC, GPIO::Pin13}), initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortC, GPIO::Pin14}), initSPIDevice(DRV_IMU_DEVTYPE_LSM303D, SPI::CS{GPIO::PortC, GPIO::Pin15}), }), diff --git a/platforms/common/include/px4_platform_common/spi.h b/platforms/common/include/px4_platform_common/spi.h index b9e56bb900..da305efa41 100644 --- a/platforms/common/include/px4_platform_common/spi.h +++ b/platforms/common/include/px4_platform_common/spi.h @@ -50,7 +50,7 @@ typedef uint32_t spi_drdy_gpio_t; -#define SPI_BUS_MAX_DEVICES 6 +#define SPI_BUS_MAX_DEVICES 7 struct px4_spi_bus_device_t { uint32_t cs_gpio; ///< chip-select GPIO (0 if this device is not used) spi_drdy_gpio_t drdy_gpio; ///< data ready GPIO (0 if not set) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 796ca1e538..184b67b769 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -70,6 +70,7 @@ #define DRV_IMU_DEVTYPE_MPU6000 0x21 #define DRV_GYR_DEVTYPE_L3GD20 0x22 +#define DRV_GYR_DEVTYPE_L3GD20H 0x23 #define DRV_IMU_DEVTYPE_MPU9250 0x24 #define DRV_IMU_DEVTYPE_ICM20649 0x25 #define DRV_IMU_DEVTYPE_ICM42688P 0x26 diff --git a/src/drivers/imu/Kconfig b/src/drivers/imu/Kconfig index 9a1de9995a..62c1762650 100644 --- a/src/drivers/imu/Kconfig +++ b/src/drivers/imu/Kconfig @@ -21,8 +21,6 @@ menu "IMU" select DRIVERS_IMU_INVENSENSE_MPU6000 select DRIVERS_IMU_INVENSENSE_MPU6500 select DRIVERS_IMU_INVENSENSE_MPU9250 - select DRIVERS_IMU_L3GD20 - select DRIVERS_IMU_LSM303D select DRIVERS_IMU_ST ---help--- Enable default set of IMU drivers diff --git a/src/drivers/imu/l3gd20/Kconfig b/src/drivers/imu/l3gd20/Kconfig deleted file mode 100644 index 417becfc95..0000000000 --- a/src/drivers/imu/l3gd20/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig DRIVERS_IMU_L3GD20 - bool "l3gd20" - default n - ---help--- - Enable support for l3gd20 \ No newline at end of file diff --git a/src/drivers/imu/l3gd20/L3GD20.cpp b/src/drivers/imu/l3gd20/L3GD20.cpp deleted file mode 100644 index 152673048c..0000000000 --- a/src/drivers/imu/l3gd20/L3GD20.cpp +++ /dev/null @@ -1,440 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 "L3GD20.hpp" - -constexpr uint8_t L3GD20::_checked_registers[]; - -L3GD20::L3GD20(const I2CSPIDriverConfig &config) : - SPI(config), - I2CSPIDriver(config), - _px4_gyro(get_device_id(), config.rotation), - _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), - _errors(perf_alloc(PC_COUNT, MODULE_NAME": err")), - _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")), - _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": dupe")) -{ -} - -L3GD20::~L3GD20() -{ - perf_free(_sample_perf); - perf_free(_errors); - perf_free(_bad_registers); - perf_free(_duplicates); -} - -int -L3GD20::init() -{ - /* do SPI init (and probe) first */ - if (SPI::init() != OK) { - return PX4_ERROR; - } - - reset(); - - start(); - - return PX4_OK; -} - -int -L3GD20::probe() -{ - /* read dummy value to void to clear SPI statemachine on sensor */ - read_reg(ADDR_WHO_AM_I); - - bool success = false; - uint8_t v = 0; - - /* verify that the device is attached and functioning, accept - * L3GD20, L3GD20H and L3G4200D */ - if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) { - _orientation = SENSOR_BOARD_ROTATION_DEFAULT; - success = true; - - } else if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) { - _orientation = SENSOR_BOARD_ROTATION_180_DEG; - success = true; - - } else if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) { - /* Detect the L3G4200D used on AeroCore */ - _is_l3g4200d = true; - _orientation = SENSOR_BOARD_ROTATION_DEFAULT; - success = true; - } - - if (success) { - _checked_values[0] = v; - return OK; - } - - return -EIO; -} - -uint8_t -L3GD20::read_reg(unsigned reg) -{ - uint8_t cmd[2] {}; - - cmd[0] = reg | DIR_READ; - cmd[1] = 0; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; -} - -int -L3GD20::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2] {}; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - return transfer(cmd, nullptr, sizeof(cmd)); -} - -void -L3GD20::write_checked_reg(unsigned reg, uint8_t value) -{ - write_reg(reg, value); - - for (uint8_t i = 0; i < L3GD20_NUM_CHECKED_REGISTERS; i++) { - if (reg == _checked_registers[i]) { - _checked_values[i] = value; - } - } -} - -void -L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_checked_reg(reg, val); -} - -int -L3GD20::set_range(unsigned max_dps) -{ - uint8_t bits = REG4_BDU; - float new_range_scale_dps_digit; - - if (max_dps == 0) { - max_dps = 2000; - } - - if (max_dps <= 250) { - //new_range = 250; - bits |= RANGE_250DPS; - new_range_scale_dps_digit = 8.75e-3f; - - } else if (max_dps <= 500) { - //new_range = 500; - bits |= RANGE_500DPS; - new_range_scale_dps_digit = 17.5e-3f; - - } else if (max_dps <= 2000) { - //new_range = 2000; - bits |= RANGE_2000DPS; - new_range_scale_dps_digit = 70e-3f; - - } else { - return -EINVAL; - } - - _px4_gyro.set_scale(new_range_scale_dps_digit / 180.0f * M_PI_F); - - write_checked_reg(ADDR_CTRL_REG4, bits); - - return OK; -} - -int -L3GD20::set_samplerate(unsigned frequency) -{ - uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE; - - if (frequency == 0) { - frequency = _is_l3g4200d ? 800 : 760; - } - - /* - * Use limits good for H or non-H models. Rates are slightly different - * for L3G4200D part but register settings are the same. - */ - if (frequency <= 100) { - bits |= RATE_95HZ_LP_25HZ; - - } else if (frequency <= 200) { - bits |= RATE_190HZ_LP_50HZ; - - } else if (frequency <= 400) { - bits |= RATE_380HZ_LP_50HZ; - - } else if (frequency <= 800) { - bits |= RATE_760HZ_LP_50HZ; - - } else { - return -EINVAL; - } - - write_checked_reg(ADDR_CTRL_REG1, bits); - - return OK; -} - -void -L3GD20::start() -{ - /* start polling at the specified rate */ - uint64_t interval = 1000000 / L3GD20_DEFAULT_RATE; - ScheduleOnInterval(interval - L3GD20_TIMER_REDUCTION, 10000); -} - -void -L3GD20::disable_i2c() -{ - uint8_t retries = 10; - - while (retries--) { - // add retries - uint8_t a = read_reg(0x05); - write_reg(0x05, (0x20 | a)); - - if (read_reg(0x05) == (a | 0x20)) { - // this sets the I2C_DIS bit on the - // L3GD20H. The l3gd20 datasheet doesn't - // mention this register, but it does seem to - // accept it. - write_checked_reg(ADDR_LOW_ODR, 0x08); - return; - } - } - - DEVICE_DEBUG("FAILED TO DISABLE I2C"); -} - -void -L3GD20::reset() -{ - // ensure the chip doesn't interpret any other bus traffic as I2C - disable_i2c(); - - /* set default configuration */ - write_checked_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); - write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_checked_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ - write_checked_reg(ADDR_CTRL_REG4, REG4_BDU); - write_checked_reg(ADDR_CTRL_REG5, 0); - write_checked_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */ - - /* disable FIFO. This makes things simpler and ensures we - * aren't getting stale data. It means we must run the hrt - * callback fast enough to not miss data. */ - write_checked_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - - set_samplerate(0); // 760Hz or 800Hz - set_range(L3GD20_DEFAULT_RANGE_DPS); - - _read = 0; -} - -void -L3GD20::check_registers() -{ - uint8_t v; - - if ((v = read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) { - /* - if we get the wrong value then we know the SPI bus - or sensor is very sick. We set _register_wait to 20 - and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. - */ - perf_count(_bad_registers); - - /* - try to fix the bad register value. We only try to - fix one per loop to prevent a bad sensor hogging the - bus. We skip zero as that is the WHO_AM_I, which - is not writeable - */ - if (_checked_next != 0) { - write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); - } - - _register_wait = 20; - } - - _checked_next = (_checked_next + 1) % L3GD20_NUM_CHECKED_REGISTERS; -} - -void -L3GD20::RunImpl() -{ - /* status register and data as read back from the device */ -#pragma pack(push, 1) - struct { - uint8_t cmd; - int8_t temp; - uint8_t status; - int16_t x; - int16_t y; - int16_t z; - } raw_report{}; -#pragma pack(pop) - - /* start the performance counter */ - perf_begin(_sample_perf); - - check_registers(); - - /* fetch data from the sensor */ - const hrt_abstime timestamp_sample = hrt_absolute_time(); - raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; - transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); - - if (!(raw_report.status & STATUS_ZYXDA)) { - perf_end(_sample_perf); - perf_count(_duplicates); - return; - } - - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - _px4_gyro.set_error_count(perf_event_count(_bad_registers)); - - _px4_gyro.set_temperature(L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp); - - switch (_orientation) { - case SENSOR_BOARD_ROTATION_090_DEG: - /* swap x and y */ - _px4_gyro.update(timestamp_sample, raw_report.y, raw_report.x, raw_report.z); - break; - - case SENSOR_BOARD_ROTATION_180_DEG: { - /* swap x and y and negate both */ - int16_t x = ((raw_report.x == -32768) ? 32767 : -raw_report.x); - int16_t y = ((raw_report.y == -32768) ? 32767 : -raw_report.y); - _px4_gyro.update(timestamp_sample, x, y, raw_report.z); - } - - break; - - case SENSOR_BOARD_ROTATION_270_DEG: { - /* swap x and y and negate y */ - int16_t x = raw_report.y; - int16_t y = ((raw_report.x == -32768) ? 32767 : -raw_report.x); - _px4_gyro.update(timestamp_sample, x, y, raw_report.z); - } - break; - - case SENSOR_BOARD_ROTATION_000_DEG: - - // FALLTHROUGH - default: - // keep axes in place - _px4_gyro.update(timestamp_sample, raw_report.x, raw_report.y, raw_report.z); - } - - _read++; - - /* stop the perf counter */ - perf_end(_sample_perf); -} - -void -L3GD20::print_status() -{ - I2CSPIDriverBase::print_status(); - printf("gyro reads: %u\n", _read); - perf_print_counter(_sample_perf); - perf_print_counter(_errors); - perf_print_counter(_bad_registers); - perf_print_counter(_duplicates); - - ::printf("checked_next: %u\n", _checked_next); - - for (uint8_t i = 0; i < L3GD20_NUM_CHECKED_REGISTERS; i++) { - uint8_t v = read_reg(_checked_registers[i]); - - if (v != _checked_values[i]) { - ::printf("reg %02x:%02x should be %02x\n", - (unsigned)_checked_registers[i], - (unsigned)v, - (unsigned)_checked_values[i]); - } - } - -} - -void -L3GD20::print_registers() -{ - printf("L3GD20 registers\n"); - - for (uint8_t reg = 0; reg <= 0x40; reg++) { - uint8_t v = read_reg(reg); - printf("%02x:%02x ", (unsigned)reg, (unsigned)v); - - if ((reg + 1) % 16 == 0) { - printf("\n"); - } - } - - printf("\n"); -} - -void -L3GD20::test_error() -{ - // trigger a deliberate error - write_reg(ADDR_CTRL_REG3, 0); -} diff --git a/src/drivers/imu/l3gd20/L3GD20.hpp b/src/drivers/imu/l3gd20/L3GD20.hpp deleted file mode 100644 index 71bd532681..0000000000 --- a/src/drivers/imu/l3gd20/L3GD20.hpp +++ /dev/null @@ -1,286 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 l3gd20.cpp - * Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI. - * - * Note: With the exception of the self-test feature, the ST L3G4200D is - * also supported by this driver. - */ - -#include -#include -#include -#include -#include -#include - -/* Orientation on board */ -#define SENSOR_BOARD_ROTATION_000_DEG 0 -#define SENSOR_BOARD_ROTATION_090_DEG 1 -#define SENSOR_BOARD_ROTATION_180_DEG 2 -#define SENSOR_BOARD_ROTATION_270_DEG 3 - -/* SPI protocol address bits */ -#define DIR_READ (1<<7) -#define DIR_WRITE (0<<7) -#define ADDR_INCREMENT (1<<6) - -/* register addresses */ -#define ADDR_WHO_AM_I 0x0F -#define WHO_I_AM_H 0xD7 -#define WHO_I_AM 0xD4 -#define WHO_I_AM_L3G4200D 0xD3 /* for L3G4200D */ - -#define ADDR_CTRL_REG1 0x20 -#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ - -/* keep lowpass low to avoid noise issues */ -#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) -#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) -#define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4)) -#define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) -#define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) -#define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4)) -#define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) -#define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) -#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) -#define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4)) -#define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4)) -#define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) - -#define ADDR_CTRL_REG2 0x21 -#define ADDR_CTRL_REG3 0x22 -#define ADDR_CTRL_REG4 0x23 -#define REG4_RANGE_MASK 0x30 /* Mask to guard partial register update */ -#define RANGE_250DPS (0<<4) -#define RANGE_500DPS (1<<4) -#define RANGE_2000DPS (3<<4) - -#define ADDR_CTRL_REG5 0x24 -#define ADDR_REFERENCE 0x25 -#define ADDR_OUT_TEMP 0x26 -#define ADDR_STATUS_REG 0x27 -#define ADDR_OUT_X_L 0x28 -#define ADDR_OUT_X_H 0x29 -#define ADDR_OUT_Y_L 0x2A -#define ADDR_OUT_Y_H 0x2B -#define ADDR_OUT_Z_L 0x2C -#define ADDR_OUT_Z_H 0x2D -#define ADDR_FIFO_CTRL_REG 0x2E -#define ADDR_FIFO_SRC_REG 0x2F -#define ADDR_INT1_CFG 0x30 -#define ADDR_INT1_SRC 0x31 -#define ADDR_INT1_TSH_XH 0x32 -#define ADDR_INT1_TSH_XL 0x33 -#define ADDR_INT1_TSH_YH 0x34 -#define ADDR_INT1_TSH_YL 0x35 -#define ADDR_INT1_TSH_ZH 0x36 -#define ADDR_INT1_TSH_ZL 0x37 -#define ADDR_INT1_DURATION 0x38 -#define ADDR_LOW_ODR 0x39 - - -/* Internal configuration values */ -#define REG1_POWER_NORMAL (1<<3) -#define REG1_Z_ENABLE (1<<2) -#define REG1_Y_ENABLE (1<<1) -#define REG1_X_ENABLE (1<<0) - -#define REG4_BDU (1<<7) -#define REG4_BLE (1<<6) -//#define REG4_SPI_3WIRE (1<<0) - -#define REG5_FIFO_ENABLE (1<<6) -#define REG5_REBOOT_MEMORY (1<<7) - -#define STATUS_ZYXOR (1<<7) -#define STATUS_ZOR (1<<6) -#define STATUS_YOR (1<<5) -#define STATUS_XOR (1<<4) -#define STATUS_ZYXDA (1<<3) -#define STATUS_ZDA (1<<2) -#define STATUS_YDA (1<<1) -#define STATUS_XDA (1<<0) - -#define FIFO_CTRL_BYPASS_MODE (0<<5) -#define FIFO_CTRL_FIFO_MODE (1<<5) -#define FIFO_CTRL_STREAM_MODE (1<<6) -#define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5) -#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7) - -#define L3GD20_DEFAULT_RATE 760 -#define L3GD20_DEFAULT_RANGE_DPS 2000 -#define L3GD20_TEMP_OFFSET_CELSIUS 40 - -#define L3GD20_MAX_OFFSET 0.45f /**< max offset: 25 degrees/s */ - -#ifndef SENSOR_BOARD_ROTATION_DEFAULT -#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG -#endif - -/* - we set the timer interrupt to run a bit faster than the desired - sample rate and then throw away duplicates using the data ready bit. - This time reduction is enough to cope with worst case timing jitter - due to other timers - */ -#define L3GD20_TIMER_REDUCTION 600 - -class L3GD20 : public device::SPI, public I2CSPIDriver -{ -public: - L3GD20(const I2CSPIDriverConfig &config); - ~L3GD20() override; - - static void print_usage(); - - void RunImpl(); - - int init() override; - - void print_status() override; - - // print register dump - void print_registers(); - - // trigger an error - void test_error(); - -protected: - void custom_method(const BusCLIArguments &cli) override; - int probe() override; - -private: - - PX4Gyroscope _px4_gyro; - - unsigned _orientation{SENSOR_BOARD_ROTATION_DEFAULT}; - - unsigned _read{0}; - - perf_counter_t _sample_perf; - perf_counter_t _errors; - perf_counter_t _bad_registers; - perf_counter_t _duplicates; - - uint8_t _register_wait{0}; - - /* true if an L3G4200D is detected */ - bool _is_l3g4200d{false}; - - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset - static constexpr int L3GD20_NUM_CHECKED_REGISTERS{8}; - static constexpr uint8_t _checked_registers[] = { - ADDR_WHO_AM_I, - ADDR_CTRL_REG1, - ADDR_CTRL_REG2, - ADDR_CTRL_REG3, - ADDR_CTRL_REG4, - ADDR_CTRL_REG5, - ADDR_FIFO_CTRL_REG, - ADDR_LOW_ODR - }; - - uint8_t _checked_values[L3GD20_NUM_CHECKED_REGISTERS] {}; - uint8_t _checked_next{0}; - - void start(); - void reset(); - void disable_i2c(); - - /** - * check key registers for correct values - */ - void check_registers(); - - /** - * Read a register from the L3GD20 - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg) override; - - /** - * Write a register in the L3GD20 - * - * @param reg The register to write. - * @param value The new value to write. - * @return OK on success, negative errno otherwise. - */ - int write_reg(unsigned reg, uint8_t value) override; - - /** - * Modify a register in the L3GD20 - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Write a register in the L3GD20, updating _checked_values - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_checked_reg(unsigned reg, uint8_t value); - - /** - * Set the L3GD20 measurement range. - * - * @param max_dps The measurement range is set to permit reading at least - * this rate in degrees per second. - * Zero selects the maximum supported range. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int set_range(unsigned max_dps); - - /** - * Set the L3GD20 internal sampling frequency. - * - * @param frequency The internal sampling frequency is set to not less than - * this value. - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int set_samplerate(unsigned frequency); - -}; diff --git a/src/drivers/imu/lsm303d/Kconfig b/src/drivers/imu/lsm303d/Kconfig deleted file mode 100644 index 49c139db4f..0000000000 --- a/src/drivers/imu/lsm303d/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig DRIVERS_IMU_LSM303D - bool "lsm303d" - default n - ---help--- - Enable support for lsm303d \ No newline at end of file diff --git a/src/drivers/imu/lsm303d/LSM303D.cpp b/src/drivers/imu/lsm303d/LSM303D.cpp deleted file mode 100644 index 7a43f9f0a7..0000000000 --- a/src/drivers/imu/lsm303d/LSM303D.cpp +++ /dev/null @@ -1,589 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-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 LSM303D.cpp - * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. - */ - -#include "LSM303D.hpp" - -/* - list of registers that will be checked in check_registers(). Note - that ADDR_WHO_AM_I must be first in the list. - */ -static constexpr uint8_t _checked_registers[] = { - ADDR_WHO_AM_I, - ADDR_CTRL_REG1, - ADDR_CTRL_REG2, - ADDR_CTRL_REG3, - ADDR_CTRL_REG4, - ADDR_CTRL_REG5, - ADDR_CTRL_REG6, - ADDR_CTRL_REG7 -}; - -LSM303D::LSM303D(const I2CSPIDriverConfig &config) : - SPI(config), - I2CSPIDriver(config), - _px4_accel(get_device_id(), config.rotation), - _px4_mag(get_device_id(), config.rotation), - _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: acc_read")), - _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d: mag_read")), - _bad_registers(perf_alloc(PC_COUNT, "lsm303d: bad_reg")), - _bad_values(perf_alloc(PC_COUNT, "lsm303d: bad_val")), - _accel_duplicates(perf_alloc(PC_COUNT, "lsm303d: acc_dupe")) -{ -} - -LSM303D::~LSM303D() -{ - // delete the perf counter - perf_free(_accel_sample_perf); - perf_free(_mag_sample_perf); - perf_free(_bad_registers); - perf_free(_bad_values); - perf_free(_accel_duplicates); -} - -int -LSM303D::init() -{ - /* do SPI init (and probe) first */ - int ret = SPI::init(); - - if (ret != OK) { - DEVICE_DEBUG("SPI init failed (%i)", ret); - return ret; - } - - reset(); - - start(); - - return ret; -} - -void -LSM303D::disable_i2c(void) -{ - uint8_t a = read_reg(0x02); - write_reg(0x02, (0x10 | a)); - a = read_reg(0x02); - write_reg(0x02, (0xF7 & a)); - a = read_reg(0x15); - write_reg(0x15, (0x80 | a)); - a = read_reg(0x02); - write_reg(0x02, (0xE7 & a)); -} - -void -LSM303D::reset() -{ - // ensure the chip doesn't interpret any other bus traffic as I2C - disable_i2c(); - - // enable accel - write_checked_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | - REG1_RATE_800HZ_A); - - // enable mag - write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M | REG5_ENABLE_T); - write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 - write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 - - accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); - accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); - - // we setup the anti-alias on-chip filter as 50Hz. We believe - // this operates in the analog domain, and is critical for - // anti-aliasing. The 2 pole software filter is designed to - // operate in conjunction with this on-chip filter - accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); - - mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); - mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); -} - -int -LSM303D::probe() -{ - // read dummy value to void to clear SPI statemachine on sensor - read_reg(ADDR_WHO_AM_I); - - // verify that the device is attached and functioning - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { - _checked_values[0] = WHO_I_AM; - return OK; - } - - return -EIO; -} - -uint8_t -LSM303D::read_reg(unsigned reg) -{ - uint8_t cmd[2] {}; - cmd[0] = reg | DIR_READ; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; -} - -int -LSM303D::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2] {}; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - return transfer(cmd, nullptr, sizeof(cmd)); -} - -void -LSM303D::write_checked_reg(unsigned reg, uint8_t value) -{ - write_reg(reg, value); - - for (uint8_t i = 0; i < LSM303D_NUM_CHECKED_REGISTERS; i++) { - if (reg == _checked_registers[i]) { - _checked_values[i] = value; - } - } -} - -void -LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_checked_reg(reg, val); -} - -int -LSM303D::accel_set_range(unsigned max_g) -{ - uint8_t setbits = 0; - uint8_t clearbits = REG2_FULL_SCALE_BITS_A; - float new_scale_g_digit = 0.0f; - - if (max_g == 0) { - max_g = 16; - } - - if (max_g <= 2) { - // accel_range_m_s2 = 2.0f * CONSTANTS_ONE_G; - setbits |= REG2_FULL_SCALE_2G_A; - new_scale_g_digit = 0.061e-3f; - - } else if (max_g <= 4) { - // accel_range_m_s2 = 4.0f * CONSTANTS_ONE_G; - setbits |= REG2_FULL_SCALE_4G_A; - new_scale_g_digit = 0.122e-3f; - - } else if (max_g <= 6) { - // accel_range_m_s2 = 6.0f * CONSTANTS_ONE_G; - setbits |= REG2_FULL_SCALE_6G_A; - new_scale_g_digit = 0.183e-3f; - - } else if (max_g <= 8) { - // accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G; - setbits |= REG2_FULL_SCALE_8G_A; - new_scale_g_digit = 0.244e-3f; - - } else if (max_g <= 16) { - // accel_range_m_s2 = 16.0f * CONSTANTS_ONE_G; - setbits |= REG2_FULL_SCALE_16G_A; - new_scale_g_digit = 0.732e-3f; - - } else { - return -EINVAL; - } - - float accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G; - - _px4_accel.set_scale(accel_range_scale); - - modify_reg(ADDR_CTRL_REG2, clearbits, setbits); - - return OK; -} - -int -LSM303D::mag_set_range(unsigned max_ga) -{ - uint8_t setbits = 0; - uint8_t clearbits = REG6_FULL_SCALE_BITS_M; - float new_scale_ga_digit = 0.0f; - - if (max_ga == 0) { - max_ga = 12; - } - - if (max_ga <= 2) { - // mag_range_ga = 2; - setbits |= REG6_FULL_SCALE_2GA_M; - new_scale_ga_digit = 0.080e-3f; - - } else if (max_ga <= 4) { - // mag_range_ga = 4; - setbits |= REG6_FULL_SCALE_4GA_M; - new_scale_ga_digit = 0.160e-3f; - - } else if (max_ga <= 8) { - // mag_range_ga = 8; - setbits |= REG6_FULL_SCALE_8GA_M; - new_scale_ga_digit = 0.320e-3f; - - } else if (max_ga <= 12) { - // mag_range_ga = 12; - setbits |= REG6_FULL_SCALE_12GA_M; - new_scale_ga_digit = 0.479e-3f; - - } else { - return -EINVAL; - } - - _px4_mag.set_scale(new_scale_ga_digit); - - modify_reg(ADDR_CTRL_REG6, clearbits, setbits); - - return OK; -} - -int -LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth) -{ - uint8_t setbits = 0; - uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; - - if (bandwidth == 0) { - bandwidth = 773; - } - - if (bandwidth <= 50) { - // accel_onchip_filter_bandwith = 50; - setbits |= REG2_AA_FILTER_BW_50HZ_A; - - } else if (bandwidth <= 194) { - // accel_onchip_filter_bandwith = 194; - setbits |= REG2_AA_FILTER_BW_194HZ_A; - - } else if (bandwidth <= 362) { - // accel_onchip_filter_bandwith = 362; - setbits |= REG2_AA_FILTER_BW_362HZ_A; - - } else if (bandwidth <= 773) { - // accel_onchip_filter_bandwith = 773; - setbits |= REG2_AA_FILTER_BW_773HZ_A; - - } else { - return -EINVAL; - } - - modify_reg(ADDR_CTRL_REG2, clearbits, setbits); - - return OK; -} - -int -LSM303D::accel_set_samplerate(unsigned frequency) -{ - uint8_t setbits = 0; - uint8_t clearbits = REG1_RATE_BITS_A; - - if (frequency == 0) { - frequency = 1600; - } - - int accel_samplerate = 100; - - if (frequency <= 100) { - setbits |= REG1_RATE_100HZ_A; - accel_samplerate = 100; - - } else if (frequency <= 200) { - setbits |= REG1_RATE_200HZ_A; - accel_samplerate = 200; - - } else if (frequency <= 400) { - setbits |= REG1_RATE_400HZ_A; - accel_samplerate = 400; - - } else if (frequency <= 800) { - setbits |= REG1_RATE_800HZ_A; - accel_samplerate = 800; - - } else if (frequency <= 1600) { - setbits |= REG1_RATE_1600HZ_A; - accel_samplerate = 1600; - - } else { - return -EINVAL; - } - - _call_accel_interval = 1000000 / accel_samplerate; - - modify_reg(ADDR_CTRL_REG1, clearbits, setbits); - - return OK; -} - -int -LSM303D::mag_set_samplerate(unsigned frequency) -{ - uint8_t setbits = 0; - uint8_t clearbits = REG5_RATE_BITS_M; - - if (frequency == 0) { - frequency = 100; - } - - int mag_samplerate = 100; - - if (frequency <= 25) { - setbits |= REG5_RATE_25HZ_M; - mag_samplerate = 25; - - } else if (frequency <= 50) { - setbits |= REG5_RATE_50HZ_M; - mag_samplerate = 50; - - } else if (frequency <= 100) { - setbits |= REG5_RATE_100HZ_M; - mag_samplerate = 100; - - } else { - return -EINVAL; - } - - _call_mag_interval = 1000000 / mag_samplerate; - - modify_reg(ADDR_CTRL_REG5, clearbits, setbits); - - return OK; -} - -void -LSM303D::start() -{ - // start polling at the specified rate - ScheduleOnInterval(_call_accel_interval - LSM303D_TIMER_REDUCTION); -} - -void -LSM303D::RunImpl() -{ - // make another accel measurement - measureAccelerometer(); - - if (hrt_elapsed_time(&_mag_last_measure) >= _call_mag_interval) { - measureMagnetometer(); - } -} - -void -LSM303D::check_registers(void) -{ - uint8_t v = 0; - - if ((v = read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) { - /* - if we get the wrong value then we know the SPI bus - or sensor is very sick. We set _register_wait to 20 - and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. - */ - perf_count(_bad_registers); - - /* - try to fix the bad register value. We only try to - fix one per loop to prevent a bad sensor hogging the - bus. We skip zero as that is the WHO_AM_I, which - is not writeable - */ - if (_checked_next != 0) { - write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); - } - - _register_wait = 20; - } - - _checked_next = (_checked_next + 1) % LSM303D_NUM_CHECKED_REGISTERS; -} - -void -LSM303D::measureAccelerometer() -{ - perf_begin(_accel_sample_perf); - - // status register and data as read back from the device -#pragma pack(push, 1) - struct { - uint8_t cmd; - uint8_t status; - int16_t x; - int16_t y; - int16_t z; - } raw_accel_report{}; -#pragma pack(pop) - - check_registers(); - - if (_register_wait != 0) { - // we are waiting for some good transfers before using - // the sensor again. - _register_wait--; - perf_end(_accel_sample_perf); - return; - } - - /* fetch data from the sensor */ - const hrt_abstime timestamp_sample = hrt_absolute_time(); - raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; - transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); - - if (!(raw_accel_report.status & REG_STATUS_A_NEW_ZYXADA)) { - perf_end(_accel_sample_perf); - perf_count(_accel_duplicates); - return; - } - - /* - we have logs where the accelerometers get stuck at a fixed - large value. We want to detect this and mark the sensor as - being faulty - */ - if (((_last_accel[0] - raw_accel_report.x) == 0) && - ((_last_accel[1] - raw_accel_report.y) == 0) && - ((_last_accel[2] - raw_accel_report.z) == 0)) { - - _constant_accel_count++; - - } else { - _constant_accel_count = 0; - } - - if (_constant_accel_count > 100) { - // we've had 100 constant accel readings with large - // values. The sensor is almost certainly dead. We - // will raise the error_count so that the top level - // flight code will know to avoid this sensor, but - // we'll still give the data so that it can be logged - // and viewed - perf_count(_bad_values); - - _constant_accel_count = 0; - - perf_end(_accel_sample_perf); - return; - } - - // report the error count as the sum of the number of bad - // register reads and bad values. This allows the higher level - // code to decide if it should use this sensor based on - // whether it has had failures - _px4_accel.set_error_count(perf_event_count(_bad_registers) + perf_event_count(_bad_values)); - _px4_accel.update(timestamp_sample, raw_accel_report.x, raw_accel_report.y, raw_accel_report.z); - - _last_accel[0] = raw_accel_report.x; - _last_accel[1] = raw_accel_report.y; - _last_accel[2] = raw_accel_report.z; - - perf_end(_accel_sample_perf); -} - -void -LSM303D::measureMagnetometer() -{ - perf_begin(_mag_sample_perf); - - // status register and data as read back from the device -#pragma pack(push, 1) - struct { - uint8_t cmd; - int16_t temperature; - uint8_t status; - int16_t x; - int16_t y; - int16_t z; - } raw_mag_report{}; -#pragma pack(pop) - - // fetch data from the sensor - const hrt_abstime timestamp_sample = hrt_absolute_time(); - raw_mag_report.cmd = ADDR_OUT_TEMP_L | DIR_READ | ADDR_INCREMENT; - transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); - - /* remember the temperature. The datasheet isn't clear, but it - * seems to be a signed offset from 25 degrees C in units of 0.125C - */ - _last_temperature = 25.0f + (raw_mag_report.temperature * 0.125f); - _px4_accel.set_temperature(_last_temperature); - _px4_mag.set_temperature(_last_temperature); - - _px4_mag.set_error_count(perf_event_count(_bad_registers) + perf_event_count(_bad_values)); - _px4_mag.update(timestamp_sample, raw_mag_report.x, raw_mag_report.y, raw_mag_report.z); - - _mag_last_measure = timestamp_sample; - - perf_end(_mag_sample_perf); -} - -void -LSM303D::print_status() -{ - I2CSPIDriverBase::print_status(); - perf_print_counter(_accel_sample_perf); - perf_print_counter(_mag_sample_perf); - perf_print_counter(_bad_registers); - perf_print_counter(_bad_values); - perf_print_counter(_accel_duplicates); - - ::printf("checked_next: %u\n", _checked_next); - - for (uint8_t i = 0; i < LSM303D_NUM_CHECKED_REGISTERS; i++) { - uint8_t v = read_reg(_checked_registers[i]); - - if (v != _checked_values[i]) { - ::printf("reg %02x:%02x should be %02x\n", - (unsigned)_checked_registers[i], - (unsigned)v, - (unsigned)_checked_values[i]); - } - } -} diff --git a/src/drivers/imu/lsm303d/LSM303D.hpp b/src/drivers/imu/lsm303d/LSM303D.hpp deleted file mode 100644 index 62d3de5122..0000000000 --- a/src/drivers/imu/lsm303d/LSM303D.hpp +++ /dev/null @@ -1,294 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-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 LSM303D.hpp - * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -/* SPI protocol address bits */ -#define DIR_READ (1<<7) -#define DIR_WRITE (0<<7) -#define ADDR_INCREMENT (1<<6) - -/* register addresses: A: accel, M: mag, T: temp */ -#define ADDR_WHO_AM_I 0x0F -#define WHO_I_AM 0x49 - -#define ADDR_OUT_TEMP_L 0x05 - -#define ADDR_STATUS_A 0x27 - -#define ADDR_CTRL_REG0 0x1F -#define ADDR_CTRL_REG1 0x20 -#define ADDR_CTRL_REG2 0x21 -#define ADDR_CTRL_REG3 0x22 -#define ADDR_CTRL_REG4 0x23 -#define ADDR_CTRL_REG5 0x24 -#define ADDR_CTRL_REG6 0x25 -#define ADDR_CTRL_REG7 0x26 - -#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4)) -#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) -#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) -#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4)) -#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4)) -#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4)) -#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) -#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) -#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) -#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) -#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) -#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) - -#define REG1_BDU_UPDATE (1<<3) -#define REG1_Z_ENABLE_A (1<<2) -#define REG1_Y_ENABLE_A (1<<1) -#define REG1_X_ENABLE_A (1<<0) - -#define REG2_ANTIALIAS_FILTER_BW_BITS_A ((1<<7) | (1<<6)) -#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6)) -#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6)) -#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6)) -#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6)) - -#define REG2_FULL_SCALE_BITS_A ((1<<5) | (1<<4) | (1<<3)) -#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3)) -#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3)) -#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3)) -#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3)) -#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3)) - -#define REG5_ENABLE_T (1<<7) - -#define REG5_RES_HIGH_M ((1<<6) | (1<<5)) -#define REG5_RES_LOW_M ((0<<6) | (0<<5)) - -#define REG5_RATE_BITS_M ((1<<4) | (1<<3) | (1<<2)) -#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2)) -#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2)) -#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2)) -#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2)) -#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2)) -#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) -#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) - -#define REG6_FULL_SCALE_BITS_M ((1<<6) | (1<<5)) -#define REG6_FULL_SCALE_2GA_M ((0<<6) | (0<<5)) -#define REG6_FULL_SCALE_4GA_M ((0<<6) | (1<<5)) -#define REG6_FULL_SCALE_8GA_M ((1<<6) | (0<<5)) -#define REG6_FULL_SCALE_12GA_M ((1<<6) | (1<<5)) - -#define REG7_CONT_MODE_M ((0<<1) | (0<<0)) - -#define REG_STATUS_A_NEW_ZYXADA 0x08 - -/* default values for this device */ -#define LSM303D_ACCEL_DEFAULT_RANGE_G 16 -#define LSM303D_ACCEL_DEFAULT_RATE 800 -#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 - -#define LSM303D_MAG_DEFAULT_RANGE_GA 2 -#define LSM303D_MAG_DEFAULT_RATE 100 - -/* - we set the timer interrupt to run a bit faster than the desired - sample rate and then throw away duplicates using the data ready bit. - This time reduction is enough to cope with worst case timing jitter - due to other timers - */ -#define LSM303D_TIMER_REDUCTION 200 - -class LSM303D : public device::SPI, public I2CSPIDriver -{ -public: - LSM303D(const I2CSPIDriverConfig &config); - ~LSM303D() override; - - static void print_usage(); - - void RunImpl(); - - int init() override; - - void print_status() override; - -protected: - int probe() override; -private: - - void start(); - void reset(); - - /** - * disable I2C on the chip - */ - void disable_i2c(); - - /** - * check key registers for correct values - */ - void check_registers(void); - - /** - * Fetch accel measurements from the sensor and update the report ring. - */ - void measureAccelerometer(); - - /** - * Fetch mag measurements from the sensor and update the report ring. - */ - void measureMagnetometer(); - - /** - * Read a register from the LSM303D - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg) override; - - /** - * Write a register in the LSM303D - * - * @param reg The register to write. - * @param value The new value to write. - * @return OK on success, negative errno otherwise. - */ - int write_reg(unsigned reg, uint8_t value) override; - - /** - * Modify a register in the LSM303D - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Write a register in the LSM303D, updating _checked_values - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_checked_reg(unsigned reg, uint8_t value); - - /** - * Set the LSM303D accel measurement range. - * - * @param max_g The measurement range of the accel is in g (9.81m/s^2) - * Zero selects the maximum supported range. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int accel_set_range(unsigned max_g); - - /** - * Set the LSM303D mag measurement range. - * - * @param max_ga The measurement range of the mag is in Ga - * Zero selects the maximum supported range. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int mag_set_range(unsigned max_g); - - /** - * Set the LSM303D on-chip anti-alias filter bandwith. - * - * @param bandwidth The anti-alias filter bandwidth in Hz - * Zero selects the highest bandwidth - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); - - /** - * Set the LSM303D internal accel sampling frequency. - * - * @param frequency The internal accel sampling frequency is set to not less than - * this value. - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int accel_set_samplerate(unsigned frequency); - - /** - * Set the LSM303D internal mag sampling frequency. - * - * @param frequency The internal mag sampling frequency is set to not less than - * this value. - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int mag_set_samplerate(unsigned frequency); - - - PX4Accelerometer _px4_accel; - PX4Magnetometer _px4_mag; - - unsigned _call_accel_interval{1000000 / LSM303D_ACCEL_DEFAULT_RATE}; - unsigned _call_mag_interval{1000000 / LSM303D_MAG_DEFAULT_RATE}; - - perf_counter_t _accel_sample_perf; - perf_counter_t _mag_sample_perf; - perf_counter_t _bad_registers; - perf_counter_t _bad_values; - perf_counter_t _accel_duplicates; - - uint8_t _register_wait{0}; - - int16_t _last_accel[3] {}; - uint8_t _constant_accel_count{0}; - - hrt_abstime _mag_last_measure{0}; - - float _last_temperature{0.0f}; - - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset - static constexpr int LSM303D_NUM_CHECKED_REGISTERS{8}; - uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS] {}; - uint8_t _checked_next{0}; - -}; diff --git a/src/drivers/imu/l3gd20/CMakeLists.txt b/src/drivers/imu/st/l3gd20/CMakeLists.txt similarity index 92% rename from src/drivers/imu/l3gd20/CMakeLists.txt rename to src/drivers/imu/st/l3gd20/CMakeLists.txt index fd1e672436..b009d2bfba 100644 --- a/src/drivers/imu/l3gd20/CMakeLists.txt +++ b/src/drivers/imu/st/l3gd20/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2022 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 @@ -30,15 +30,17 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__imu__l3gd20 + MODULE drivers__imu__st__l3gd20 MAIN l3gd20 COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable + #-DDEBUG_BUILD SRCS - L3GD20.hpp - L3GD20.cpp l3gd20_main.cpp + L3GD20.cpp + L3GD20.hpp + ST_L3GD20_registers.hpp DEPENDS drivers_gyroscope px4_work_queue diff --git a/src/drivers/imu/st/l3gd20/Kconfig b/src/drivers/imu/st/l3gd20/Kconfig new file mode 100644 index 0000000000..063c494801 --- /dev/null +++ b/src/drivers/imu/st/l3gd20/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_ST_L3GD20 + bool "l3gd20" + default n + ---help--- + Enable support for l3gd20 diff --git a/src/drivers/imu/st/l3gd20/L3GD20.cpp b/src/drivers/imu/st/l3gd20/L3GD20.cpp new file mode 100644 index 0000000000..057587fc70 --- /dev/null +++ b/src/drivers/imu/st/l3gd20/L3GD20.cpp @@ -0,0 +1,457 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 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 "L3GD20.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +L3GD20::L3GD20(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_gyro(get_device_id(), config.rotation) +{ + if (_drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); + } + + ConfigureSampleRate(ST_L3GD20::ODR); // default to max ODR +} + +L3GD20::~L3GD20() +{ + 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 L3GD20::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool L3GD20::Reset() +{ + _state = STATE::RESET; + DataReadyInterruptDisable(); + ScheduleClear(); + ScheduleNow(); + return true; +} + +void L3GD20::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void L3GD20::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + + 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 L3GD20::probe() +{ + const uint8_t whoami = RegisterRead(Register::WHO_AM_I); + + if (whoami != WHOAMI) { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; + } + + return PX4_OK; +} + +void L3GD20::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // CTRL_REG5 + RegisterSetBits(Register::CTRL_REG5, CTRL_REG5_BIT::BOOT); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(150_ms); + break; + + case STATE::WAIT_FOR_RESET: + if (RegisterRead(Register::WHO_AM_I) == WHOAMI) { + // if reset succeeded then configure + RegisterSetBits(Register::CTRL_REG1, CTRL_REG1_BIT::PD); + _state = STATE::CONFIGURE; + ScheduleDelayed(50_ms); // Turn-on time 50 ms + + } else { + // RESET not complete + 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()) { + // if configure succeeded then start reading from FIFO + _state = STATE::FIFO_READ; + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // default to max ODR with data ready available + ConfigureSampleRate(ST_L3GD20::ODR); + + // backup schedule as a watchdog timeout + ScheduleDelayed(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + + // default to half of max ODR if no data ready available + ConfigureSampleRate(ST_L3GD20::ODR / 2); + + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + FIFOReset(); + + } else { + // CONFIGURE not complete + 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_READ: { + hrt_abstime timestamp_sample = now; + + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + 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); + } + + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } + + bool success = false; + + // always check FIFO status and count + const uint8_t FIFO_SRC_REG = RegisterRead(Register::FIFO_SRC_REG); + const uint8_t samples = FIFO_SRC_REG & FIFO_SRC_REG_BIT::FSS; // FSS4-FSS0 FIFO stored data level + + if ((FIFO_SRC_REG & FIFO_SRC_REG_BIT::OVRN) || (samples > FIFO_MAX_SAMPLES)) { + // not necessarily an overflow, but more samples than we expected or can publish + perf_count(_fifo_overflow_perf); + FIFOReset(); + + } else if ((FIFO_SRC_REG & FIFO_SRC_REG_BIT::EMPTY) || (samples == 0)) { + perf_count(_fifo_empty_perf); + + } else { + if (FIFORead(timestamp_sample, samples)) { + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 100) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + + } else { + // periodically update temperature (~1 Hz) + if (now >= _temperature_update_timestamp + 1_s) { + UpdateTemperature(); + _temperature_update_timestamp = now; + } + } + } + + break; + } +} + +void L3GD20::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = ST_L3GD20::ODR; // default to max ODR + } + + // round down to nearest FIFO sample dt + 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_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES)); + + // recompute FIFO empty interval (us) with actual gyro sample limit + _fifo_empty_interval_us = _fifo_samples * (1e6f / GYRO_RATE); +} + +bool L3GD20::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + _px4_gyro.set_scale(math::radians(70.f * 1e-3f)); // 70 mdps/LSB (FS = 2000 dps) + _px4_gyro.set_range(math::radians(2000.f)); // ±2000 dps + + return success; +} + +int L3GD20::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void L3GD20::DataReady() +{ + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); +} + +bool L3GD20::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, true, false, false, &DataReadyInterruptCallback, this) == 0; +} + +bool L3GD20::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool L3GD20::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 L3GD20::RegisterRead(Register reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +void L3GD20::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +void L3GD20::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 L3GD20::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) +{ + FIFOTransferBuffer buffer{}; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_count(_bad_transfer_perf); + return false; + } + + ProcessGyro(timestamp_sample, buffer.f, samples); + + return true; +} + +void L3GD20::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // FIFO_CTRL_REG: switch to bypass mode to restart data collection + RegisterClearBits(Register::FIFO_CTRL_REG, FIFO_CTRL_REG_BIT::Bypass_mode); + + // reset while FIFO is disabled + _drdy_timestamp_sample.store(0); + + // FIFO_CTRL_REG: mode + watermark + for (auto &r : _register_cfg) { + if (r.reg == Register::FIFO_CTRL_REG) { + RegisterSetAndClearBits(Register::FIFO_CTRL_REG, r.set_bits, r.clear_bits); + } + } +} + +void L3GD20::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) +{ + sensor_gyro_fifo_s gyro{}; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = samples; + gyro.dt = FIFO_SAMPLE_DT; + + for (int i = 0; i < samples; i++) { + int16_t gyro_x = combine(fifo[i].OUT_X_H, fifo[i].OUT_X_L); + int16_t gyro_y = combine(fifo[i].OUT_Y_H, fifo[i].OUT_Y_L); + int16_t gyro_z = combine(fifo[i].OUT_Z_H, fifo[i].OUT_Z_L); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + gyro.x[i] = gyro_x; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); + } + + _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); +} + +void L3GD20::UpdateTemperature() +{ + // Temperature data (-1LSB/deg with 8 bit resolution). The value is expressed as two’s complement. + const int8_t OUT_TEMP = RegisterRead(Register::OUT_TEMP); + + const float temperature_c = (OUT_TEMP / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET; + + if (PX4_ISFINITE(temperature_c) + && (temperature_c >= TEMPERATURE_SENSOR_MIN) + && (temperature_c <= TEMPERATURE_SENSOR_MAX)) { + + _px4_gyro.set_temperature(temperature_c); + } + +} diff --git a/src/drivers/imu/st/l3gd20/L3GD20.hpp b/src/drivers/imu/st/l3gd20/L3GD20.hpp new file mode 100644 index 0000000000..d04bd861c5 --- /dev/null +++ b/src/drivers/imu/st/l3gd20/L3GD20.hpp @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 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 L3GD20.hpp + * + * Driver for the ST L3GD20 connected via SPI. + * + */ + +#pragma once + +#include "ST_L3GD20_registers.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace ST_L3GD20; + +class L3GD20 : public device::SPI, public I2CSPIDriver +{ +public: + L3GD20(const I2CSPIDriverConfig &config); + ~L3GD20() 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 / ST_L3GD20::ODR}; + static constexpr float GYRO_RATE{ST_L3GD20::ODR}; + + static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; + + // Transfer data + struct FIFOTransferBuffer { + uint8_t cmd{static_cast(Register::OUT_X_L) | DIR_READ | AUTO_INCREMENT}; + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // ensure no struct padding + static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + 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); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + 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); + void RegisterSetBits(Register reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); } + void RegisterClearBits(Register reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); } + + bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); + void FIFOReset(); + + void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); + void UpdateTemperature(); + + const spi_drdy_gpio_t _drdy_gpio; + + 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_READ, + } _state{STATE::RESET}; + + uint16_t _fifo_empty_interval_us{1000}; + uint32_t _fifo_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{5}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::CTRL_REG1, CTRL_REG1_BIT::ODR_760HZ_CUTOFF_100HZ | CTRL_REG1_BIT::PD | CTRL_REG1_BIT::Zen | CTRL_REG1_BIT::Yen | CTRL_REG1_BIT::Xen, 0 }, + { Register::CTRL_REG3, CTRL_REG3_BIT::I2_DRDY, CTRL_REG3_BIT::I2_WTM }, + { Register::CTRL_REG4, CTRL_REG4_BIT::BDU | CTRL_REG4_BIT::FS_2000DPS, 0 }, + { Register::CTRL_REG5, CTRL_REG5_BIT::FIFO_EN, 0 }, + { Register::FIFO_CTRL_REG, FIFO_CTRL_REG_BIT::FIFO_mode_SET, FIFO_CTRL_REG_BIT::FIFO_mode_CLEAR }, + }; +}; diff --git a/src/drivers/imu/st/l3gd20/ST_L3GD20_registers.hpp b/src/drivers/imu/st/l3gd20/ST_L3GD20_registers.hpp new file mode 100644 index 0000000000..d2b0f0a6cb --- /dev/null +++ b/src/drivers/imu/st/l3gd20/ST_L3GD20_registers.hpp @@ -0,0 +1,158 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 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_L3GD20_registers.hpp + * + * ST L3GD20 registers. + * + */ + +#pragma once + +#include + +// 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_L3GD20 +{ +static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface + +static constexpr uint8_t DIR_READ = Bit7; +static constexpr uint8_t AUTO_INCREMENT = Bit6; + +static constexpr uint8_t WHOAMI = 0b11010100; // Who I am ID 0x0F + +static constexpr uint32_t ODR = 760; // Digital output data rate (ODR) + +static constexpr float TEMPERATURE_SENSITIVITY = -1.f; // LSB/C +static constexpr float TEMPERATURE_OFFSET = 40.f; // C +static constexpr float TEMPERATURE_SENSOR_MIN = -40.f; // °C +static constexpr float TEMPERATURE_SENSOR_MAX = 85.f; // °C + +enum class Register : uint8_t { + WHO_AM_I = 0x0F, + + CTRL_REG1 = 0x20, + CTRL_REG2 = 0x21, + CTRL_REG3 = 0x22, + CTRL_REG4 = 0x23, + CTRL_REG5 = 0x24, + + OUT_TEMP = 0x26, + STATUS_REG = 0x27, + OUT_X_L = 0x28, + OUT_X_H = 0x29, + OUT_Y_L = 0x2A, + OUT_Y_H = 0x2B, + OUT_Z_L = 0x2C, + OUT_Z_H = 0x2D, + FIFO_CTRL_REG = 0x2E, + FIFO_SRC_REG = 0x2F, +}; + +// CTRL_REG1 +enum CTRL_REG1_BIT : uint8_t { + // DR1 DR0 BW1 BW0 [7-4] + ODR_760HZ_CUTOFF_100HZ = Bit7 | Bit6 | Bit5 | Bit4, + PD = Bit3, // Power-down mode enable (1: normal mode or sleep mode) + Zen = Bit2, // Z axis enable + Yen = Bit1, // Y axis enable + Xen = Bit0, // X axis enable +}; + +// CTRL_REG3 +enum CTRL_REG3_BIT : uint8_t { + I2_DRDY = Bit3, // Date-ready on DRDY/INT2. Default value 0. (0: disable; 1: enable) + I2_WTM = Bit2, // FIFO watermark interrupt on DRDY/INT2. Default value: 0. (0: disable; 1: enable) +}; + +// CTRL_REG4 +enum CTRL_REG4_BIT : uint8_t { + BDU = Bit7, // Block data update + + // FS1-FS0 + FS_2000DPS = Bit5 | Bit4, // Full scale selection 2000 dps +}; + +// CTRL_REG5 +enum CTRL_REG5_BIT : uint8_t { + BOOT = Bit7, // Reboot memory content + FIFO_EN = Bit6, +}; + +// FIFO_CTRL_REG +enum FIFO_CTRL_REG_BIT : uint8_t { + // FM2-FM0 + Bypass_mode = Bit7 | Bit6 | Bit5, // 000 Bypass mode + + FIFO_mode_SET = Bit5, // 001 FIFO mode + FIFO_mode_CLEAR = Bit7 | Bit6, + + // WTM4-WTM0: FIFO threshold. Watermark level setting + WTM40 = Bit4 | Bit3 | Bit2 | Bit1 | Bit0 +}; + +// FIFO_SRC_REG +enum FIFO_SRC_REG_BIT : uint8_t { + WTM = Bit7, // Watermark status + OVRN = Bit6, // Overrun bit status + EMPTY = Bit5, // FIFO empty bit + FSS = Bit4 | Bit3 | Bit2 | Bit1 | Bit0, // FSS4-FSS0: FIFO stored data level +}; + +namespace FIFO +{ +// L3GD20 embeds 32 slots of 16-bit data FIFO for each of the three output channels +static constexpr size_t SIZE = 32 * 2 * 3; + +struct DATA { + uint8_t OUT_X_L; + uint8_t OUT_X_H; + uint8_t OUT_Y_L; + uint8_t OUT_Y_H; + uint8_t OUT_Z_L; + uint8_t OUT_Z_H; +}; + +} + +} // namespace ST_L3GD20 diff --git a/src/drivers/imu/l3gd20/l3gd20_main.cpp b/src/drivers/imu/st/l3gd20/l3gd20_main.cpp similarity index 79% rename from src/drivers/imu/l3gd20/l3gd20_main.cpp rename to src/drivers/imu/st/l3gd20/l3gd20_main.cpp index 4c0a25ad20..e7b4ff1031 100644 --- a/src/drivers/imu/l3gd20/l3gd20_main.cpp +++ b/src/drivers/imu/st/l3gd20/l3gd20_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 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 @@ -36,34 +36,22 @@ #include #include -void -L3GD20::print_usage() +void L3GD20::print_usage() { PRINT_MODULE_USAGE_NAME("l3gd20", "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("regdump"); - PRINT_MODULE_USAGE_COMMAND("testerror"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -void L3GD20::custom_method(const BusCLIArguments &cli) -{ - switch (cli.custom1) { - case 0: print_registers(); break; - - case 1: test_error(); break; - } -} - extern "C" int l3gd20_main(int argc, char *argv[]) { int ch; using ThisDriver = L3GD20; BusCLIArguments cli{false, true}; - cli.default_spi_frequency = 11 * 1000 * 1000; + cli.default_spi_frequency = ST_L3GD20::SPI_SPEED; while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { @@ -84,26 +72,14 @@ extern "C" int l3gd20_main(int argc, char *argv[]) if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator); - } - if (!strcmp(verb, "stop")) { + } else if (!strcmp(verb, "stop")) { return ThisDriver::module_stop(iterator); - } - if (!strcmp(verb, "status")) { + } else if (!strcmp(verb, "status")) { return ThisDriver::module_status(iterator); } - if (!strcmp(verb, "regdump")) { - cli.custom1 = 0; - return ThisDriver::module_custom_method(cli, iterator); - } - - if (!strcmp(verb, "testerror")) { - cli.custom1 = 1; - return ThisDriver::module_custom_method(cli, iterator); - } - ThisDriver::print_usage(); return -1; } diff --git a/src/drivers/imu/st/l3gd20h/CMakeLists.txt b/src/drivers/imu/st/l3gd20h/CMakeLists.txt new file mode 100644 index 0000000000..befa92f037 --- /dev/null +++ b/src/drivers/imu/st/l3gd20h/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2012-2022 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__l3gd20h + MAIN l3gd20h + COMPILE_FLAGS + #-DDEBUG_BUILD + SRCS + l3gd20h_main.cpp + L3GD20H.cpp + L3GD20H.hpp + ST_L3GD20H_registers.hpp + DEPENDS + drivers_gyroscope + px4_work_queue + ) diff --git a/src/drivers/imu/st/l3gd20h/Kconfig b/src/drivers/imu/st/l3gd20h/Kconfig new file mode 100644 index 0000000000..99c423ec88 --- /dev/null +++ b/src/drivers/imu/st/l3gd20h/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_ST_L3GD20H + bool "l3gd20h" + default n + ---help--- + Enable support for l3gd20h diff --git a/src/drivers/imu/st/l3gd20h/L3GD20H.cpp b/src/drivers/imu/st/l3gd20h/L3GD20H.cpp new file mode 100644 index 0000000000..911aa1688d --- /dev/null +++ b/src/drivers/imu/st/l3gd20h/L3GD20H.cpp @@ -0,0 +1,459 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 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 "L3GD20H.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +L3GD20H::L3GD20H(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_gyro(get_device_id(), config.rotation) +{ + if (_drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); + } + + ConfigureSampleRate(ST_L3GD20H::ODR); // default to max ODR +} + +L3GD20H::~L3GD20H() +{ + 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 L3GD20H::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool L3GD20H::Reset() +{ + _state = STATE::RESET; + DataReadyInterruptDisable(); + ScheduleClear(); + ScheduleNow(); + return true; +} + +void L3GD20H::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void L3GD20H::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + + 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 L3GD20H::probe() +{ + const uint8_t whoami = RegisterRead(Register::WHO_AM_I); + + if (whoami != WHOAMI) { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; + } + + return PX4_OK; +} + +void L3GD20H::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // LOW_ODR SW_RES + RegisterWrite(Register::LOW_ODR, LOW_ODR_BIT::SW_RES); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(150_ms); + break; + + case STATE::WAIT_FOR_RESET: + if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) + && ((RegisterRead(Register::LOW_ODR) & LOW_ODR_BIT::SW_RES) == 0)) { + + // if reset succeeded then configure + RegisterSetBits(Register::CTRL1, CTRL1_BIT::PD); + _state = STATE::CONFIGURE; + ScheduleDelayed(50_ms); // Turn-on time 50 ms + + } else { + // RESET not complete + 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()) { + // if configure succeeded then start reading from FIFO + _state = STATE::FIFO_READ; + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // default to max ODR with data ready available + ConfigureSampleRate(ST_L3GD20H::ODR); + + // backup schedule as a watchdog timeout + ScheduleDelayed(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + + // default to half of max ODR if no data ready available + ConfigureSampleRate(ST_L3GD20H::ODR / 2); + + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + FIFOReset(); + + } else { + // CONFIGURE not complete + 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_READ: { + hrt_abstime timestamp_sample = now; + + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + 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); + } + + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } + + bool success = false; + + // always check FIFO status and count + const uint8_t FIFO_SRC = RegisterRead(Register::FIFO_SRC); + const uint8_t samples = FIFO_SRC & FIFO_SRC_BIT::FSS; // FSS4-FSS0 FIFO stored data level + + if ((FIFO_SRC & FIFO_SRC_BIT::OVRN) || (samples > FIFO_MAX_SAMPLES)) { + // not necessarily an overflow, but more samples than we expected or can publish + perf_count(_fifo_overflow_perf); + FIFOReset(); + + } else if ((FIFO_SRC & FIFO_SRC_BIT::EMPTY) || (samples == 0)) { + perf_count(_fifo_empty_perf); + + } else { + if (FIFORead(timestamp_sample, samples)) { + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 100) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + + } else { + // periodically update temperature (~1 Hz) + if (now >= _temperature_update_timestamp + 1_s) { + UpdateTemperature(); + _temperature_update_timestamp = now; + } + } + } + + break; + } +} + +void L3GD20H::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = ST_L3GD20H::ODR; // default to max ODR + } + + // round down to nearest FIFO sample dt + 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_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES)); + + // recompute FIFO empty interval (us) with actual gyro sample limit + _fifo_empty_interval_us = _fifo_samples * (1e6f / GYRO_RATE); +} + +bool L3GD20H::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + _px4_gyro.set_scale(math::radians(70.f * 1e-3f)); // 70 mdps/LSB (FS = 2000 dps) + _px4_gyro.set_range(math::radians(2000.f)); // ±2000 dps + + return success; +} + +int L3GD20H::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void L3GD20H::DataReady() +{ + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); +} + +bool L3GD20H::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0; +} + +bool L3GD20H::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool L3GD20H::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 L3GD20H::RegisterRead(Register reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +void L3GD20H::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +void L3GD20H::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 L3GD20H::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) +{ + FIFOTransferBuffer buffer{}; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_count(_bad_transfer_perf); + return false; + } + + ProcessGyro(timestamp_sample, buffer.f, samples); + + return true; +} + +void L3GD20H::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // To reset FIFO content Bypass mode should be written in FIFO_CTRL + RegisterClearBits(Register::FIFO_CTRL, FIFO_CTRL_BIT::Bypass_mode); + + // reset while FIFO is disabled + _drdy_timestamp_sample.store(0); + + // FIFO_CTRL: mode + watermark + for (auto &r : _register_cfg) { + if (r.reg == Register::FIFO_CTRL) { + RegisterSetAndClearBits(Register::FIFO_CTRL, r.set_bits, r.clear_bits); + } + } +} + +void L3GD20H::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) +{ + sensor_gyro_fifo_s gyro{}; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = samples; + gyro.dt = FIFO_SAMPLE_DT; + + for (int i = 0; i < samples; i++) { + int16_t gyro_x = combine(fifo[i].OUT_X_H, fifo[i].OUT_X_L); + int16_t gyro_y = combine(fifo[i].OUT_Y_H, fifo[i].OUT_Y_L); + int16_t gyro_z = combine(fifo[i].OUT_Z_H, fifo[i].OUT_Z_L); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + gyro.x[i] = gyro_x; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); + } + + _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); +} + +void L3GD20H::UpdateTemperature() +{ + // Temperature data (-1LSB/deg with 8 bit resolution). The value is expressed as two’s complement. + const int8_t OUT_TEMP = RegisterRead(Register::OUT_TEMP); + + const float temperature_c = (OUT_TEMP / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET; + + if (PX4_ISFINITE(temperature_c) + && (temperature_c >= TEMPERATURE_SENSOR_MIN) + && (temperature_c <= TEMPERATURE_SENSOR_MAX)) { + + _px4_gyro.set_temperature(temperature_c); + } + +} diff --git a/src/drivers/imu/st/l3gd20h/L3GD20H.hpp b/src/drivers/imu/st/l3gd20h/L3GD20H.hpp new file mode 100644 index 0000000000..3d002469c5 --- /dev/null +++ b/src/drivers/imu/st/l3gd20h/L3GD20H.hpp @@ -0,0 +1,156 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 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 L3GD20H.hpp + * + * Driver for the ST L3GD20H connected via SPI. + * + */ + +#pragma once + +#include "ST_L3GD20H_registers.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace ST_L3GD20H; + +class L3GD20H : public device::SPI, public I2CSPIDriver +{ +public: + L3GD20H(const I2CSPIDriverConfig &config); + ~L3GD20H() 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 / ST_L3GD20H::ODR}; + static constexpr float GYRO_RATE{ST_L3GD20H::ODR}; + + static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]))}; + + // Transfer data + struct FIFOTransferBuffer { + uint8_t cmd{static_cast(Register::OUT_X_L) | DIR_READ | AUTO_INCREMENT}; + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // ensure no struct padding + static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + 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); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + 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); + void RegisterSetBits(Register reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); } + void RegisterClearBits(Register reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); } + + bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); + void FIFOReset(); + + void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); + void UpdateTemperature(); + + const spi_drdy_gpio_t _drdy_gpio; + + 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_READ, + } _state{STATE::RESET}; + + uint16_t _fifo_empty_interval_us{1000}; + uint32_t _fifo_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{6}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::CTRL1, CTRL1_BIT::ODR_800HZ_CUTOFF_100HZ | CTRL1_BIT::PD | CTRL1_BIT::Zen | CTRL1_BIT::Yen | CTRL1_BIT::Xen, 0 }, + { Register::CTRL3, CTRL3_BIT::H_Lactive | CTRL3_BIT::INT2_DRDY, CTRL3_BIT::INT2_FTH }, + { Register::CTRL4, CTRL4_BIT::BDU | CTRL4_BIT::FS_2000DPS, 0 }, + { Register::CTRL5, CTRL5_BIT::FIFO_EN, 0 }, + { Register::FIFO_CTRL, FIFO_CTRL_BIT::FIFO_mode_SET, FIFO_CTRL_BIT::FIFO_mode_CLEAR }, + { Register::LOW_ODR, LOW_ODR_BIT::DRDY_HL | LOW_ODR_BIT::I2C_dis, LOW_ODR_BIT::SW_RES | LOW_ODR_BIT::Low_ODR }, + }; +}; diff --git a/src/drivers/imu/st/l3gd20h/ST_L3GD20H_registers.hpp b/src/drivers/imu/st/l3gd20h/ST_L3GD20H_registers.hpp new file mode 100644 index 0000000000..8aecfc5e95 --- /dev/null +++ b/src/drivers/imu/st/l3gd20h/ST_L3GD20H_registers.hpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 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_L3GD20H_registers.hpp + * + * ST L3GD20H registers. + * + */ + +#pragma once + +#include + +// 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_L3GD20H +{ +static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface + +static constexpr uint8_t DIR_READ = Bit7; +static constexpr uint8_t AUTO_INCREMENT = Bit6; + +static constexpr uint8_t WHOAMI = 0b11010111; // Who I am ID + +static constexpr float ODR = 757.6; // 757.6 Hz Angular rate output data rate + +static constexpr float TEMPERATURE_SENSITIVITY = -1.f; // LSB/C +static constexpr float TEMPERATURE_OFFSET = 40.f; // C +static constexpr float TEMPERATURE_SENSOR_MIN = -40.f; // °C +static constexpr float TEMPERATURE_SENSOR_MAX = 85.f; // °C + +enum class Register : uint8_t { + WHO_AM_I = 0x0F, + + CTRL1 = 0x20, + CTRL2 = 0x21, + CTRL3 = 0x22, + CTRL4 = 0x23, + CTRL5 = 0x24, + + OUT_TEMP = 0x26, + STATUS = 0x27, + OUT_X_L = 0x28, + OUT_X_H = 0x29, + OUT_Y_L = 0x2A, + OUT_Y_H = 0x2B, + OUT_Z_L = 0x2C, + OUT_Z_H = 0x2D, + FIFO_CTRL = 0x2E, + FIFO_SRC = 0x2F, + + LOW_ODR = 0x39, +}; + +// CTRL1 +enum CTRL1_BIT : uint8_t { + // DR1 DR0 BW1 BW0 + ODR_800HZ_CUTOFF_100HZ = Bit7 | Bit6 | Bit5 | Bit4, + PD = Bit3, // Power-down mode enable (1=Normal Mode) + Zen = Bit2, // Z axis enable + Yen = Bit1, // Y axis enable + Xen = Bit0, // X axis enable +}; + +// CTRL3 +enum CTRL3_BIT : uint8_t { + H_Lactive = Bit5, // Interrupt active configuration on INT. Default value 0. (0: high; 1:low) + + INT2_DRDY = Bit3, // Date Ready on DRDY/INT2 pin. + INT2_FTH = Bit2, // FIFO Threshold interrupt on DRDY/INT2 pin. +}; + +// CTRL4 +enum CTRL4_BIT : uint8_t { + BDU = Bit7, // Block data update + + // FS1-FS0 + FS_2000DPS = Bit5 | Bit4, // Full scale selection 2000 dps +}; + +// CTRL5 +enum CTRL5_BIT : uint8_t { + BOOT = Bit7, // Reboot memory content + FIFO_EN = Bit6, +}; + +// FIFO_CTRL +enum FIFO_CTRL_BIT : uint8_t { + // FM2-FM0 + Bypass_mode = Bit7 | Bit6 | Bit5, // 000 Bypass mode + + FIFO_mode_SET = Bit5, // 001 FIFO mode + FIFO_mode_CLEAR = Bit7 | Bit6, + + // FTH4-FTH0: FIFO threshold setting. + FTH40 = Bit4 | Bit3 | Bit2 | Bit1 | Bit0 +}; + +// FIFO_SRC +enum FIFO_SRC_BIT : uint8_t { + FTH = Bit7, // FIFO threshold status. + OVRN = Bit6, // Overrun bit status. + EMPTY = Bit5, // FIFO empty bit. + FSS = Bit4 | Bit3 | Bit2 | Bit1 | Bit0, // FSS4-FSS0: FIFO stored data level +}; + +// LOW_ODR +enum LOW_ODR_BIT : uint8_t { + DRDY_HL = Bit5, // DRDY/INT2 pin active level. 1 = DRDY active low + + I2C_dis = Bit3, // 1 = SPI only + SW_RES = Bit2, // 1 = Reset Device + + Low_ODR = Bit0, // 0 = Low Speed ODR disabled +}; + +namespace FIFO +{ +// L3GD20H embeds 32 slots of 16-bit data FIFO for each of the three output channels +static constexpr size_t SIZE = 32 * 2 * 3; + +struct DATA { + uint8_t OUT_X_L; + uint8_t OUT_X_H; + uint8_t OUT_Y_L; + uint8_t OUT_Y_H; + uint8_t OUT_Z_L; + uint8_t OUT_Z_H; +}; + +} + +} // namespace ST_L3GD20H diff --git a/src/drivers/imu/st/l3gd20h/l3gd20h_main.cpp b/src/drivers/imu/st/l3gd20h/l3gd20h_main.cpp new file mode 100644 index 0000000000..65a57e718a --- /dev/null +++ b/src/drivers/imu/st/l3gd20h/l3gd20h_main.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 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 "L3GD20H.hpp" + +#include +#include + +void L3GD20H::print_usage() +{ + PRINT_MODULE_USAGE_NAME("l3gd20h", "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 l3gd20h_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = L3GD20H; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = ST_L3GD20H::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_GYR_DEVTYPE_L3GD20H); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/imu/lsm303d/CMakeLists.txt b/src/drivers/imu/st/lsm303d/CMakeLists.txt similarity index 92% rename from src/drivers/imu/lsm303d/CMakeLists.txt rename to src/drivers/imu/st/lsm303d/CMakeLists.txt index 2ad45e295f..28084c9927 100644 --- a/src/drivers/imu/lsm303d/CMakeLists.txt +++ b/src/drivers/imu/st/lsm303d/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2022 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 @@ -30,14 +30,16 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__lsm303d + MODULE drivers__imu__st__lsm303d MAIN lsm303d COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable SRCS lsm303d_main.cpp LSM303D.cpp + LSM303D.hpp + ST_LSM303D_registers.hpp DEPENDS drivers_accelerometer drivers_magnetometer diff --git a/src/drivers/imu/st/lsm303d/Kconfig b/src/drivers/imu/st/lsm303d/Kconfig new file mode 100644 index 0000000000..8e0fb7463e --- /dev/null +++ b/src/drivers/imu/st/lsm303d/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_ST_LSM303D + bool "lsm303d" + default n + ---help--- + Enable support for lsm303d diff --git a/src/drivers/imu/st/lsm303d/LSM303D.cpp b/src/drivers/imu/st/lsm303d/LSM303D.cpp new file mode 100644 index 0000000000..6ce6a9265f --- /dev/null +++ b/src/drivers/imu/st/lsm303d/LSM303D.cpp @@ -0,0 +1,433 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2022 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 "LSM303D.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +LSM303D::LSM303D(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _px4_accel(get_device_id(), config.rotation), + _px4_mag(get_device_id(), config.rotation) +{ + ConfigureSampleRate(_px4_accel.get_max_rate_hz()); +} + +LSM303D::~LSM303D() +{ + 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); +} + +int LSM303D::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool LSM303D::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void LSM303D::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + + 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); +} + +int LSM303D::probe() +{ + const uint8_t whoami = RegisterRead(Register::WHO_AM_I); + + if (whoami != WHOAMI) { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; + } + + return PX4_OK; +} + +void LSM303D::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // CTRL0 + RegisterSetBits(Register::CTRL0, CTRL0_BIT::BOOT); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(150_ms); + break; + + case STATE::WAIT_FOR_RESET: + if (RegisterRead(Register::WHO_AM_I) == WHOAMI) { + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } else { + // RESET not complete + 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()) { + // if configure succeeded then start reading from FIFO + _state = STATE::FIFO_READ; + + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + + FIFOReset(); + + } else { + // CONFIGURE not complete + 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_READ: { + hrt_abstime timestamp_sample = now; + + bool success = false; + + // always check FIFO status and count + const uint8_t FIFO_SRC = RegisterRead(Register::FIFO_SRC); + const uint8_t samples = (FIFO_SRC & FIFO_SRC_BIT::FSS); // FSS4-FSS0 FIFO stored data level + + if ((FIFO_SRC & FIFO_SRC_BIT::OVRN) || (samples > FIFO_MAX_SAMPLES)) { + // not necessarily an overflow, but more samples than we expected or can publish + perf_count(_fifo_overflow_perf); + FIFOReset(); + + } else if ((FIFO_SRC & FIFO_SRC_BIT::EMPTY) || (samples == 0)) { + perf_count(_fifo_empty_perf); + + } else { + if (FIFORead(timestamp_sample, samples)) { + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + + } else { + // periodically update temperature (50 Hz) + if (now >= _temperature_and_magnetometer_update_timestamp + 20_ms) { + UpdateTemperatureAndMagnetometer(); + _temperature_and_magnetometer_update_timestamp = now; + } + } + } + + break; + } +} + +void LSM303D::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = ST_LSM303D::LA_ODR; // default to max ODR + } + + // round down to nearest FIFO sample dt + 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_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES)); + + _fifo_samples = math::max(_fifo_samples, (uint32_t)2); // require at least 2 FIFO samples per iteration + + // recompute FIFO empty interval (us) with actual accel sample limit + _fifo_empty_interval_us = _fifo_samples * (1e6f / ACCEL_RATE); +} + +bool LSM303D::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + // Linear acceleration sensitivity ±16g (0.732 mg/LSB) + _px4_accel.set_scale(CONSTANTS_ONE_G * 0.732f * 1e-3f); + _px4_accel.set_range(16.f * CONSTANTS_ONE_G); + + // Magnetic sensitivity ±12gauss (0.479 mgauss/LSB) + _px4_mag.set_scale(0.479f * 1e-3f); + + return success; +} + +bool LSM303D::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 LSM303D::RegisterRead(Register reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +void LSM303D::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +void LSM303D::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 LSM303D::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) +{ + FIFOTransferBuffer buffer{}; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_count(_bad_transfer_perf); + return false; + } + + ProcessAccel(timestamp_sample, buffer.f, samples); + + return true; +} + +void LSM303D::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // FIFO_CTRL_REG: switch to bypass mode to restart data collection + RegisterClearBits(Register::FIFO_CTRL, FIFO_CTRL_BIT::Bypass_mode); + + // FIFO_CTRL_REG: mode + watermark + for (auto &r : _register_cfg) { + if (r.reg == Register::FIFO_CTRL) { + RegisterSetAndClearBits(Register::FIFO_CTRL, r.set_bits, r.clear_bits); + } + } +} + +void LSM303D::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) +{ + sensor_accel_fifo_s accel{}; + accel.timestamp_sample = timestamp_sample; + accel.samples = samples; + accel.dt = FIFO_SAMPLE_DT; + + for (int i = 0; i < samples; i++) { + int16_t accel_x = combine(fifo[i].OUT_X_H_A, fifo[i].OUT_X_L_A); + int16_t accel_y = combine(fifo[i].OUT_Y_H_A, fifo[i].OUT_Y_L_A); + int16_t accel_z = combine(fifo[i].OUT_Z_H_A, fifo[i].OUT_Z_L_A); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + accel.x[i] = accel_x; + accel.y[i] = math::negate(accel_y); + accel.z[i] = math::negate(accel_z); + } + + _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); +} + +bool LSM303D::UpdateTemperatureAndMagnetometer() +{ + const hrt_abstime time_now_us = hrt_absolute_time(); + + // read current temperature + struct TransferBuffer { + uint8_t cmd; + uint8_t TEMP_OUT_L; // 0x05 + uint8_t TEMP_OUT_H; // 0x06 + uint8_t STATUS_M; // 0x07 + uint8_t OUT_X_L_M; // 0x08 + uint8_t OUT_X_H_M; // 0x09 + uint8_t OUT_Y_L_M; // 0x0A + uint8_t OUT_Y_H_M; // 0x0B + uint8_t OUT_Z_L_M; // 0x0C + uint8_t OUT_Z_H_M; // 0x0D + } buffer{}; + + buffer.cmd = static_cast(Register::TEMP_OUT_L) | DIR_READ | AUTO_INCREMENT; + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return false; + } + + if (buffer.STATUS_M & STATUS_M_BIT::ZYXMDA) { + // 8 LSB/°C, 25 °C offset + // Temperature data is stored inside TEMP_OUT_L (05h), TEMP_OUT_H (06h) as 2’s + // complement data in 12-bit format, right justified. + uint16_t TEMP_OUT = combine(buffer.TEMP_OUT_H, buffer.TEMP_OUT_L); + + // shift to align sign bit, cast to int16_t, then shift back + int16_t temperature_raw = static_cast(TEMP_OUT << 4) >> 4; + + const float temperature_c = (temperature_raw / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET; + + if (PX4_ISFINITE(temperature_c) + && (temperature_c >= TEMPERATURE_SENSOR_MIN) + && (temperature_c <= TEMPERATURE_SENSOR_MAX)) { + + _px4_accel.set_temperature(temperature_c); + _px4_mag.set_temperature(temperature_c); + + } else { + return false; + } + + int16_t mag_x = combine(buffer.OUT_X_H_M, buffer.OUT_X_L_M); + int16_t mag_y = combine(buffer.OUT_Y_H_M, buffer.OUT_Y_L_M); + int16_t mag_z = combine(buffer.OUT_Z_H_M, buffer.OUT_Z_L_M); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + mag_y = math::negate(mag_y); + mag_z = math::negate(mag_z); + + _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)); + _px4_mag.update(time_now_us, mag_x, mag_y, mag_z); + + return true; + } + + return false; +} diff --git a/src/drivers/imu/st/lsm303d/LSM303D.hpp b/src/drivers/imu/st/lsm303d/LSM303D.hpp new file mode 100644 index 0000000000..bfad35e82e --- /dev/null +++ b/src/drivers/imu/st/lsm303d/LSM303D.hpp @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2022 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 LSM303D.hpp + * + * Driver for the ST LSM303D connected via SPI. + * + */ + +#pragma once + +#include "ST_LSM303D_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace ST_LSM303D; + +class LSM303D : public device::SPI, public I2CSPIDriver +{ +public: + LSM303D(const I2CSPIDriverConfig &config); + ~LSM303D() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + // Sensor Configuration + static constexpr float FIFO_SAMPLE_DT{1e6f / ST_LSM303D::LA_ODR}; + static constexpr float ACCEL_RATE{ST_LSM303D::LA_ODR}; + + static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]))}; + + // Transfer data + struct FIFOTransferBuffer { + uint8_t cmd{static_cast(Register::OUT_X_L_A) | DIR_READ | AUTO_INCREMENT}; + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // ensure no struct padding + static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + 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); + void ConfigureFIFOWatermark(uint8_t samples); + + 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); + void RegisterSetBits(Register reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); } + void RegisterClearBits(Register reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); } + + bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); + void FIFOReset(); + + void ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); + bool UpdateTemperatureAndMagnetometer(); + + PX4Accelerometer _px4_accel; + PX4Magnetometer _px4_mag; + + 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")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + hrt_abstime _temperature_and_magnetometer_update_timestamp{0}; + int _failure_count{0}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + FIFO_READ, + } _state{STATE::RESET}; + + uint16_t _fifo_empty_interval_us{1000}; + uint32_t _fifo_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{8}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::CTRL0, CTRL0_BIT::FIFO_EN, CTRL0_BIT::BOOT | CTRL0_BIT::BIT4_MUST_BE_CLEARED | CTRL0_BIT::BIT3_MUST_BE_CLEARED }, + { Register::CTRL1, CTRL1_BIT::AODR_1600HZ_SET | CTRL1_BIT::BDU | CTRL1_BIT::AZEN | CTRL1_BIT::AYEN | CTRL1_BIT::AXEN, CTRL1_BIT::AODR_1600HZ_CLEAR }, + { Register::CTRL2, CTRL2_BIT::AFS_16G_SET, CTRL2_BIT::ABW_773_HZ | CTRL2_BIT::AFS_16G_CLEAR | CTRL2_BIT::BIT2_MUST_BE_CLEARED }, + { Register::CTRL5, CTRL5_BIT::TEMP_EN | CTRL5_BIT::M_RES_HIGH | CTRL5_BIT::M_ODR_50_HZ_SET, CTRL5_BIT::M_ODR_50_HZ_CLEAR }, + { Register::CTRL6, CTRL6_BIT::MFS_12_GAUSS, 0 }, + { Register::CTRL7, 0, CTRL7_BIT::MD }, + { Register::FIFO_CTRL, FIFO_CTRL_BIT::FIFO_MODE_SET, FIFO_CTRL_BIT::FIFO_MODE_CLEAR }, + }; +}; diff --git a/src/drivers/imu/st/lsm303d/ST_LSM303D_registers.hpp b/src/drivers/imu/st/lsm303d/ST_LSM303D_registers.hpp new file mode 100644 index 0000000000..066f3832f0 --- /dev/null +++ b/src/drivers/imu/st/lsm303d/ST_LSM303D_registers.hpp @@ -0,0 +1,209 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2022 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_LSM303D_registers.hpp + * + * ST LSM303D registers. + * + */ + +#pragma once + +#include + +// 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_LSM303D +{ +static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10MHz SPI serial interface + +static constexpr uint8_t DIR_READ = Bit7; +static constexpr uint8_t AUTO_INCREMENT = Bit6; + +static constexpr uint8_t WHOAMI = 0b01001001; // Who I am ID + +static constexpr uint32_t LA_ODR = 1600; // Accelerometer output data rate + +static constexpr float TEMPERATURE_SENSITIVITY = 8.f; // LSB/C +static constexpr float TEMPERATURE_OFFSET = 25.f; // C +static constexpr float TEMPERATURE_SENSOR_MIN = -40.f; // °C +static constexpr float TEMPERATURE_SENSOR_MAX = 85.f; // °C + +enum class Register : uint8_t { + TEMP_OUT_L = 0x05, + TEMP_OUT_H = 0x06, + STATUS_M = 0x07, + OUT_X_L_M = 0x08, + + WHO_AM_I = 0x0F, + + CTRL0 = 0x1F, + CTRL1 = 0x20, + CTRL2 = 0x21, + CTRL3 = 0x22, + CTRL4 = 0x23, + CTRL5 = 0x24, + CTRL6 = 0x25, + CTRL7 = 0x26, + STATUS_A = 0x27, + OUT_X_L_A = 0x28, + + FIFO_CTRL = 0x2E, + FIFO_SRC = 0x2F, +}; + +// STATUS_M +enum STATUS_M_BIT : uint8_t { + ZYXMOR = Bit7, // Magnetic X, Y and Z-axis and temperature data overrun + + ZYXMDA = Bit3, // X, Y and Z-axis and temperature new data available +}; + +// CTRL0 +enum CTRL0_BIT : uint8_t { + BOOT = Bit7, // Reboot memory content + FIFO_EN = Bit6, + FTH_EN = Bit5, // FIFO programmable threshold enable + + BIT4_MUST_BE_CLEARED = Bit4, // This bit must be set to ‘0’ for the correct working of the device. + BIT3_MUST_BE_CLEARED = Bit3, // This bit must be set to ‘0’ for the correct working of the device. +}; + +// CTRL1 +enum CTRL1_BIT : uint8_t { + // AODR [7:4] + AODR_1600HZ_SET = Bit7 | Bit5, // 1010 1600 Hz + AODR_1600HZ_CLEAR = Bit6 | Bit4, + + BDU = Bit3, // Block data update + AZEN = Bit2, // Acceleration Z-axis enable + AYEN = Bit2, // Acceleration Y-axis enable + AXEN = Bit2, // Acceleration X-axis enable +}; + +// CTRL2 +enum CTRL2_BIT : uint8_t { + // ABW [7:6] - Accelerometer anti-alias filter bandwidth + ABW_773_HZ = Bit7 | Bit6, // Accelerometer anti-alias filter bandwidth 773 Hz (maximum) + + // AFS [5:3] - Acceleration full-scale selection + AFS_16G_SET = Bit5, // 100 ± 16g + AFS_16G_CLEAR = Bit4 | Bit3, + BIT2_MUST_BE_CLEARED = Bit2, // This bit must be set to ‘0’ for the correct working of the device. +}; + +// CTRL3 +enum CTRL3_BIT : uint8_t { + INT1_DRDY_A = Bit2, // Accelerometer data-ready signal on INT1 +}; + +// CTRL4 +enum CTRL4_BIT : uint8_t { + INT2_DRDY_A = Bit3, // Accelerometer data-ready signal on INT2. + + INT2_FTH = Bit0, // FIFO threshold interrupt on INT2 +}; + +// CTRL5 +enum CTRL5_BIT : uint8_t { + TEMP_EN = Bit7, // Temperature sensor enable + + // M_RES [6:5] + M_RES_HIGH = Bit6 | Bit5, // Magnetic resolution selection (11: high resolution) + + // M_ODR [4:2] + M_ODR_50_HZ_SET = Bit4, // 100 50Hz + M_ODR_50_HZ_CLEAR = Bit3 | Bit2, +}; + +// CTRL6 +enum CTRL6_BIT : uint8_t { + // MFS [6:5] + MFS_12_GAUSS = Bit6 | Bit5, // Magnetic full-scale selection ± 12 gauss +}; + +// CTRL7 +enum CTRL7_BIT : uint8_t { + AFDS = Bit5, // Filtered acceleration data selection (0: internal filter bypassed) + + MD = Bit1 | Bit0, // Magnetic sensor mode selection (00: Continuous-conversion mode) +}; + +// FIFO_CTRL +enum FIFO_CTRL_BIT : uint8_t { + // FM [7:5] + FIFO_MODE_SET = Bit5, + FIFO_MODE_CLEAR = Bit7 | Bit6, + + Bypass_mode = Bit7 | Bit6 | Bit5, // 000 + + // FTH [4:0]: FIFO threshold level. + FIFO_THRESHOLD = Bit4 | Bit3 | Bit2 | Bit1 | Bit0, +}; + +// FIFO_SRC_REG +enum FIFO_SRC_BIT : uint8_t { + FTH = Bit7, // FIFO threshold status (set to 1 when FIFO content exceeds threshold level) + OVRN = Bit6, // FIFO overrun status + EMPTY = Bit5, // Empty status. + // FSS [4:0]: FIFO stored data level. + FSS = Bit4 | Bit3 | Bit2 | Bit1 | Bit0, +}; + + +namespace FIFO +{ +// LSM303D embeds 32 slots of 16-bit data FIFO for each of the three output channels +static constexpr size_t SIZE = 32 * 2 * 3; + +struct DATA { + uint8_t OUT_X_L_A; + uint8_t OUT_X_H_A; + uint8_t OUT_Y_L_A; + uint8_t OUT_Y_H_A; + uint8_t OUT_Z_L_A; + uint8_t OUT_Z_H_A; +}; + +} + +} // namespace ST_LSM303D diff --git a/src/drivers/imu/lsm303d/lsm303d_main.cpp b/src/drivers/imu/st/lsm303d/lsm303d_main.cpp similarity index 88% rename from src/drivers/imu/lsm303d/lsm303d_main.cpp rename to src/drivers/imu/st/lsm303d/lsm303d_main.cpp index 47220012e4..3272f2aa76 100644 --- a/src/drivers/imu/lsm303d/lsm303d_main.cpp +++ b/src/drivers/imu/st/lsm303d/lsm303d_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 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 @@ -31,18 +31,12 @@ * ****************************************************************************/ -/** - * @file lsm303d_main.cpp - * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. - */ - #include "LSM303D.hpp" #include #include -void -LSM303D::print_usage() +void LSM303D::print_usage() { PRINT_MODULE_USAGE_NAME("lsm303d", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("imu"); @@ -57,7 +51,7 @@ extern "C" int lsm303d_main(int argc, char *argv[]) int ch; using ThisDriver = LSM303D; BusCLIArguments cli{false, true}; - cli.default_spi_frequency = 11 * 1000 * 1000; + cli.default_spi_frequency = ST_LSM303D::SPI_SPEED; while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { switch (ch) { @@ -78,13 +72,11 @@ extern "C" int lsm303d_main(int argc, char *argv[]) if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator); - } - if (!strcmp(verb, "stop")) { + } else if (!strcmp(verb, "stop")) { return ThisDriver::module_stop(iterator); - } - if (!strcmp(verb, "status")) { + } else if (!strcmp(verb, "status")) { return ThisDriver::module_status(iterator); }