From e4f65f81f72ab6c2bb599bece9e55317826d981c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 21 Jul 2020 11:40:04 -0400 Subject: [PATCH] vtol_att_control: limit excessive scheduling - this is to prevent vtol_att_control running at absurdly high rates (kilohertz) from every single actuator controls publication from both the multicopter and fixed wing controllers - I don't really like this solution, but it works for now until there are larger VTOL architectural changes in place --- .../vtol_att_control/vtol_att_control_main.cpp | 14 ++++++++++++++ .../vtol_att_control/vtol_att_control_main.h | 2 ++ 2 files changed, 16 insertions(+) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 6617b715a6..b003b01908 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -51,6 +51,7 @@ #include using namespace matrix; +using namespace time_literals; VtolAttitudeControl::VtolAttitudeControl() : WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), @@ -342,6 +343,19 @@ VtolAttitudeControl::Run() return; } + const hrt_abstime now = hrt_absolute_time(); + +#if !defined(ENABLE_LOCKSTEP_SCHEDULER) + + // prevent excessive scheduling (> 500 Hz) + if (now - _last_run_timestamp < 2_ms) { + return; + } + +#endif // !ENABLE_LOCKSTEP_SCHEDULER + + _last_run_timestamp = now; + if (!_initialized) { parameters_update(); // initialize parameter cache diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 83a6f4a034..25259d54d2 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -213,6 +213,8 @@ private: param_t mpc_land_alt2; } _params_handles{}; + hrt_abstime _last_run_timestamp{0}; + /* for multicopters it is usual to have a non-zero idle speed of the engines * for fixed wings we want to have an idle speed of zero since we do not want * to waste energy when gliding. */