diff --git a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp index 273fba9320..3e4260659e 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -33,12 +33,12 @@ #include "LSM9DS1_MAG.hpp" -using namespace ST_LSM9DS1_MAG; -using ST_LSM9DS1_MAG::Register; - using namespace time_literals; -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_MAG::LSM9DS1_MAG(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, spi_mode_e spi_mode) : @@ -51,18 +51,8 @@ LSM9DS1_MAG::LSM9DS1_MAG(I2CSPIBusOption bus_option, int bus, uint32_t device, e LSM9DS1_MAG::~LSM9DS1_MAG() { - perf_free(_interval_perf); - perf_free(_transfer_perf); - perf_free(_data_overrun_perf); -} - -int LSM9DS1_MAG::probe() -{ - if (RegisterRead(Register::WHO_AM_I) == LSM9DS1_MAG_WHO_AM_I) { - return PX4_OK; - } - - return PX4_ERROR; + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); } int LSM9DS1_MAG::init() @@ -70,46 +60,205 @@ int LSM9DS1_MAG::init() int ret = SPI::init(); if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); return ret; } - if (!Reset()) { - PX4_ERR("reset failed"); - return PX4_ERROR; - } - - Start(); - - return PX4_OK; + return Reset() ? 0 : -1; } bool LSM9DS1_MAG::Reset() { - // Reset - // CTRL_REG2_M: SOFT_RST - RegisterWrite(Register::CTRL_REG2_M, CTRL_REG2_M_BIT::SOFT_RST); - usleep(50); - - // CTRL_REG1_M: Temp comp, ultra high perofmrance mode, ODR 80 Hz, fast ODR - RegisterWrite(Register::CTRL_REG1_M, - CTRL_REG1_M_BIT::TEMP_COMP | CTRL_REG1_M_BIT::OM_ULTRA_HIGH_PERFORMANCE | CTRL_REG1_M_BIT::DO_80HZ); - - // CTRL_REG2_M: - RegisterSetBits(Register::CTRL_REG2_M, CTRL_REG2_M_BIT::FS_16_GAUSS); - _px4_mag.set_scale(0.58f / 1000.0f); // Magnetic FS = ±16 gauss 0.58 mgauss/LSB - - // CTRL_REG3_M: I2C_DISABLE, Continuous-conversion mode - RegisterClearBits(Register::CTRL_REG3_M, CTRL_REG3_M_BIT::MD_CONTINUOUS_MODE); - - // CTRL_REG4_M: Z-axis Ultra-high performance mode - RegisterSetBits(Register::CTRL_REG4_M, CTRL_REG4_M_BIT::OMZ_ULTRA_HIGH_PERFORMANCE); - - // CTRL_REG5_M: Block data update for magnetic data. - RegisterSetBits(Register::CTRL_REG5_M, CTRL_REG5_M_BIT::BDU); - + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); return true; } +void LSM9DS1_MAG::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); +} + +int LSM9DS1_MAG::probe() +{ + const uint8_t WHO_AM_I_M = RegisterRead(Register::WHO_AM_I_M); + + if (WHO_AM_I_M != Device_identification) { + DEVICE_DEBUG("unexpected WHO_AM_I_M 0x%02x", WHO_AM_I_M); + return PX4_ERROR; + } + + return PX4_OK; +} + +void LSM9DS1_MAG::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // CTRL_REG2_M: SOFT_RST + RegisterWrite(Register::CTRL_REG2_M, CTRL_REG2_M_BIT::SOFT_RST); + _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_M) == Device_identification) { + // 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 100 ms"); + ScheduleDelayed(100_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading + _state = STATE::READ; + ScheduleOnInterval(1000000 / ST_LSM9DS1_MAG::M_ODR); + + } 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::READ: { + struct TransferBuffer { + uint8_t cmd; + uint8_t STATUS_REG_M; + uint8_t OUT_X_L_M; + uint8_t OUT_X_H_M; + uint8_t OUT_Y_L_M; + uint8_t OUT_Y_H_M; + uint8_t OUT_Z_L_M; + uint8_t OUT_Z_H_M; + } buffer{}; + + buffer.cmd = static_cast(Register::STATUS_REG_M) | RW_BIT_READ | MS_BIT_AUTO_INCREMENT; + + bool success = false; + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) { + if (buffer.STATUS_REG_M & STATUS_REG_M_BIT::ZYXDA) { + // X, Y and Z-axis new data available. + int16_t x = combine(buffer.OUT_X_H_M, buffer.OUT_X_L_M); + int16_t y = combine(buffer.OUT_Y_H_M, buffer.OUT_Y_L_M); + int16_t z = combine(buffer.OUT_Z_H_M, buffer.OUT_Z_L_M); + + // sensor Z is up (RHC), flip z for publication + // sensor X is aligned with -X of lsm9ds1 accel/gyro + x = (x == INT16_MIN) ? INT16_MAX : -x; + y = y; + z = (z == INT16_MIN) ? INT16_MAX : -z; + + _px4_mag.update(now, x, y, z); + + 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(); + } + } + } + + break; + } +} + +bool LSM9DS1_MAG::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; + } + } + + // Magnetic FS = ±16 gauss 0.58 mgauss/LSB + _px4_mag.set_scale(0.58f / 1000.0f); + + return success; +} + +bool LSM9DS1_MAG::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_MAG::RegisterRead(Register reg) { uint8_t cmd[2] {}; @@ -124,80 +273,13 @@ void LSM9DS1_MAG::RegisterWrite(Register reg, uint8_t value) transfer(cmd, cmd, sizeof(cmd)); } -void LSM9DS1_MAG::RegisterSetBits(Register reg, uint8_t setbits) +void LSM9DS1_MAG::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_MAG::RegisterClearBits(Register reg, uint8_t clearbits) -{ - uint8_t val = RegisterRead(reg); - - if (val & clearbits) { - val &= !clearbits; - RegisterWrite(reg, val); - } -} - -void LSM9DS1_MAG::Start() -{ - ScheduleOnInterval(1000000 / ST_LSM9DS1_MAG::M_ODR / 2); -} - -void LSM9DS1_MAG::RunImpl() -{ - perf_count(_interval_perf); - - struct MagReport { - uint8_t cmd; - uint8_t STATUS_REG_M; - uint8_t OUT_X_L_M; - uint8_t OUT_X_H_M; - uint8_t OUT_Y_L_M; - uint8_t OUT_Y_H_M; - uint8_t OUT_Z_L_M; - uint8_t OUT_Z_H_M; - } mreport{}; - mreport.cmd = static_cast(Register::STATUS_REG_M) | RW_BIT_READ | MS_BIT_AUTO_INCREMENT; - - perf_begin(_transfer_perf); - const hrt_abstime timestamp_sample = hrt_absolute_time(); - - if (transfer((uint8_t *)&mreport, (uint8_t *)&mreport, sizeof(MagReport)) != PX4_OK) { - perf_end(_transfer_perf); - return; - } - - perf_end(_transfer_perf); - - if (mreport.STATUS_REG_M & STATUS_REG_M_BIT::ZYXOR) { - // X, Y and Z-axis data overrun. - perf_count(_data_overrun_perf); - return; - } - - if (mreport.STATUS_REG_M & STATUS_REG_M_BIT::ZYXDA) { - // X, Y and Z-axis new data available. - - // sensor Z is up (RHC), flip z for publication - // sensor X is aligned with -X of lsm9ds1 accel/gyro - int16_t x = -combine(mreport.OUT_X_L_M, mreport.OUT_X_H_M); - int16_t y = combine(mreport.OUT_Y_L_M, mreport.OUT_Y_H_M); - int16_t z = -combine(mreport.OUT_Z_L_M, mreport.OUT_Z_H_M); - - _px4_mag.update(timestamp_sample, x, y, z); - } -} - -void LSM9DS1_MAG::print_status() -{ - I2CSPIDriverBase::print_status(); - perf_print_counter(_interval_perf); - perf_print_counter(_transfer_perf); - perf_print_counter(_data_overrun_perf); -} diff --git a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp index a0805d2036..e97c7aca7b 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -46,10 +46,10 @@ #include #include #include -#include -#include #include +using namespace ST_LSM9DS1_MAG; + class LSM9DS1_MAG : public device::SPI, public I2CSPIDriver { public: @@ -61,26 +61,55 @@ public: int runtime_instance); static void print_usage(); - void print_status(); + void RunImpl(); int init() override; - - void Start(); - bool Reset(); - - void RunImpl(); + void print_status() override; private: - int probe() override; - uint8_t RegisterRead(ST_LSM9DS1_MAG::Register reg); - void RegisterWrite(ST_LSM9DS1_MAG::Register reg, uint8_t value); - void RegisterSetBits(ST_LSM9DS1_MAG::Register reg, uint8_t setbits); - void RegisterClearBits(ST_LSM9DS1_MAG::Register reg, uint8_t clearbits); + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + + 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); PX4Magnetometer _px4_mag; - 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 _data_overrun_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": overrun")}; + 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")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + int _failure_count{0}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + 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_M, CTRL_REG1_M_BIT::TEMP_COMP | CTRL_REG1_M_BIT::OM_ULTRA_HIGH_PERFORMANCE | CTRL_REG1_M_BIT::DO_80HZ, CTRL_REG1_M_BIT::FAST_ODR | CTRL_REG1_M_BIT::ST }, + { Register::CTRL_REG2_M, CTRL_REG2_M_BIT::FS_16_GAUSS, 0 }, + { Register::CTRL_REG3_M, CTRL_REG3_M_BIT::I2C_DISABLE, CTRL_REG3_M_BIT::SIM | CTRL_REG3_M_BIT::MD_CONTINUOUS_MODE }, + { Register::CTRL_REG4_M, CTRL_REG4_M_BIT::OMZ_ULTRA_HIGH_PERFORMANCE, 0 }, + { Register::CTRL_REG5_M, CTRL_REG5_M_BIT::BDU, 0 }, + }; }; diff --git a/src/drivers/magnetometer/lsm9ds1_mag/ST_LSM9DS1_MAG_Registers.hpp b/src/drivers/magnetometer/lsm9ds1_mag/ST_LSM9DS1_MAG_Registers.hpp index 79ec3668ac..018d22f02e 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/ST_LSM9DS1_MAG_Registers.hpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/ST_LSM9DS1_MAG_Registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -55,24 +55,23 @@ static constexpr uint8_t Bit5 = (1 << 5); static constexpr uint8_t Bit6 = (1 << 6); static constexpr uint8_t Bit7 = (1 << 7); +static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10 MHz SPI clock frequency + static constexpr uint8_t RW_BIT_READ = Bit7; static constexpr uint8_t MS_BIT_AUTO_INCREMENT = Bit6; -static constexpr uint8_t LSM9DS1_MAG_WHO_AM_I = 0b00111101; // Who I am ID - -static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10 MHz SPI clock frequency +static constexpr uint8_t Device_identification = 0b00111101; // Who I am ID static constexpr uint32_t M_ODR = 80; // Magnetometer output data rate -enum class -Register : uint8_t { - WHO_AM_I = 0x0F, +enum class Register : uint8_t { + WHO_AM_I_M = 0x0F, CTRL_REG1_M = 0x20, CTRL_REG2_M = 0x21, CTRL_REG3_M = 0x22, CTRL_REG4_M = 0x23, - CTRL_REG5_M = 0x25, + CTRL_REG5_M = 0x24, STATUS_REG_M = 0x27, OUT_X_L_M = 0x28, @@ -84,8 +83,7 @@ Register : uint8_t { }; // CTRL_REG1_M -enum -CTRL_REG1_M_BIT : uint8_t { +enum CTRL_REG1_M_BIT : uint8_t { TEMP_COMP = Bit7, // Temperature compensation enable. OM_ULTRA_HIGH_PERFORMANCE = Bit6 | Bit5, // X and Y axes operative mode selection. DO_80HZ = Bit4 | Bit3 | Bit2, // 80 Hz Output data rate selection. @@ -94,37 +92,30 @@ CTRL_REG1_M_BIT : uint8_t { }; // CTRL_REG2_M -enum -CTRL_REG2_M_BIT : uint8_t { +enum CTRL_REG2_M_BIT : uint8_t { FS_16_GAUSS = Bit6 | Bit5, // Full-scale selection ± 16 gauss - - SOFT_RST = Bit2, + SOFT_RST = Bit2, }; // CTRL_REG3_M -enum -CTRL_REG3_M_BIT : uint8_t { - I2C_DISABLE = Bit7, - - MD_CONTINUOUS_MODE = Bit1 | Bit0, +enum CTRL_REG3_M_BIT : uint8_t { + I2C_DISABLE = Bit7, + SIM = Bit2, // SPI Serial Interface mode selection. + MD_CONTINUOUS_MODE = Bit1 | Bit0, // Continuous-conversion mode }; // CTRL_REG4_M -enum -CTRL_REG4_M_BIT : uint8_t { +enum CTRL_REG4_M_BIT : uint8_t { OMZ_ULTRA_HIGH_PERFORMANCE = Bit4 | Bit3, // Ultra-high performance mode }; // CTRL_REG5_M -enum -CTRL_REG5_M_BIT : uint8_t { +enum CTRL_REG5_M_BIT : uint8_t { BDU = Bit6, // Block data update for magnetic data. }; - // STATUS_REG_M -enum -STATUS_REG_M_BIT : uint8_t { +enum STATUS_REG_M_BIT : uint8_t { ZYXOR = Bit7, // X, Y and Z-axis data overrun. ZYXDA = Bit3, // X, Y and Z-axis new data available. }; diff --git a/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp b/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp index 4aca1c0344..19811bd4a4 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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,8 +36,27 @@ #include #include -void -LSM9DS1_MAG::print_usage() +I2CSPIDriverBase *LSM9DS1_MAG::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + LSM9DS1_MAG *instance = new LSM9DS1_MAG(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (OK != instance->init()) { + delete instance; + PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + return nullptr; + } + + return instance; +} + +void LSM9DS1_MAG::print_usage() { PRINT_MODULE_USAGE_NAME("lsm9ds1_mag", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); @@ -47,32 +66,12 @@ LSM9DS1_MAG::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *LSM9DS1_MAG::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - LSM9DS1_MAG *instance = new LSM9DS1_MAG(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), - cli.rotation, cli.bus_frequency, cli.spi_mode); - - if (instance == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (OK != instance->init()) { - delete instance; - return nullptr; - } - - return instance; -} - - extern "C" __EXPORT int lsm9ds1_mag_main(int argc, char *argv[]) { - using ThisDriver = LSM9DS1_MAG; int ch; + using ThisDriver = LSM9DS1_MAG; BusCLIArguments cli{false, true}; - cli.default_spi_frequency = ST_LSM9DS1_MAG::SPI_SPEED; + cli.default_spi_frequency = SPI_SPEED; while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) {