From eb2a714aa58f13fcecff08451bc26e5e7b58c551 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 22 Sep 2022 20:17:00 -0400 Subject: [PATCH] ak09916: PX4_ERR if reset/configure is failing after 5s --- .../magnetometer/akm/ak09916/AK09916.cpp | 103 +++++++++++------- .../magnetometer/akm/ak09916/AK09916.hpp | 20 +--- .../akm/ak09916/AKM_AK09916_registers.hpp | 8 +- .../magnetometer/akm/ak09916/CMakeLists.txt | 2 +- .../magnetometer/akm/ak09916/ak09916_main.cpp | 2 +- 5 files changed, 69 insertions(+), 66 deletions(-) diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp index fc7c94f339..fe35d03d15 100644 --- a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -50,7 +50,6 @@ AK09916::AK09916(const I2CSPIDriverConfig &config) : AK09916::~AK09916() { perf_free(_reset_perf); - perf_free(_transfer_perf); perf_free(_bad_register_perf); perf_free(_bad_transfer_perf); perf_free(_magnetic_sensor_overflow_perf); @@ -65,8 +64,6 @@ int AK09916::init() return ret; } - _retries = 2; - return Reset() ? 0 : -1; } @@ -83,7 +80,6 @@ void AK09916::print_status() I2CSPIDriverBase::print_status(); perf_print_counter(_reset_perf); - perf_print_counter(_transfer_perf); perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); perf_print_counter(_magnetic_sensor_overflow_perf); @@ -91,32 +87,39 @@ void AK09916::print_status() int AK09916::probe() { - const uint8_t WIA1 = RegisterRead(Register::WIA1); + // 3 retries + for (int i = 0; i < 3; i++) { - if (WIA1 != Company_ID) { - DEVICE_DEBUG("unexpected WIA1 0x%02x", WIA1); - return PX4_ERROR; + const uint8_t WIA1 = RegisterRead(Register::WIA1); + const uint8_t WIA2 = RegisterRead(Register::WIA2); + + if ((WIA1 == Company_ID) && (WIA2 == Device_ID)) { + return PX4_OK; + } + + if (WIA1 != Company_ID) { + DEVICE_DEBUG("unexpected WIA1 0x%02x", WIA1); + } + + if (WIA2 != Device_ID) { + DEVICE_DEBUG("unexpected WIA2 0x%02x", WIA2); + } } - const uint8_t WIA2 = RegisterRead(Register::WIA2); - - if (WIA2 != Device_ID) { - DEVICE_DEBUG("unexpected WIA2 0x%02x", WIA2); - return PX4_ERROR; - } - - return PX4_OK; + return PX4_ERROR; } void AK09916::RunImpl() { + const hrt_abstime now = hrt_absolute_time(); + switch (_state) { case STATE::RESET: perf_count(_reset_perf); // CNTL3 SRST: Soft reset RegisterWrite(Register::CNTL3, CNTL3_BIT::SRST); - _reset_timestamp = hrt_absolute_time(); - _consecutive_failures = 0; + _reset_timestamp = now; + _failure_count = 0; _state = STATE::WAIT_FOR_RESET; ScheduleDelayed(100_ms); break; @@ -124,16 +127,14 @@ void AK09916::RunImpl() case STATE::WAIT_FOR_RESET: if ((RegisterRead(Register::WIA1) == Company_ID) && (RegisterRead(Register::WIA2) == Device_ID)) { // if reset succeeded then configure - RegisterWrite(Register::CNTL2, CNTL2_BIT::MODE3); _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); + if (hrt_elapsed_time(&_reset_timestamp) > 30_s) { + PX4_ERR("Reset failed, retrying"); + Reset(); } else { PX4_DEBUG("Reset not complete, check again in 100 ms"); @@ -151,26 +152,34 @@ void AK09916::RunImpl() } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { - PX4_DEBUG("Configure failed, resetting"); - _state = STATE::RESET; + if (hrt_elapsed_time(&_reset_timestamp) > 30_s) { + PX4_ERR("Configure failed, resetting"); + Reset(); } else { PX4_DEBUG("Configure failed, retrying"); + ScheduleDelayed(100_ms); } - - ScheduleDelayed(100_ms); } break; case STATE::READ: { - perf_begin(_transfer_perf); - TransferBuffer buffer{}; - const hrt_abstime timestamp_sample = hrt_absolute_time(); + + struct TransferBuffer { + uint8_t ST1; + uint8_t HXL; + uint8_t HXH; + uint8_t HYL; + uint8_t HYH; + uint8_t HZL; + uint8_t HZH; + uint8_t TMPS; + uint8_t ST2; + } buffer{}; + uint8_t cmd = static_cast(Register::ST1); int ret = transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(TransferBuffer)); - perf_end(_transfer_perf); bool success = false; @@ -186,31 +195,39 @@ void AK09916::RunImpl() // sensor's frame is +X forward (X), +Y right (Y), +Z down (Z) _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + perf_event_count(_magnetic_sensor_overflow_perf)); - _px4_mag.update(timestamp_sample, x, y, z); + + _px4_mag.update(now, x, y, z); success = true; - _consecutive_failures = 0; + 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 = timestamp_sample; + _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(); - return; } } - - if (_consecutive_failures > 10) { - Reset(); - } } break; @@ -219,9 +236,11 @@ void AK09916::RunImpl() bool AK09916::Configure() { + _retries = 2; + // first set and clear all configured register bits for (const auto ®_cfg : _register_cfg) { - RegisterWrite(reg_cfg.reg, reg_cfg.set_bits); + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); } // now check that all are configured diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.hpp b/src/drivers/magnetometer/akm/ak09916/AK09916.hpp index 3ab00c092b..567ce1af9b 100644 --- a/src/drivers/magnetometer/akm/ak09916/AK09916.hpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -64,19 +64,6 @@ public: void print_status() override; private: - - struct TransferBuffer { - uint8_t ST1; - uint8_t HXL; - uint8_t HXH; - uint8_t HYL; - uint8_t HYH; - uint8_t HZL; - uint8_t HZH; - uint8_t TMPS; - uint8_t ST2; - }; - struct register_config_t { Register reg; uint8_t set_bits{0}; @@ -98,14 +85,13 @@ private: PX4Magnetometer _px4_mag; perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; - perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; 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 _magnetic_sensor_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": magnetic sensor overflow")}; hrt_abstime _reset_timestamp{0}; hrt_abstime _last_config_check_timestamp{0}; - unsigned _consecutive_failures{0}; + int _failure_count{0}; enum class STATE : uint8_t { RESET, @@ -118,6 +104,6 @@ private: static constexpr uint8_t size_register_cfg{1}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits - { Register::CNTL2, CNTL2_BIT::MODE3, 0 }, + { Register::CNTL2, CNTL2_BIT::MODE3_SET, CNTL2_BIT::MODE3_CLEAR }, }; }; diff --git a/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp b/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp index 61340d9c00..74f5f24f66 100644 --- a/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp +++ b/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -93,10 +93,8 @@ enum ST2_BIT : uint8_t { // CNTL2 enum CNTL2_BIT : uint8_t { // MODE[4:0] bits - MODE1 = Bit1, // “00010”: Continuous measurement mode 1 (10Hz) - MODE2 = Bit2, // “00100”: Continuous measurement mode 2 (20Hz) - MODE3 = Bit2 | Bit1, // “00110”: Continuous measurement mode 3 (50Hz) - MODE4 = Bit3, // “01000”: Continuous measurement mode 4 (100Hz) + MODE3_SET = Bit2 | Bit1, // “00110”: Continuous measurement mode 3 (50Hz) + MODE3_CLEAR = Bit4 | Bit3 | Bit0, }; // CNTL3 diff --git a/src/drivers/magnetometer/akm/ak09916/CMakeLists.txt b/src/drivers/magnetometer/akm/ak09916/CMakeLists.txt index 13c86a7a4d..27a0fedc24 100644 --- a/src/drivers/magnetometer/akm/ak09916/CMakeLists.txt +++ b/src/drivers/magnetometer/akm/ak09916/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-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 diff --git a/src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp b/src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp index 01d5cce823..beeb8e9527 100644 --- a/src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp +++ b/src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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