diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index da74f697dd..ca2eea4985 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -115,7 +115,7 @@ void MixingOutput::updateParams() } } -bool MixingOutput::updateSubscriptions(bool allow_wq_switch) +bool MixingOutput::updateSubscriptions(bool allow_wq_switch, bool limit_callbacks_to_primary) { if (_groups_subscribed == _groups_required) { return false; @@ -143,12 +143,33 @@ bool MixingOutput::updateSubscriptions(bool allow_wq_switch) } } + bool sub_group_0_callback_registered = false; + bool sub_group_1_callback_registered = false; + // register callback to all required actuator control groups for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_groups_required & (1 << i)) { - PX4_DEBUG("subscribe to actuator_controls_%d", i); - if (!_control_subs[i].registerCallback()) { + if (limit_callbacks_to_primary) { + // don't register additional callbacks if actuator_controls_0 or actuator_controls_1 are already registered + if ((i > 1) && (sub_group_0_callback_registered || sub_group_1_callback_registered)) { + break; + } + } + + if (_groups_required & (1 << i)) { + if (_control_subs[i].registerCallback()) { + PX4_DEBUG("subscribed to actuator_controls_%d", i); + + if (limit_callbacks_to_primary) { + if (i == 0) { + sub_group_0_callback_registered = true; + + } else if (i == 1) { + sub_group_1_callback_registered = true; + } + } + + } else { PX4_ERR("actuator_controls_%d register callback failed!", i); } } diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 8fbeaca48d..79c39a7c2a 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -124,9 +124,10 @@ public: * Check for subscription updates (e.g. after a mixer is loaded). * Call this at the very end of Run() if allow_wq_switch * @param allow_wq_switch if true + * @param limit_callbacks_to_primary set to only register callbacks for primary actuator controls (if used) * @return true if subscriptions got changed */ - bool updateSubscriptions(bool allow_wq_switch); + bool updateSubscriptions(bool allow_wq_switch, bool limit_callbacks_to_primary = false); /** * unregister uORB subscription callbacks