dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fix afbrs50 high cpu load from switching

This commit is contained in:
alexklimaj 2021-08-05 17:35:16 -06:00 committed by Daniel Agar
parent 741f9c6d1a
commit a904bb506f
2 changed files with 19 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
View File

@ -207,11 +207,6 @@ int AFBRS50::init()
void AFBRS50::Run() void AFBRS50::Run()
{ {
// backup schedule
ScheduleDelayed(100_ms);
UpdateMode();
switch (_state) { switch (_state) {
case STATE::TEST: { case STATE::TEST: {
Argus_VerifyHALImplementation(Argus_GetSPISlave(_hnd)); Argus_VerifyHALImplementation(Argus_GetSPISlave(_hnd));
@ -253,6 +248,11 @@ void AFBRS50::Run()
default: default:
break; break;
} }
UpdateMode();
// backup schedule
ScheduleDelayed(100_ms);
} }
void AFBRS50::UpdateMode() void AFBRS50::UpdateMode()
@ -260,19 +260,19 @@ void AFBRS50::UpdateMode()
// only update mode if _current_distance is a valid measurement // only update mode if _current_distance is a valid measurement
if (_current_distance > 0) { 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 // change to long range mode
argus_mode_t mode = ARGUS_MODE_A; _mode = ARGUS_MODE_A;
set_mode(mode); set_mode(_mode);
_measure_interval = (1000000 / LONG_RANGE_MODE_HZ); _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 // change to short range mode
argus_mode_t mode = ARGUS_MODE_B; _mode = ARGUS_MODE_B;
set_mode(mode); set_mode(_mode);
_measure_interval = (1000000 / SHORT_RANGE_MODE_HZ); _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() void AFBRS50::print_info()
{ {
perf_print_counter(_sample_perf); perf_print_counter(_sample_perf);
get_mode();
} }
void AFBRS50::set_mode(argus_mode_t mode) void AFBRS50::set_mode(argus_mode_t mode)
@ -301,11 +302,10 @@ void AFBRS50::get_mode()
Argus_GetConfigurationMeasurementMode(_hnd, &current_mode); Argus_GetConfigurationMeasurementMode(_hnd, &current_mode);
Argus_GetConfigurationDFMMode(_hnd, current_mode, &dfm_mode); Argus_GetConfigurationDFMMode(_hnd, current_mode, &dfm_mode);
int dist = _current_distance; PX4_INFO_RAW("distance: %.3fm\n", (double)_current_distance);
PX4_INFO_RAW("distance: %d\n", dist);
PX4_INFO_RAW("mode: %d\n", current_mode); PX4_INFO_RAW("mode: %d\n", current_mode);
// PX4_INFO_RAW("dfm mode: %d\n", dfm_mode); PX4_INFO_RAW("dfm mode: %d\n", dfm_mode);
PX4_INFO_RAW("rate: %d Hz\n\n", (1000000 / _measure_interval)); PX4_INFO_RAW("rate: %d Hz\n", (1000000 / _measure_interval));
} }
namespace afbrs50 namespace afbrs50

4
src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp
View File

@ -81,7 +81,7 @@ private:
void set_mode(argus_mode_t mode); void set_mode(argus_mode_t mode);
argus_hnd_t *_hnd{nullptr}; 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 { enum class STATE : uint8_t {
TEST, TEST,
@ -96,7 +96,7 @@ private:
perf_counter_t _sample_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": sample interval")}; 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}; float _current_distance{0};
const float _short_range_threshold = 4.0; //meters const float _short_range_threshold = 4.0; //meters
const float _long_range_threshold = 6.0; //meters const float _long_range_threshold = 6.0; //meters