From 0586079a844ae658bcb64e5594f4088b63889a4c Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 20 Jul 2020 11:04:34 -0400
Subject: [PATCH] mixer_module: add subscription option to limit additional
 actuator_control callbacks

 - optionally only register callbacks for primary control groups (if used)
---
 src/lib/mixer_module/mixer_module.cpp | 29 +++++++++++++++++++++++----
 src/lib/mixer_module/mixer_module.hpp |  3 ++-
 2 files changed, 27 insertions(+), 5 deletions(-)

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