fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 12:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers/imu/st: rewrite l3gd20/l3gd20h/lsm303d drivers

Browse Source
This commit is contained in:
Daniel Agar
2022-04-11 23:34:32 -04:00
parent bb2ea574aa
commit 0e9572546e
34 changed files with 2634 additions and 1825 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/airmind/mindpx-v2/default.px4board
+6 -5
View File
@@ -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
boards/airmind/mindpx-v2/init/rc.board_sensors
+6 -2
View File
@@ -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
boards/px4/fmu-v2/default.px4board
+2 -2
View File
@@ -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
boards/px4/fmu-v2/init/rc.board_sensors
+48 -71
View File
@@ -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
boards/px4/fmu-v3/default.px4board
+6 -5
View File
@@ -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
boards/px4/fmu-v3/init/rc.board_sensors
+44 -66
View File
@@ -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
boards/px4/fmu-v3/src/spi.cpp
+4 -2
View File
@@ -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}),
}),
platforms/common/include/px4_platform_common/spi.h
+1 -1
View File
@@ -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)
src/drivers/drv_sensor.h
+1
View File
@@ -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
src/drivers/imu/Kconfig
-2
View File
@@ -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
src/drivers/imu/l3gd20/Kconfig
-5
View File
@@ -1,5 +0,0 @@
menuconfig DRIVERS_IMU_L3GD20
bool "l3gd20"
default n
---help---
Enable support for l3gd20
src/drivers/imu/l3gd20/L3GD20.cpp
-440
View File
@@ -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);
}
src/drivers/imu/l3gd20/L3GD20.hpp
-286
View File
@@ -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 <drivers/device/spi.h>
#include <lib/conversion/rotation.h>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <perf/perf_counter.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h>
/* 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<L3GD20>
{
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);
};
src/drivers/imu/lsm303d/Kconfig
-5
View File
@@ -1,5 +0,0 @@
menuconfig DRIVERS_IMU_LSM303D
bool "lsm303d"
default n
---help---
Enable support for lsm303d
src/drivers/imu/lsm303d/LSM303D.cpp
-589
View File
@@ -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]);
}
}
}
src/drivers/imu/lsm303d/LSM303D.hpp
-294
View File
@@ -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 <drivers/device/spi.h>
#include <geo/geo.h>
#include <perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
/* 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<LSM303D>
{
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};
};
src/drivers/imu/l3gd20/CMakeLists.txt → src/drivers/imu/st/l3gd20/CMakeLists.txt
+7 -5
View File
@@ -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
src/drivers/imu/st/l3gd20/Kconfig
+5
View File
@@ -0,0 +1,5 @@
menuconfig DRIVERS_IMU_ST_L3GD20
bool "l3gd20"
default n
---help---
Enable support for l3gd20
src/drivers/imu/st/l3gd20/L3GD20.cpp
+457
View File
@@ -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 &reg_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 &reg_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<L3GD20 *>(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 &reg_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<uint8_t>(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 &timestamp_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 &timestamp_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);
}
}
src/drivers/imu/st/l3gd20/L3GD20.hpp
+155
View File
@@ -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 <drivers/drv_hrt.h>
#include <lib/drivers/device/spi.h>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/i2c_spi_buses.h>
using namespace ST_L3GD20;
class L3GD20 : public device::SPI, public I2CSPIDriver<L3GD20>
{
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<uint8_t>(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 &reg_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 &timestamp_sample, uint8_t samples);
void FIFOReset();
void ProcessGyro(const hrt_abstime &timestamp_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<hrt_abstime> _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<uint32_t>(_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 },
};
};
src/drivers/imu/st/l3gd20/ST_L3GD20_registers.hpp
+158
View File
@@ -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 <cstdint>
// 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
src/drivers/imu/l3gd20/l3gd20_main.cpp → src/drivers/imu/st/l3gd20/l3gd20_main.cpp
+5 -29
View File
@@ -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 <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
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;
}
src/drivers/imu/st/l3gd20h/CMakeLists.txt
+47
View File
@@ -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
)
src/drivers/imu/st/l3gd20h/Kconfig
+5
View File
@@ -0,0 +1,5 @@
menuconfig DRIVERS_IMU_ST_L3GD20H
bool "l3gd20h"
default n
---help---
Enable support for l3gd20h
src/drivers/imu/st/l3gd20h/L3GD20H.cpp
+459
View File
@@ -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 &reg_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 &reg_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<L3GD20H *>(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 &reg_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<uint8_t>(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 &timestamp_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 &timestamp_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);
}
}
src/drivers/imu/st/l3gd20h/L3GD20H.hpp
+156
View File
@@ -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 <drivers/drv_hrt.h>
#include <lib/drivers/device/spi.h>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/i2c_spi_buses.h>
using namespace ST_L3GD20H;
class L3GD20H : public device::SPI, public I2CSPIDriver<L3GD20H>
{
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<uint8_t>(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 &reg_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 &timestamp_sample, uint8_t samples);
void FIFOReset();
void ProcessGyro(const hrt_abstime &timestamp_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<hrt_abstime> _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<uint32_t>(_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 },
};
};
src/drivers/imu/st/l3gd20h/ST_L3GD20H_registers.hpp
+172
View File
@@ -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 <cstdint>
// 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
src/drivers/imu/st/l3gd20h/l3gd20h_main.cpp
+85
View File
@@ -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 <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
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;
}
src/drivers/imu/lsm303d/CMakeLists.txt → src/drivers/imu/st/lsm303d/CMakeLists.txt
+5 -3
View File
@@ -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
src/drivers/imu/st/lsm303d/Kconfig
+5
View File
@@ -0,0 +1,5 @@
menuconfig DRIVERS_IMU_ST_LSM303D
bool "lsm303d"
default n
---help---
Enable support for lsm303d
src/drivers/imu/st/lsm303d/LSM303D.cpp
+433
View File
@@ -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 &reg_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 &reg_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 &reg_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<uint8_t>(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 &timestamp_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 &timestamp_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<uint8_t>(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<int16_t>(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;
}
src/drivers/imu/st/lsm303d/LSM303D.hpp
+148
View File
@@ -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 <drivers/drv_hrt.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/device/spi.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/i2c_spi_buses.h>
using namespace ST_LSM303D;
class LSM303D : public device::SPI, public I2CSPIDriver<LSM303D>
{
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<uint8_t>(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 &reg_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 &timestamp_sample, uint8_t samples);
void FIFOReset();
void ProcessAccel(const hrt_abstime &timestamp_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<uint32_t>(_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 },
};
};
src/drivers/imu/st/lsm303d/ST_LSM303D_registers.hpp
+209
View File
@@ -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 <cstdint>
// 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
src/drivers/imu/lsm303d/lsm303d_main.cpp → src/drivers/imu/st/lsm303d/lsm303d_main.cpp
+5 -13
View File
@@ -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 <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
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);
}