diff --git a/src/drivers/imu/st/lsm9ds1/CMakeLists.txt b/src/drivers/imu/st/lsm9ds1/CMakeLists.txt index dea6ed2af3..fee777206e 100644 --- a/src/drivers/imu/st/lsm9ds1/CMakeLists.txt +++ b/src/drivers/imu/st/lsm9ds1/CMakeLists.txt @@ -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 diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp index d893121ec7..6a12447992 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp @@ -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(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(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(Register::STATUS_REG_G) | DIR_READ; + struct GyroTransferBuffer { + uint8_t cmd{static_cast(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(Register::STATUS_REG_A) | DIR_READ; + struct AccelTransferBuffer { + uint8_t cmd{static_cast(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(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(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); - } diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp index bda45ed4b5..15bbc3be6e 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp @@ -48,11 +48,9 @@ #include #include #include -#include -#include #include -using ST_LSM9DS1::Register; +using namespace ST_LSM9DS1; class LSM9DS1 : public device::SPI, public I2CSPIDriver { @@ -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(_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 }, + }; }; diff --git a/src/drivers/imu/st/lsm9ds1/ST_LSM9DS1_Registers.hpp b/src/drivers/imu/st/lsm9ds1/ST_LSM9DS1_Registers.hpp index 06364de27b..7530dd54a6 100644 --- a/src/drivers/imu/st/lsm9ds1/ST_LSM9DS1_Registers.hpp +++ b/src/drivers/imu/st/lsm9ds1/ST_LSM9DS1_Registers.hpp @@ -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 diff --git a/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp b/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp index c1a55ffc2e..24dbc8d331 100644 --- a/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp +++ b/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp @@ -36,13 +36,23 @@ #include #include +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) {