fix(mavlink): gate UAVCAN param bridge on observed camera heartbeat (#27181)

* fix(mavlink): gate UAVCAN param bridge on observed camera heartbeat

Only exclude target_component 100..105 from the MAVLink -> UAVCAN
parameter bridge when a MAVLink HEARTBEAT with MAV_TYPE_CAMERA has
been observed on that component within the last 5s. When no camera
has been seen, forward to the matching UAVCAN node as before.

The bridge maps target_component 1:1 onto UAVCAN node_id, so the
unconditional 100..105 exclusion added in #25651 also made DroneCAN
peripherals assigned those node IDs unreachable via MAVLink params.
This preserves #25651's intent for real MAVLink cameras while
unblocking DroneCAN devices in that ID range.

Fixes #27180

* fix(mavlink): warn when camera shadows DroneCAN node in 100..105

Emit a one-shot mavlink_log_warning per comp ID when both a MAVLink
camera (observed via camera_status / MAV_TYPE_CAMERA heartbeat) and a
DroneCAN node (via dronecan_node_status) are present at the same ID
in 100..105. The camera takes precedence at the UAVCAN parameter
bridge (intent of #25651), so the CAN node's params become unreachable
via MAVLink. The warning surfaces the ambiguity in QGC's message tray
so the user can reassign the CAN node ID.

Fires regardless of which side joined first. Sticky per comp ID for
the boot — the user only sees it once per conflicting ID.

* Update src/modules/mavlink/mavlink_parameters.cpp

* Update src/modules/mavlink/mavlink_parameters.cpp

src/modules/mavlink/mavlink_parameters.cpp

@@ -58,9 +58,95 @@ MavlinkParametersManager::get_size()
 	return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 }
 
+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+
+void
+MavlinkParametersManager::update_observed_camera_components()
+{
+	camera_status_s cam_status {};
+
+	while (_camera_status_sub.update(&cam_status)) {
+		if (cam_status.active_comp_id >= MAV_COMP_ID_CAMERA
+		    && cam_status.active_comp_id <= MAV_COMP_ID_CAMERA6) {
+			_camera_comp_last_seen[cam_status.active_comp_id - MAV_COMP_ID_CAMERA] = cam_status.timestamp;
+		}
+	}
+
+	// Warn once per comp ID when a MAVLink camera and a DroneCAN node share an
+	// ID in 100..105. Camera takes precedence at the UAVCAN bridge, so the CAN
+	// node's params become unreachable via MAVLink until the user reassigns
+	// the CAN node ID. Fires regardless of which side joined first.
+	static constexpr uint8_t kAllWarned = (1u << CAMERA_COMP_ID_COUNT) - 1;
+
+	if (_camera_cannode_collision_warned_mask == kAllWarned) {
+		return;
+	}
+
+	for (unsigned i = 0; i < CAMERA_COMP_ID_COUNT; ++i) {
+		const uint8_t warn_bit = 1u << i;
+
+		if (_camera_cannode_collision_warned_mask & warn_bit) {
+			continue;
+		}
+
+		if (_camera_comp_last_seen[i] == 0
+		    || hrt_elapsed_time(&_camera_comp_last_seen[i]) >= CAMERA_OBSERVATION_TIMEOUT) {
+			continue;
+		}
+
+		const uint8_t comp_id = MAV_COMP_ID_CAMERA + i;
+
+		if (is_dronecan_node_online(comp_id)) {
+			_camera_cannode_collision_warned_mask |= warn_bit;
+			mavlink_log_warning(_mavlink.get_mavlink_log_pub(),
+					    "CAN node %u blocked by MAV camera (reassign ID)\t",
+					    comp_id);
+		}
+	}
+}
+
+bool
+MavlinkParametersManager::is_observed_mavlink_camera(uint8_t target_component) const
+{
+	if (target_component < MAV_COMP_ID_CAMERA || target_component > MAV_COMP_ID_CAMERA6) {
+		return false;
+	}
+
+	const hrt_abstime last_seen = _camera_comp_last_seen[target_component - MAV_COMP_ID_CAMERA];
+
+	if (last_seen == 0) {
+		return false;
+	}
+
+	return hrt_elapsed_time(&last_seen) < CAMERA_OBSERVATION_TIMEOUT;
+}
+
+bool
+MavlinkParametersManager::is_dronecan_node_online(uint8_t node_id)
+{
+	dronecan_node_status_s status {};
+
+	for (auto &sub : _dronecan_node_status_subs) {
+		if (sub.copy(&status)
+		    && status.node_id == node_id
+		    && status.mode != dronecan_node_status_s::MODE_OFFLINE
+		    && hrt_elapsed_time(&status.timestamp) < CAMERA_OBSERVATION_TIMEOUT) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
+
 void
 MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 {
+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+	update_observed_camera_components();
+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
+
 	switch (msg->msgid) {
 	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
 			/* request all parameters */
@@ -81,7 +167,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 #if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
 
 			if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 &&
-			    !(req_list.target_component >= MAV_COMP_ID_CAMERA && req_list.target_component <= MAV_COMP_ID_CAMERA6) &&
+			    !is_observed_mavlink_camera(req_list.target_component) &&
 			    (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
 				// publish list request to UAVCAN driver via uORB.
 				uavcan_parameter_request_s req{};
@@ -151,7 +237,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 #if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
 
 			if (set.target_system == mavlink_system.sysid && set.target_component < 127 &&
-			    !(set.target_component >= MAV_COMP_ID_CAMERA && set.target_component <= MAV_COMP_ID_CAMERA6) &&
+			    !is_observed_mavlink_camera(set.target_component) &&
 			    (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
 				// publish set request to UAVCAN driver via uORB.
 				uavcan_parameter_request_s req{};
@@ -244,7 +330,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 #if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
 
 			if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
-			    !(req_read.target_component >= MAV_COMP_ID_CAMERA && req_read.target_component <= MAV_COMP_ID_CAMERA6) &&
+			    !is_observed_mavlink_camera(req_read.target_component) &&
 			    (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
 				// publish set request to UAVCAN driver via uORB.
 				uavcan_parameter_request_s req{};

src/modules/mavlink/mavlink_parameters.h

@@ -48,6 +48,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/rc_parameter_map.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
@@ -55,6 +56,8 @@
 #if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
 # include <uORB/topics/uavcan_parameter_request.h>
 # include <uORB/topics/uavcan_parameter_value.h>
+# include <uORB/topics/camera_status.h>
+# include <uORB/topics/dronecan_node_status.h>
 #endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
 
 using namespace time_literals;
@@ -169,6 +172,36 @@ protected:
 		      "uavcan_parameter_request_s MAV_PARAM_TYPE_INT64 constant mismatch");
 
 	uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)};
+
+	/**
+	 * Poll camera_status uORB and record the last time each comp ID in the
+	 * MAV_COMP_ID_CAMERA..MAV_COMP_ID_CAMERA6 range was seen as a MAVLink
+	 * camera (HEARTBEAT with MAV_TYPE_CAMERA).
+	 */
+	void update_observed_camera_components();
+
+	/**
+	 * True if target_component is in MAV_COMP_ID_CAMERA..MAV_COMP_ID_CAMERA6
+	 * and we have observed a MAVLink camera heartbeat on it within
+	 * CAMERA_OBSERVATION_TIMEOUT. When false, the UAVCAN parameter bridge may
+	 * forward to CAN node target_component even if that ID falls in 100..105,
+	 * which is required for DroneCAN peripherals (e.g. ESCs) assigned those IDs.
+	 */
+	bool is_observed_mavlink_camera(uint8_t target_component) const;
+
+	/**
+	 * True if any dronecan_node_status instance reports a live (non-OFFLINE,
+	 * recently updated) node at the given node_id.
+	 */
+	bool is_dronecan_node_online(uint8_t node_id);
+
+	static constexpr hrt_abstime CAMERA_OBSERVATION_TIMEOUT = 5_s;
+	static constexpr unsigned CAMERA_COMP_ID_COUNT = MAV_COMP_ID_CAMERA6 - MAV_COMP_ID_CAMERA + 1;
+
+	uORB::Subscription _camera_status_sub{ORB_ID(camera_status)};
+	uORB::SubscriptionMultiArray<dronecan_node_status_s, ORB_MULTI_MAX_INSTANCES> _dronecan_node_status_subs{ORB_ID::dronecan_node_status};
+	hrt_abstime _camera_comp_last_seen[CAMERA_COMP_ID_COUNT] {};
+	uint8_t _camera_cannode_collision_warned_mask{0}; ///< bit i: already warned for MAV_COMP_ID_CAMERA + i
 #endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
 
 	uORB::Publication<rc_parameter_map_s>	_rc_param_map_pub{ORB_ID(rc_parameter_map)};