From a904bb506ff7366f915965d47dd29147d381ffc9 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Thu, 5 Aug 2021 17:35:16 -0600 Subject: [PATCH] Fix afbrs50 high cpu load from switching --- .../broadcom/afbrs50/AFBRS50.cpp | 34 +++++++++---------- .../broadcom/afbrs50/AFBRS50.hpp | 4 +-- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp index 2aaa58d7e0..dfdac0a865 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp @@ -207,11 +207,6 @@ int AFBRS50::init() void AFBRS50::Run() { - // backup schedule - ScheduleDelayed(100_ms); - - UpdateMode(); - switch (_state) { case STATE::TEST: { Argus_VerifyHALImplementation(Argus_GetSPISlave(_hnd)); @@ -253,6 +248,11 @@ void AFBRS50::Run() default: break; } + + UpdateMode(); + + // backup schedule + ScheduleDelayed(100_ms); } void AFBRS50::UpdateMode() @@ -260,19 +260,19 @@ void AFBRS50::UpdateMode() // only update mode if _current_distance is a valid measurement if (_current_distance > 0) { - if (_current_distance >= _long_range_threshold) { + if ((_current_distance >= _long_range_threshold) && (_mode != ARGUS_MODE_A)) { // change to long range mode - argus_mode_t mode = ARGUS_MODE_A; - set_mode(mode); + _mode = ARGUS_MODE_A; + set_mode(_mode); _measure_interval = (1000000 / LONG_RANGE_MODE_HZ); - ScheduleDelayed(100_ms); + ScheduleDelayed(1000_ms); // don't switch again for at least 1 second - } else if (_current_distance <= _short_range_threshold) { + } else if ((_current_distance <= _short_range_threshold) && (_mode != ARGUS_MODE_B)) { // change to short range mode - argus_mode_t mode = ARGUS_MODE_B; - set_mode(mode); + _mode = ARGUS_MODE_B; + set_mode(_mode); _measure_interval = (1000000 / SHORT_RANGE_MODE_HZ); - ScheduleDelayed(100_ms); + ScheduleDelayed(1000_ms); // don't switch again for at least 1 second } } } @@ -286,6 +286,7 @@ void AFBRS50::stop() void AFBRS50::print_info() { perf_print_counter(_sample_perf); + get_mode(); } void AFBRS50::set_mode(argus_mode_t mode) @@ -301,11 +302,10 @@ void AFBRS50::get_mode() Argus_GetConfigurationMeasurementMode(_hnd, ¤t_mode); Argus_GetConfigurationDFMMode(_hnd, current_mode, &dfm_mode); - int dist = _current_distance; - PX4_INFO_RAW("distance: %d\n", dist); + PX4_INFO_RAW("distance: %.3fm\n", (double)_current_distance); PX4_INFO_RAW("mode: %d\n", current_mode); - // PX4_INFO_RAW("dfm mode: %d\n", dfm_mode); - PX4_INFO_RAW("rate: %d Hz\n\n", (1000000 / _measure_interval)); + PX4_INFO_RAW("dfm mode: %d\n", dfm_mode); + PX4_INFO_RAW("rate: %d Hz\n", (1000000 / _measure_interval)); } namespace afbrs50 diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp index abcd0d0669..65af6c04d1 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp @@ -81,7 +81,7 @@ private: void set_mode(argus_mode_t mode); argus_hnd_t *_hnd{nullptr}; - argus_mode_t _mode{ARGUS_MODE_A}; // Long-Range + argus_mode_t _mode{ARGUS_MODE_B}; // Short-Range enum class STATE : uint8_t { TEST, @@ -96,7 +96,7 @@ private: perf_counter_t _sample_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": sample interval")}; - int _measure_interval{1000000 / 100}; // 100Hz + int _measure_interval{1000000 / 50}; // 50Hz float _current_distance{0}; const float _short_range_threshold = 4.0; //meters const float _long_range_threshold = 6.0; //meters