fix(gimbal): require assigned outputs for AUX gimbal manager (#27877)

In AUX output mode, the gimbal module unconditionally acted as a
virtual gimbal device and therefore advertised a gimbal manager, even
if no output channel was assigned a gimbal output function, i.e. no
gimbal could possibly be connected. This conflicts with setups where a
gimbal manager external to PX4 is used.

Only act as a gimbal device (and hence advertise a gimbal manager) if
at least one output function param is set to gimbal roll/pitch/yaw,
mirroring the MAVLink v2 output mode which only advertises once a
gimbal device is discovered. The check is done once at module startup
to avoid scanning all params on every parameter update, so assigning
a gimbal output function requires a reboot to take effect.

Signed-off-by: Julian Oes <julian@oes.ch>

src/modules/gimbal/CMakeLists.txt

@@ -48,4 +48,5 @@ px4_add_module(
 		gimbal_params.yaml
 	DEPENDS
 		geo
+		output_functions_header
 	)

src/modules/gimbal/gimbal_params.yaml

@@ -30,6 +30,11 @@ parameters:
           This is the protocol used between the autopilot and a connected gimbal.
 
           Recommended is the MAVLink gimbal protocol v2 if the gimbal supports it.
+
+          In AUX mode, the gimbal (and hence the gimbal manager) is only made
+          available if at least one output channel is assigned a gimbal
+          roll/pitch/yaw output function (checked once at startup, so newly
+          assigned output functions require a reboot).
       type: enum
       values:
         0: AUX

src/modules/gimbal/output_rc.cpp

@@ -34,9 +34,11 @@
 
 #include "output_rc.h"
 
+#include <string.h>
 #include <uORB/topics/gimbal_controls.h>
 #include <px4_platform_common/defines.h>
 #include <matrix/matrix/math.hpp>
+#include <mixer_module/output_functions.hpp>
 
 namespace gimbal
 {
@@ -44,6 +46,7 @@ namespace gimbal
 OutputRC::OutputRC(const Parameters &parameters)
 	: OutputBase(parameters)
 {
+	_check_gimbal_output_functions_assigned();
 }
 
 void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8_t &gimbal_device_id)
@@ -58,10 +61,14 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8
 
 	_calculate_angle_output(now);
 
-	_stream_device_attitude_status();
+	// Without any gimbal output function assigned to a channel there is no gimbal,
+	// so we should not act as a gimbal device (and hence not advertise a gimbal manager).
+	if (_gimbal_output_functions_assigned) {
+		_stream_device_attitude_status();
+	}
 
 	// If the output is RC, then we signal this by referring to compid 1.
-	gimbal_device_id = 1;
+	gimbal_device_id = _gimbal_output_functions_assigned ? 1 : 0;
 
 	// _angle_outputs are in radians, gimbal_controls are in [-1, 1]
 	gimbal_controls_s gimbal_controls{};
@@ -74,6 +81,46 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8
 	_last_update = now;
 }
 
+void OutputRC::_check_gimbal_output_functions_assigned()
+{
+	_gimbal_output_functions_assigned = false;
+
+	for (unsigned i = 0; i < param_count(); ++i) {
+		const param_t param = param_for_index(i);
+
+		// Match the output function params of all output drivers, e.g. PWM_MAIN_FUNC1.
+		const char *suffix = strstr(param_name(param), "_FUNC");
+
+		if (suffix == nullptr || suffix[5] == '\0') {
+			continue;
+		}
+
+		bool digits_only = true;
+
+		for (const char *c = suffix + 5; *c != '\0'; ++c) {
+			if (*c < '0' || *c > '9') {
+				digits_only = false;
+				break;
+			}
+		}
+
+		if (!digits_only) {
+			continue;
+		}
+
+		int32_t function;
+
+		if (param_type(param) != PARAM_TYPE_INT32 || param_get(param, &function) != PX4_OK) {
+			continue;
+		}
+
+		if (function >= (int32_t)OutputFunction::Gimbal_Roll && function <= (int32_t)OutputFunction::Gimbal_Yaw) {
+			_gimbal_output_functions_assigned = true;
+			return;
+		}
+	}
+}
+
 float OutputRC::anglesMappedToOutput(const uint8_t index)
 {
 
@@ -120,6 +167,8 @@ float OutputRC::anglesMappedToOutput(const uint8_t index)
 void OutputRC::print_status() const
 {
 	PX4_INFO("Output: AUX");
+	PX4_INFO_RAW("  gimbal output function assigned: %s\n",
+		     _gimbal_output_functions_assigned ? "yes (acting as gimbal device)" : "no (not acting as gimbal device)");
 }
 
 void OutputRC::_stream_device_attitude_status()

src/modules/gimbal/output_rc.h

@@ -56,9 +56,12 @@ public:
 private:
 	void _stream_device_attitude_status();
 	float anglesMappedToOutput(const uint8_t index);
+	void _check_gimbal_output_functions_assigned();
 
 	uORB::Publication <gimbal_controls_s>	_gimbal_controls_pub{ORB_ID(gimbal_controls)};
 	uORB::Publication <gimbal_device_attitude_status_s>	_attitude_status_pub{ORB_ID(gimbal_device_attitude_status)};
+
+	bool _gimbal_output_functions_assigned{false};
 };
 
 } /* namespace gimbal */