mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-19 04:33:34 +08:00
imu/st/lsm9ds1: refactor to same form as other new IMU drivers
- register configuration captured in table and checked periodically - simple state machine - all driver sleeps removed - driver respects IMU_GYRO_RATEMAX
This commit is contained in:
Daniel Agar
@@ -34,10 +34,11 @@
|
||||
px4_add_module(
|
||||
MODULE drivers__imu__st__lsm9ds1
|
||||
MAIN lsm9ds1
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
lsm9ds1_main.cpp
|
||||
LSM9DS1.cpp
|
||||
LSM9DS1.hpp
|
||||
lsm9ds1_main.cpp
|
||||
ST_LSM9DS1_Registers.hpp
|
||||
DEPENDS
|
||||
drivers_accelerometer
|
||||
|
||||
@@ -34,9 +34,11 @@
|
||||
#include "LSM9DS1.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
using namespace ST_LSM9DS1;
|
||||
|
||||
static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; }
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
||||
{
|
||||
return (msb << 8u) | lsb;
|
||||
}
|
||||
|
||||
LSM9DS1::LSM9DS1(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
|
||||
spi_mode_e spi_mode) :
|
||||
@@ -45,92 +47,256 @@ LSM9DS1::LSM9DS1(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rota
|
||||
_px4_accel(get_device_id(), rotation),
|
||||
_px4_gyro(get_device_id(), rotation)
|
||||
{
|
||||
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
|
||||
}
|
||||
|
||||
LSM9DS1::~LSM9DS1()
|
||||
{
|
||||
perf_free(_interval_perf);
|
||||
perf_free(_transfer_perf);
|
||||
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 LSM9DS1::probe()
|
||||
{
|
||||
uint8_t whoami = RegisterRead(Register::WHO_AM_I);
|
||||
|
||||
if (whoami == LSM9DS1_WHO_AM_I) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int LSM9DS1::init()
|
||||
{
|
||||
int ret = SPI::init();
|
||||
|
||||
if (ret != OK) {
|
||||
DEVICE_DEBUG("SPI init failed (%i)", ret);
|
||||
if (ret != PX4_OK) {
|
||||
DEVICE_DEBUG("SPI::init failed (%i)", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (!Reset()) {
|
||||
PX4_ERR("reset failed");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
Start();
|
||||
|
||||
return PX4_OK;
|
||||
return Reset() ? 0 : -1;
|
||||
}
|
||||
|
||||
bool LSM9DS1::Reset()
|
||||
{
|
||||
// Reset
|
||||
// CTRL_REG8: SW_RESET
|
||||
RegisterSetBits(Register::CTRL_REG8, CTRL_REG8_BIT::SW_RESET);
|
||||
usleep(50); // Wait 50 μs (or wait until the SW_RESET bit of the CTRL_REG8 register returns to 0).
|
||||
|
||||
RegisterSetBits(Register::CTRL_REG9, CTRL_REG9_BIT::I2C_DISABLE);
|
||||
|
||||
// Gyroscope configuration
|
||||
// CTRL_REG1_G: Gyroscope 2000 degrees/second and ODR 952 Hz
|
||||
RegisterWrite(Register::CTRL_REG1_G,
|
||||
CTRL_REG1_G_BIT::ODR_G_952HZ | CTRL_REG1_G_BIT::FS_G_2000DPS | CTRL_REG1_G_BIT::BW_G_100Hz);
|
||||
_px4_gyro.set_scale(math::radians(70.0f / 1000.0f)); // 70 mdps/LSB
|
||||
_px4_gyro.set_range(math::radians(2000.0f));
|
||||
|
||||
// Accelerometer configuration
|
||||
// CTRL_REG6_XL: Accelerometer 16 G range and ODR 952 Hz
|
||||
RegisterWrite(Register::CTRL_REG6_XL, CTRL_REG6_XL_BIT::ODR_XL_952HZ | CTRL_REG6_XL_BIT::FS_XL_16);
|
||||
_px4_accel.set_scale(0.732f * (CONSTANTS_ONE_G / 1000.0f)); // 0.732 mg/LSB
|
||||
_px4_accel.set_range(16.0f * CONSTANTS_ONE_G);
|
||||
|
||||
_state = STATE::RESET;
|
||||
ScheduleClear();
|
||||
ScheduleNow();
|
||||
return true;
|
||||
}
|
||||
|
||||
void LSM9DS1::ResetFIFO()
|
||||
void LSM9DS1::exit_and_cleanup()
|
||||
{
|
||||
perf_count(_fifo_reset_perf);
|
||||
I2CSPIDriverBase::exit_and_cleanup();
|
||||
}
|
||||
|
||||
// CTRL_REG9: disable FIFO
|
||||
RegisterClearBits(Register::CTRL_REG9, CTRL_REG9_BIT::FIFO_EN);
|
||||
void LSM9DS1::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
|
||||
// FIFO_CTRL: to reset FIFO content, Bypass mode (0) should be selected
|
||||
RegisterWrite(Register::FIFO_CTRL, 0);
|
||||
PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
|
||||
|
||||
// CTRL_REG9: enable FIFO
|
||||
RegisterSetBits(Register::CTRL_REG9, CTRL_REG9_BIT::FIFO_EN);
|
||||
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);
|
||||
}
|
||||
|
||||
// CTRL_REG8: Note: When the FIFO is used, the IF_INC and BDU bits must be equal to 1.
|
||||
RegisterWrite(Register::CTRL_REG8, CTRL_REG8_BIT::BDU | CTRL_REG8_BIT::IF_ADD_INC);
|
||||
usleep(1);
|
||||
int LSM9DS1::probe()
|
||||
{
|
||||
const uint8_t whoami = RegisterRead(Register::WHO_AM_I);
|
||||
|
||||
// FIFO_CTRL: FIFO continuous mode enabled
|
||||
RegisterWrite(Register::FIFO_CTRL, FIFO_CTRL_BIT::FIFO_MODE_CONTINUOUS);
|
||||
usleep(1);
|
||||
if (whoami != WHO_AM_I_ID) {
|
||||
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void LSM9DS1::RunImpl()
|
||||
{
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
switch (_state) {
|
||||
case STATE::RESET:
|
||||
// PWR_MGMT_1: Device Reset
|
||||
RegisterWrite(Register::CTRL_REG8, CTRL_REG8_BIT::SW_RESET);
|
||||
_reset_timestamp = now;
|
||||
_failure_count = 0;
|
||||
_state = STATE::WAIT_FOR_RESET;
|
||||
ScheduleDelayed(100_ms);
|
||||
break;
|
||||
|
||||
case STATE::WAIT_FOR_RESET:
|
||||
if ((RegisterRead(Register::WHO_AM_I) == WHO_AM_I_ID)) {
|
||||
|
||||
// Disable I2C, wakeup, and reset digital signal path
|
||||
RegisterWrite(Register::CTRL_REG9, CTRL_REG9_BIT::I2C_DISABLE); // set immediately to prevent switching into I2C mode
|
||||
|
||||
// if reset succeeded then configure
|
||||
_state = STATE::CONFIGURE;
|
||||
ScheduleDelayed(100_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: {
|
||||
// always check current FIFO count
|
||||
bool success = false;
|
||||
// Number of unread words (16-bit axes) stored in FIFO.
|
||||
const uint8_t FIFO_SRC = RegisterRead(Register::FIFO_SRC);
|
||||
const uint8_t samples = FIFO_SRC & static_cast<uint8_t>(FIFO_SRC_BIT::FSS);
|
||||
|
||||
if (FIFO_SRC & FIFO_SRC_BIT::OVRN) {
|
||||
// overflow
|
||||
FIFOReset();
|
||||
perf_count(_fifo_overflow_perf);
|
||||
|
||||
} else if (samples == 0) {
|
||||
perf_count(_fifo_empty_perf);
|
||||
|
||||
} else {
|
||||
if (samples > FIFO_MAX_SAMPLES) {
|
||||
// not technically an overflow, but more samples than we expected or can publish
|
||||
FIFOReset();
|
||||
perf_count(_fifo_overflow_perf);
|
||||
|
||||
} else if (samples >= 1) {
|
||||
if (FIFORead(now, 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 (~1 Hz)
|
||||
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
|
||||
UpdateTemperature();
|
||||
_temperature_update_timestamp = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void LSM9DS1::ConfigureSampleRate(int sample_rate)
|
||||
{
|
||||
if (sample_rate == 0) {
|
||||
sample_rate = ST_LSM9DS1::LA_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_gyro_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_gyro_samples * (1e6f / GYRO_RATE);
|
||||
}
|
||||
|
||||
bool LSM9DS1::Configure()
|
||||
{
|
||||
// first set and clear all configured register bits
|
||||
for (const auto ®_cfg : _register_cfg) {
|
||||
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
|
||||
}
|
||||
|
||||
// now check that all are configured
|
||||
bool success = true;
|
||||
|
||||
for (const auto ®_cfg : _register_cfg) {
|
||||
if (!RegisterCheck(reg_cfg)) {
|
||||
success = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Gyroscope configuration 2000 degrees/second
|
||||
_px4_gyro.set_scale(math::radians(70.f / 1000.f)); // 70 mdps/LSB
|
||||
_px4_gyro.set_range(math::radians(2000.f));
|
||||
|
||||
// Accelerometer configuration 16 G range
|
||||
_px4_accel.set_scale(0.732f * (CONSTANTS_ONE_G / 1000.f)); // 0.732 mg/LSB
|
||||
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
bool LSM9DS1::RegisterCheck(const register_config_t ®_cfg)
|
||||
{
|
||||
bool success = true;
|
||||
|
||||
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
|
||||
|
||||
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
|
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
|
||||
success = false;
|
||||
}
|
||||
|
||||
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
|
||||
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
|
||||
success = false;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
uint8_t LSM9DS1::RegisterRead(Register reg)
|
||||
@@ -147,166 +313,137 @@ void LSM9DS1::RegisterWrite(Register reg, uint8_t value)
|
||||
transfer(cmd, cmd, sizeof(cmd));
|
||||
}
|
||||
|
||||
void LSM9DS1::RegisterSetBits(Register reg, uint8_t setbits)
|
||||
void LSM9DS1::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
|
||||
{
|
||||
uint8_t val = RegisterRead(reg);
|
||||
const uint8_t orig_val = RegisterRead(reg);
|
||||
|
||||
if (!(val & setbits)) {
|
||||
val |= setbits;
|
||||
uint8_t val = (orig_val & ~clearbits) | setbits;
|
||||
|
||||
if (orig_val != val) {
|
||||
RegisterWrite(reg, val);
|
||||
}
|
||||
}
|
||||
|
||||
void LSM9DS1::RegisterClearBits(Register reg, uint8_t clearbits)
|
||||
bool LSM9DS1::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples)
|
||||
{
|
||||
uint8_t val = RegisterRead(reg);
|
||||
sensor_gyro_fifo_s gyro{};
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
gyro.samples = 0;
|
||||
gyro.dt = FIFO_SAMPLE_DT;
|
||||
|
||||
if (val & clearbits) {
|
||||
val &= !clearbits;
|
||||
RegisterWrite(reg, val);
|
||||
}
|
||||
}
|
||||
|
||||
void LSM9DS1::Start()
|
||||
{
|
||||
ResetFIFO();
|
||||
|
||||
ScheduleOnInterval(_fifo_interval / 2, _fifo_interval);
|
||||
}
|
||||
|
||||
void LSM9DS1::RunImpl()
|
||||
{
|
||||
perf_count(_interval_perf);
|
||||
|
||||
// Number of unread words (16-bit axes) stored in FIFO.
|
||||
const hrt_abstime timestamp_fifo_level = hrt_absolute_time();
|
||||
const uint8_t FIFO_SRC = RegisterRead(Register::FIFO_SRC);
|
||||
|
||||
if (FIFO_SRC & FIFO_SRC_BIT::OVRN) {
|
||||
// overflow
|
||||
perf_count(_fifo_overflow_perf);
|
||||
ResetFIFO();
|
||||
return;
|
||||
}
|
||||
|
||||
const uint8_t samples = FIFO_SRC & static_cast<uint8_t>(FIFO_SRC_BIT::FSS);
|
||||
|
||||
if (samples < 1) {
|
||||
perf_count(_fifo_empty_perf);
|
||||
return;
|
||||
|
||||
} else if (samples > 16) {
|
||||
// not technically an overflow, but more samples than we expected
|
||||
perf_count(_fifo_overflow_perf);
|
||||
ResetFIFO();
|
||||
return;
|
||||
}
|
||||
|
||||
// estimate timestamp of first sample in the FIFO from number of samples and fill rate
|
||||
const hrt_abstime timestamp_sample = timestamp_fifo_level;
|
||||
|
||||
int16_t accel_data[3] {};
|
||||
int16_t gyro_data[3] {};
|
||||
int accel_samples = 0;
|
||||
int gyro_samples = 0;
|
||||
|
||||
perf_begin(_transfer_perf);
|
||||
sensor_accel_fifo_s accel{};
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.samples = 0;
|
||||
accel.dt = FIFO_SAMPLE_DT;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
// Gyro
|
||||
{
|
||||
struct GyroReport {
|
||||
uint8_t cmd;
|
||||
uint8_t STATUS_REG;
|
||||
uint8_t OUT_X_L_G;
|
||||
uint8_t OUT_X_H_G;
|
||||
uint8_t OUT_Y_L_G;
|
||||
uint8_t OUT_Y_H_G;
|
||||
uint8_t OUT_Z_L_G;
|
||||
uint8_t OUT_Z_H_G;
|
||||
} greport{};
|
||||
greport.cmd = static_cast<uint8_t>(Register::STATUS_REG_G) | DIR_READ;
|
||||
struct GyroTransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::OUT_X_L_G) | DIR_READ};
|
||||
uint8_t OUT_X_L_G{0};
|
||||
uint8_t OUT_X_H_G{0};
|
||||
uint8_t OUT_Y_L_G{0};
|
||||
uint8_t OUT_Y_H_G{0};
|
||||
uint8_t OUT_Z_L_G{0};
|
||||
uint8_t OUT_Z_H_G{0};
|
||||
} buffer{};
|
||||
|
||||
if (transfer((uint8_t *)&greport, (uint8_t *)&greport, sizeof(GyroReport)) != PX4_OK) {
|
||||
perf_end(_transfer_perf);
|
||||
return;
|
||||
}
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {
|
||||
const int16_t gyro_x = combine(buffer.OUT_X_H_G, buffer.OUT_X_L_G);
|
||||
const int16_t gyro_y = combine(buffer.OUT_Y_H_G, buffer.OUT_Y_L_G);
|
||||
const int16_t gyro_z = combine(buffer.OUT_Z_H_G, buffer.OUT_Z_L_G);
|
||||
|
||||
if (greport.STATUS_REG & STATUS_REG_BIT::GDA) {
|
||||
// Gyroscope new data available
|
||||
// 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[gyro.samples] = gyro_x;
|
||||
gyro.y[gyro.samples] = gyro_y;
|
||||
gyro.z[gyro.samples] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
|
||||
gyro.samples++;
|
||||
|
||||
// sensor Z is up (RHC), flip z for publication
|
||||
gyro_data[0] = combine(greport.OUT_X_L_G, greport.OUT_X_H_G);
|
||||
gyro_data[1] = combine(greport.OUT_Y_L_G, greport.OUT_Y_H_G);
|
||||
gyro_data[2] = -combine(greport.OUT_Z_L_G, greport.OUT_Z_H_G);
|
||||
gyro_samples++;
|
||||
} else {
|
||||
perf_count(_bad_transfer_perf);
|
||||
}
|
||||
}
|
||||
|
||||
// Accel
|
||||
{
|
||||
struct AccelReport {
|
||||
uint8_t cmd;
|
||||
uint8_t STATUS_REG;
|
||||
uint8_t OUT_X_L_XL;
|
||||
uint8_t OUT_X_H_XL;
|
||||
uint8_t OUT_Y_L_XL;
|
||||
uint8_t OUT_Y_H_XL;
|
||||
uint8_t OUT_Z_L_XL;
|
||||
uint8_t OUT_Z_H_XL;
|
||||
} areport{};
|
||||
areport.cmd = static_cast<uint8_t>(Register::STATUS_REG_A) | DIR_READ;
|
||||
struct AccelTransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::OUT_X_L_XL) | DIR_READ};
|
||||
uint8_t OUT_X_L_XL{0};
|
||||
uint8_t OUT_X_H_XL{0};
|
||||
uint8_t OUT_Y_L_XL{0};
|
||||
uint8_t OUT_Y_H_XL{0};
|
||||
uint8_t OUT_Z_L_XL{0};
|
||||
uint8_t OUT_Z_H_XL{0};
|
||||
} buffer{};
|
||||
|
||||
if (transfer((uint8_t *)&areport, (uint8_t *)&areport, sizeof(AccelReport)) != PX4_OK) {
|
||||
perf_end(_transfer_perf);
|
||||
return;
|
||||
}
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {
|
||||
const int16_t accel_x = combine(buffer.OUT_X_H_XL, buffer.OUT_X_L_XL);
|
||||
const int16_t accel_y = combine(buffer.OUT_Y_H_XL, buffer.OUT_Y_L_XL);
|
||||
const int16_t accel_z = combine(buffer.OUT_Z_H_XL, buffer.OUT_Z_L_XL);
|
||||
|
||||
if (areport.STATUS_REG & STATUS_REG_BIT::XLDA) {
|
||||
// Accelerometer new data available
|
||||
// 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[accel.samples] = accel_x;
|
||||
accel.y[accel.samples] = accel_y;
|
||||
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
|
||||
accel.samples++;
|
||||
|
||||
// sensor Z is up (RHC), flip z for publication
|
||||
accel_data[0] = combine(areport.OUT_X_L_XL, areport.OUT_X_H_XL);
|
||||
accel_data[1] = combine(areport.OUT_Y_L_XL, areport.OUT_Y_H_XL);
|
||||
accel_data[2] = -combine(areport.OUT_Z_L_XL, areport.OUT_Z_H_XL);
|
||||
accel_samples++;
|
||||
} else {
|
||||
perf_count(_bad_transfer_perf);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_transfer_perf);
|
||||
if (gyro.samples > 0) {
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
|
||||
_px4_gyro.updateFIFO(gyro);
|
||||
}
|
||||
|
||||
// get current temperature at 1 Hz
|
||||
if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) {
|
||||
uint8_t temperature_buf[3] {};
|
||||
temperature_buf[0] = static_cast<uint8_t>(Register::OUT_TEMP_L) | DIR_READ;
|
||||
if (accel.samples > 0) {
|
||||
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
|
||||
_px4_accel.updateFIFO(accel);
|
||||
}
|
||||
|
||||
if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
|
||||
return;
|
||||
return (accel.samples > 0) && (gyro.samples > 0);
|
||||
}
|
||||
|
||||
void LSM9DS1::FIFOReset()
|
||||
{
|
||||
perf_count(_fifo_reset_perf);
|
||||
|
||||
// FIFO_CTRL: to reset FIFO content, Bypass mode (0) should be selected
|
||||
RegisterWrite(Register::FIFO_CTRL, 0);
|
||||
|
||||
// After this reset command, it is possible to restart FIFO mode by writing FIFO_CTRL (2Eh) (FMODE [2:0]) to '001'.
|
||||
for (auto &r : _register_cfg) {
|
||||
if ((r.reg == Register::CTRL_REG8) || (r.reg == Register::CTRL_REG9) || (r.reg == Register::FIFO_CTRL)) {
|
||||
RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 16 bits in two’s complement format with a sensitivity of 256 LSB/°C. The output zero level corresponds to 25 °C.
|
||||
const int16_t OUT_TEMP = combine(temperature_buf[1], temperature_buf[2] & 0x0F);
|
||||
const float temperature = (OUT_TEMP / 256.0f) + 25.0f;
|
||||
void LSM9DS1::UpdateTemperature()
|
||||
{
|
||||
// read current temperature
|
||||
struct TransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::OUT_TEMP_L) | DIR_READ};
|
||||
uint8_t OUT_TEMP_L{0};
|
||||
uint8_t OUT_TEMP_H{0};
|
||||
} buffer{};
|
||||
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) {
|
||||
perf_count(_bad_transfer_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
// 16 bits in two’s complement format with a sensitivity of 256 LSB/°C. The output zero level corresponds to 25 °C.
|
||||
const int16_t OUT_TEMP = combine(buffer.OUT_TEMP_H, buffer.OUT_TEMP_L);
|
||||
const float temperature = (OUT_TEMP / 256.0f) + 25.0f;
|
||||
|
||||
if (PX4_ISFINITE(temperature)) {
|
||||
_px4_accel.set_temperature(temperature);
|
||||
_px4_gyro.set_temperature(temperature);
|
||||
}
|
||||
|
||||
if ((accel_samples > 0) && (gyro_samples > 0)) {
|
||||
// published synchronized IMU
|
||||
_px4_accel.update(timestamp_sample, accel_data[0], accel_data[1], accel_data[2]);
|
||||
_px4_gyro.update(timestamp_sample, gyro_data[0], gyro_data[1], gyro_data[2]);
|
||||
}
|
||||
}
|
||||
|
||||
void LSM9DS1::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
perf_print_counter(_interval_perf);
|
||||
perf_print_counter(_transfer_perf);
|
||||
perf_print_counter(_fifo_empty_perf);
|
||||
perf_print_counter(_fifo_overflow_perf);
|
||||
perf_print_counter(_fifo_reset_perf);
|
||||
|
||||
}
|
||||
|
||||
@@ -48,11 +48,9 @@
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
|
||||
using ST_LSM9DS1::Register;
|
||||
using namespace ST_LSM9DS1;
|
||||
|
||||
class LSM9DS1 : public device::SPI, public I2CSPIDriver<LSM9DS1>
|
||||
{
|
||||
@@ -64,34 +62,81 @@ public:
|
||||
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance);
|
||||
static void print_usage();
|
||||
void print_status();
|
||||
|
||||
int init() override;
|
||||
void RunImpl();
|
||||
|
||||
void Start();
|
||||
bool Reset();
|
||||
int init() override;
|
||||
void print_status() override;
|
||||
|
||||
void RunImpl();
|
||||
private:
|
||||
int probe() override;
|
||||
void exit_and_cleanup() override;
|
||||
|
||||
uint8_t RegisterRead(Register reg);
|
||||
void RegisterWrite(Register reg, uint8_t value);
|
||||
void RegisterSetBits(Register reg, uint8_t setbits);
|
||||
void RegisterClearBits(Register reg, uint8_t clearbits);
|
||||
// Sensor Configuration
|
||||
static constexpr float FIFO_SAMPLE_DT{1e6f / ST_LSM9DS1::G_ODR};
|
||||
static constexpr float GYRO_RATE{ST_LSM9DS1::G_ODR}; // 952 Hz gyro
|
||||
static constexpr float ACCEL_RATE{ST_LSM9DS1::LA_ODR}; // 952 Hz accel
|
||||
|
||||
void ResetFIFO();
|
||||
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / 12, sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
|
||||
|
||||
struct register_config_t {
|
||||
Register reg;
|
||||
uint8_t set_bits{0};
|
||||
uint8_t clear_bits{0};
|
||||
};
|
||||
|
||||
int probe() override;
|
||||
|
||||
bool Reset();
|
||||
|
||||
bool Configure();
|
||||
void ConfigureSampleRate(int sample_rate);
|
||||
|
||||
bool RegisterCheck(const register_config_t ®_cfg);
|
||||
|
||||
uint8_t RegisterRead(Register reg);
|
||||
void RegisterWrite(Register reg, uint8_t value);
|
||||
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
|
||||
|
||||
uint16_t FIFOReadCount();
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples);
|
||||
void FIFOReset();
|
||||
|
||||
void UpdateTemperature();
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
static constexpr uint32_t _fifo_interval{1000000 / ST_LSM9DS1::LA_ODR};
|
||||
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 _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")};
|
||||
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
|
||||
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")};
|
||||
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo overflow")};
|
||||
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo reset")};
|
||||
hrt_abstime _reset_timestamp{0};
|
||||
hrt_abstime _last_config_check_timestamp{0};
|
||||
hrt_abstime _temperature_update_timestamp{0};
|
||||
int _failure_count{0};
|
||||
|
||||
hrt_abstime _time_last_temperature_update{0};
|
||||
enum class STATE : uint8_t {
|
||||
RESET,
|
||||
WAIT_FOR_RESET,
|
||||
CONFIGURE,
|
||||
FIFO_READ,
|
||||
} _state{STATE::RESET};
|
||||
|
||||
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
|
||||
uint32_t _fifo_gyro_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::CTRL_REG1_G, CTRL_REG1_G_BIT::ODR_G_952HZ | CTRL_REG1_G_BIT::FS_G_2000DPS | CTRL_REG1_G_BIT::BW_G_100Hz, 0 },
|
||||
{ Register::CTRL_REG6_XL, CTRL_REG6_XL_BIT::ODR_XL_952HZ | CTRL_REG6_XL_BIT::FS_XL_16, 0 },
|
||||
{ Register::CTRL_REG7_XL, CTRL_REG7_XL_BIT::HR, CTRL_REG7_XL_BIT::FDS },
|
||||
{ Register::CTRL_REG8, CTRL_REG8_BIT::BDU | CTRL_REG8_BIT::IF_ADD_INC, CTRL_REG8_BIT::SW_RESET },
|
||||
{ Register::CTRL_REG9, CTRL_REG9_BIT::FIFO_EN | CTRL_REG9_BIT::I2C_DISABLE, 0 },
|
||||
{ Register::FIFO_CTRL, FIFO_CTRL_BIT::FMODE_CONTINUOUS, 0 },
|
||||
};
|
||||
};
|
||||
|
||||
@@ -54,21 +54,20 @@ static constexpr uint8_t Bit7 = (1 << 7);
|
||||
|
||||
namespace ST_LSM9DS1
|
||||
{
|
||||
static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10 MHz SPI clock frequency
|
||||
|
||||
static constexpr uint8_t DIR_READ = 0x80;
|
||||
|
||||
static constexpr uint8_t LSM9DS1_WHO_AM_I = 0b01101000; // Who I am ID
|
||||
|
||||
static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10 MHz SPI clock frequency
|
||||
static constexpr uint8_t WHO_AM_I_ID = 0b01101000; // Who I am ID
|
||||
|
||||
static constexpr uint32_t LA_ODR = 952; // Linear acceleration output data rate
|
||||
static constexpr uint32_t G_ODR = 952; // Angular rate output data rate
|
||||
static constexpr uint32_t G_ODR = 952; // Angular rate output data rate
|
||||
|
||||
enum class
|
||||
Register : uint8_t {
|
||||
enum class Register : uint8_t {
|
||||
WHO_AM_I = 0x0F,
|
||||
|
||||
CTRL_REG1_G = 0x10, // Angular rate sensor Control Register 1.
|
||||
CTRL_REG2_G = 0x11, // Angular rate sensor Control Register 2.
|
||||
|
||||
OUT_TEMP_L = 0x15,
|
||||
OUT_TEMP_H = 0x16,
|
||||
@@ -81,7 +80,7 @@ Register : uint8_t {
|
||||
OUT_Z_H_G = 0x1D,
|
||||
|
||||
CTRL_REG6_XL = 0x20, // Linear acceleration sensor Control Register 6.
|
||||
|
||||
CTRL_REG7_XL = 0x21, // Linear acceleration sensor Control Register 7.
|
||||
CTRL_REG8 = 0x22, // Control register 8.
|
||||
CTRL_REG9 = 0x23, // Control register 9.
|
||||
|
||||
@@ -97,52 +96,59 @@ Register : uint8_t {
|
||||
};
|
||||
|
||||
// CTRL_REG1_G
|
||||
enum
|
||||
CTRL_REG1_G_BIT : uint8_t {
|
||||
ODR_G_952HZ = Bit7 | Bit6,
|
||||
enum CTRL_REG1_G_BIT : uint8_t {
|
||||
// ODR_G [2:0]
|
||||
ODR_G_952HZ = Bit7 | Bit6, // 952 Hz ODR
|
||||
// FS_G [1:0]
|
||||
FS_G_2000DPS = Bit4 | Bit3,
|
||||
BW_G_100Hz = Bit1 | Bit2, // BW_G 100 Hz
|
||||
// BW_G [1:0]
|
||||
BW_G_100Hz = Bit1 | Bit0, // BW_G 100 Hz
|
||||
};
|
||||
|
||||
// STATUS_REG (both STATUS_REG_A 0x17 and STATUS_REG_G 0x27)
|
||||
enum
|
||||
STATUS_REG_BIT : uint8_t {
|
||||
enum STATUS_REG_BIT : uint8_t {
|
||||
TDA = Bit2, // Temperature sensor new data available.
|
||||
GDA = Bit1, // Gyroscope new data available.
|
||||
XLDA = Bit0, // Accelerometer new data available.
|
||||
};
|
||||
|
||||
// CTRL_REG6_XL
|
||||
enum
|
||||
CTRL_REG6_XL_BIT : uint8_t {
|
||||
ODR_XL_952HZ = Bit7 | Bit6, // 952 Hz ODR
|
||||
FS_XL_16 = Bit3, // FS_XL 01: ±16 g
|
||||
enum CTRL_REG6_XL_BIT : uint8_t {
|
||||
// ODR_XL [2:0]
|
||||
ODR_XL_952HZ = Bit7 | Bit6, // 952 Hz ODR
|
||||
// FS_XL [1:0]
|
||||
FS_XL_16 = Bit3, // FS_XL 01: ±16 g
|
||||
};
|
||||
|
||||
// CTRL_REG7_XL
|
||||
enum CTRL_REG7_XL_BIT : uint8_t {
|
||||
HR = Bit7, // High resolution mode for accelerometer enable.
|
||||
FDS = Bit2, // Filtered data selection. 0: internal filter bypassed
|
||||
};
|
||||
|
||||
// CTRL_REG8
|
||||
enum
|
||||
CTRL_REG8_BIT : uint8_t {
|
||||
enum CTRL_REG8_BIT : uint8_t {
|
||||
BDU = Bit6, // Block data update
|
||||
|
||||
IF_ADD_INC = Bit2, // Register address automatically incremented
|
||||
|
||||
SW_RESET = Bit0, // Software reset
|
||||
};
|
||||
|
||||
// CTRL_REG9
|
||||
enum
|
||||
CTRL_REG9_BIT : uint8_t {
|
||||
enum CTRL_REG9_BIT : uint8_t {
|
||||
I2C_DISABLE = Bit2,
|
||||
FIFO_EN = Bit1,
|
||||
};
|
||||
|
||||
// FIFO_CTRL
|
||||
enum
|
||||
FIFO_CTRL_BIT : uint8_t {
|
||||
FIFO_MODE_CONTINUOUS = Bit7 | Bit6, // Continuous mode. If the FIFO is full, the new sample over-writes the older sample.
|
||||
enum FIFO_CTRL_BIT : uint8_t {
|
||||
// FMODE [2:0]
|
||||
FMODE_CONTINUOUS = Bit7 | Bit6, // Continuous mode. If the FIFO is full, the new sample over- writes the older sample.
|
||||
};
|
||||
|
||||
// FIFO_SRC
|
||||
enum
|
||||
FIFO_SRC_BIT : uint8_t {
|
||||
enum FIFO_SRC_BIT : uint8_t {
|
||||
OVRN = Bit6, // FIFO overrun status.
|
||||
FSS = Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0,
|
||||
};
|
||||
@@ -150,7 +156,7 @@ FIFO_SRC_BIT : uint8_t {
|
||||
|
||||
namespace FIFO
|
||||
{
|
||||
static constexpr size_t SIZE = 32 * 6; // 32 samples max
|
||||
static constexpr size_t SIZE = 32 * 12; // 32 samples max
|
||||
}
|
||||
|
||||
} // namespace ST_LSM9DS1
|
||||
|
||||
@@ -36,13 +36,23 @@
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
void LSM9DS1::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("lsm9ds1", "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();
|
||||
}
|
||||
|
||||
I2CSPIDriverBase *LSM9DS1::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance)
|
||||
{
|
||||
LSM9DS1 *instance = new LSM9DS1(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
|
||||
cli.bus_frequency, cli.spi_mode);
|
||||
|
||||
if (instance == nullptr) {
|
||||
if (!instance) {
|
||||
PX4_ERR("alloc failed");
|
||||
return nullptr;
|
||||
}
|
||||
@@ -55,23 +65,12 @@ I2CSPIDriverBase *LSM9DS1::instantiate(const BusCLIArguments &cli, const BusInst
|
||||
return instance;
|
||||
}
|
||||
|
||||
void
|
||||
LSM9DS1::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("lsm9ds1", "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" __EXPORT int lsm9ds1_main(int argc, char *argv[])
|
||||
extern "C" int lsm9ds1_main(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
using ThisDriver = LSM9DS1;
|
||||
BusCLIArguments cli{false, true};
|
||||
cli.default_spi_frequency = ST_LSM9DS1::SPI_SPEED;
|
||||
cli.default_spi_frequency = SPI_SPEED;
|
||||
|
||||
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
|
||||
switch (ch) {
|
||||
|
||||
Reference in New Issue
Block a user