From b147f8d8110973e566c02fbd107bb29510eda152 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 6 Jun 2023 16:09:44 -0400 Subject: [PATCH] drivers/magnetometer/qmc5883l: clarify set/clear bits and add overflow perf count --- .../magnetometer/qmc5883l/QMC5883L.cpp | 49 ++++++++++--------- .../magnetometer/qmc5883l/QMC5883L.hpp | 3 +- .../qmc5883l/QST_QMC5883L_registers.hpp | 10 ++-- 3 files changed, 35 insertions(+), 27 deletions(-) diff --git a/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp b/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp index 5da9accaa6..2da6a44b16 100644 --- a/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp +++ b/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp @@ -52,6 +52,7 @@ QMC5883L::~QMC5883L() perf_free(_reset_perf); perf_free(_bad_register_perf); perf_free(_bad_transfer_perf); + perf_free(_overflow_perf); } int QMC5883L::init() @@ -81,6 +82,7 @@ void QMC5883L::print_status() perf_print_counter(_reset_perf); perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); + perf_print_counter(_overflow_perf); } int QMC5883L::probe() @@ -137,8 +139,8 @@ void QMC5883L::RunImpl() ScheduleDelayed(100_ms); } else { - PX4_DEBUG("Reset not complete, check again in 10 ms"); - ScheduleDelayed(10_ms); + PX4_DEBUG("Reset not complete, check again in 100 ms"); + ScheduleDelayed(100_ms); } } @@ -180,32 +182,35 @@ void QMC5883L::RunImpl() uint8_t cmd = static_cast(Register::X_LSB); if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) { - // process data if successful transfer, no overflow - if ((buffer.STATUS & STATUS_BIT::OVL) == 0) { - int16_t x = combine(buffer.X_MSB, buffer.X_LSB); - int16_t y = combine(buffer.Y_MSB, buffer.Y_LSB); - int16_t z = combine(buffer.Z_MSB, buffer.Z_LSB); - if (x != _prev_data[0] || y != _prev_data[1] || z != _prev_data[2]) { - _prev_data[0] = x; - _prev_data[1] = y; - _prev_data[2] = z; + const int16_t x = combine(buffer.X_MSB, buffer.X_LSB); + const int16_t y = combine(buffer.Y_MSB, buffer.Y_LSB); + const int16_t z = combine(buffer.Z_MSB, buffer.Z_LSB); - // Sensor orientation - // Forward X := +X - // Right Y := -Y - // Down Z := -Z - y = (y == INT16_MIN) ? INT16_MAX : -y; // -y - z = (z == INT16_MIN) ? INT16_MAX : -z; // -z + const bool data_ready = (buffer.STATUS & STATUS_BIT::DRDY); + const bool overflow = (buffer.STATUS & STATUS_BIT::OVL); - _px4_mag.update(now, x, y, z); + const bool data_changed = ((x != _prev_data[0] || y != _prev_data[1] || z != _prev_data[2])); + _prev_data[0] = x; + _prev_data[1] = y; + _prev_data[2] = z; - success = true; + // publish data if successful transfer of new data, no overflow + if (data_ready && !overflow && data_changed) { + // Sensor orientation + // Forward X := +X + // Right Y := -Y + // Down Z := -Z + _px4_mag.update(now, x, math::negate(y), math::negate(z)); - if (_failure_count > 0) { - _failure_count--; - } + success = true; + + if (_failure_count > 0) { + _failure_count--; } + + } else if (overflow) { + perf_count(_overflow_perf); } } else { diff --git a/src/drivers/magnetometer/qmc5883l/QMC5883L.hpp b/src/drivers/magnetometer/qmc5883l/QMC5883L.hpp index 85209720af..8f316a9c29 100644 --- a/src/drivers/magnetometer/qmc5883l/QMC5883L.hpp +++ b/src/drivers/magnetometer/qmc5883l/QMC5883L.hpp @@ -88,6 +88,7 @@ private: 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 _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + perf_counter_t _overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": data overflow")}; hrt_abstime _reset_timestamp{0}; hrt_abstime _last_config_check_timestamp{0}; @@ -106,7 +107,7 @@ private: static constexpr uint8_t size_register_cfg{3}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits - { Register::CNTL1, CNTL1_BIT::ODR_50HZ | CNTL1_BIT::Mode_Continuous, CNTL1_BIT::OSR_512 | CNTL1_BIT::RNG_2G}, + { Register::CNTL1, CNTL1_BIT::ODR_50HZ_SET | CNTL1_BIT::Mode_Continuous_SET, CNTL1_BIT::OSR_512_CLEAR | CNTL1_BIT::RNG_2G_CLEAR | CNTL1_BIT::ODR_50HZ_CLEAR | CNTL1_BIT::Mode_Continuous_CLEAR}, { Register::CNTL2, CNTL2_BIT::ROL_PNT, CNTL2_BIT::SOFT_RST}, { Register::SET_RESET_PERIOD, SET_RESET_PERIOD_BIT::FBR, 0 }, }; diff --git a/src/drivers/magnetometer/qmc5883l/QST_QMC5883L_registers.hpp b/src/drivers/magnetometer/qmc5883l/QST_QMC5883L_registers.hpp index 094b1f1f8f..0363df3311 100644 --- a/src/drivers/magnetometer/qmc5883l/QST_QMC5883L_registers.hpp +++ b/src/drivers/magnetometer/qmc5883l/QST_QMC5883L_registers.hpp @@ -87,16 +87,18 @@ enum STATUS_BIT : uint8_t { // CNTL1 enum CNTL1_BIT : uint8_t { // OSR[7:6] - OSR_512 = Bit7 | Bit6, // 00 + OSR_512_CLEAR = Bit7 | Bit6, // 00: 512 // RNG[5:4] - RNG_2G = Bit5 | Bit4, // 00 + RNG_2G_CLEAR = Bit5 | Bit4, // 00: 2G // ODR[3:2] - ODR_50HZ = Bit2, // 01 + ODR_50HZ_SET = Bit2, // 01: 50Hz + ODR_50HZ_CLEAR = Bit3, // MODE[1:0] - Mode_Continuous = Bit0, // 01 + Mode_Continuous_SET = Bit0, // 01: Continuous + Mode_Continuous_CLEAR = Bit1, }; // CNTL2