diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 8759904290..a48b2dded8 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -86,7 +86,16 @@ void ManualControl::Run() updateParams(); } - const hrt_abstime now = hrt_absolute_time(); + processInput(hrt_absolute_time()); + + // reschedule timeout + ScheduleDelayed(200_ms); + + perf_end(_loop_perf); +} + +void ManualControl::processInput(hrt_abstime now) +{ _selector.updateValidityOfChosenInput(now); for (int i = 0; i < MAX_MANUAL_INPUT_COUNT; i++) { @@ -267,11 +276,6 @@ void ManualControl::Run() } _last_time = now; - - // reschedule timeout - ScheduleDelayed(200_ms); - - perf_end(_loop_perf); } void ManualControl::updateParams() diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index 8859d72f5b..fbe4ecb784 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -74,6 +74,9 @@ public: int print_status() override; +protected: + void processInput(hrt_abstime now); + private: static constexpr int MAX_MANUAL_INPUT_COUNT = 3;