diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index 2f162fdb67..dace12fd9e 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -226,8 +226,18 @@ void ManualControl::Run() // If it's valid, this should really be valid but better safe than sorry. const int instance = _selector.instance(); - if (instance >= 0 && instance < MAX_MANUAL_INPUT_COUNT) { - _manual_control_input_subs[instance].registerCallback(); + // Attach scheduling to new samples of the chosen input + if (instance != _previous_manual_control_input_instance) { + if ((0 <= _previous_manual_control_input_instance) + && (_previous_manual_control_input_instance < MAX_MANUAL_INPUT_COUNT)) { + _manual_control_input_subs[_previous_manual_control_input_instance].unregisterCallback(); + } + + if ((0 <= instance) && (instance < MAX_MANUAL_INPUT_COUNT)) { + _manual_control_input_subs[instance].registerCallback(); + } + + _previous_manual_control_input_instance = instance; } _manual_control_switches_sub.registerCallback(); diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index ef9407a998..b77b49e4f5 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -131,6 +131,7 @@ private: uORB::Publication _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + int _previous_manual_control_input_instance{-1}; uORB::SubscriptionCallbackWorkItem _manual_control_input_subs[MAX_MANUAL_INPUT_COUNT] { {this, ORB_ID(manual_control_input), 0}, {this, ORB_ID(manual_control_input), 1},