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 6617b715a6de..b003b0190813 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 83a6f4a03409..25259d54d21b 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 @@ class VtolAttitudeControl : public ModuleBase, public px4:: 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. */