fix(uavcan): block firmware update while armed and arming while updating (#28089)

* feat: track Node updates

* feat: prevent arming while updating

* fix: used make format

* fix: added dedup logic & use dynamic list

* fix: cleaned up arming_blocker

* feat: add circuit breaker for arming_blocker

* chore: used make format

* fix: fix some formating issues

* chore: make format again

* fix: implemented requested changes

msg/CMakeLists.txt

@@ -229,6 +229,7 @@ set(msg_files
 	TrajectorySetpoint6dof.msg
 	TransponderReport.msg
 	TuneControl.msg
+	UavcanFirmwareUpdate.msg
 	UavcanParameterRequest.msg
 	UavcanParameterValue.msg
 	UlogStream.msg

msg/UavcanFirmwareUpdate.msg

@@ -0,0 +1,2 @@
+uint64 timestamp        # time since system start (microseconds)
+bool pending_updates    # true when one or more nodes requiring a firmware update have been detected and the update is not yet complete

src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp

@@ -17,6 +17,7 @@
 #include <dirent.h>
 
 #include <uavcan/protocol/firmware_update_trigger.hpp>
+#include <containers/List.hpp>
 
 // TODO Get rid of the macro
 #if !defined(DIRENT_ISFILE) && defined(DT_REG)
@@ -52,6 +53,17 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker
 	 */
 	typedef uavcan::MakeString<MaxPathLength>::Type PathString;
 
+	struct UpdatingNode : public ListNode<UpdatingNode *> {
+		uint8_t unique_id[16];
+	};
+
+	// Dynamic allocation is acceptable here: firmware updates only occur before arming
+	// (shouldRequestFirmwareUpdate returns false when armed), so nodes are added and
+	// removed only during the pre-arm phase. The list is cleared in the destructor.
+	List<UpdatingNode *> _updating_nodes;
+
+	bool _armed = false;
+
 	BasePathString base_path_;
 	BasePathString alt_base_path_;
 	BasePathString nfs_base_path_;
@@ -101,6 +113,17 @@ protected:
 	{
 		using namespace std;
 
+		if (_armed) {
+			return false;
+		}
+
+		// per spec: all-zeros unique_id is undefined; node cannot be reliably identified
+		const uint8_t zero_uid[16] {};
+
+		if (memcmp(node_info.hardware_version.unique_id.begin(), zero_uid, 16) == 0) {
+			return false;
+		}
+
 		/* This is a work  around for two issues.
 		 *  1) FirmwareFilePath is 40
 		 *  2) OK using is using 32 for max file names.
@@ -153,6 +176,27 @@ protected:
 			}
 		}
 
+		const auto *uid = node_info.hardware_version.unique_id.begin();
+		UpdatingNode *existing = _updating_nodes.find([uid](UpdatingNode * node) {
+			return memcmp(node->unique_id, uid, 16) == 0;
+		});
+
+		if (rv) {
+			if (!existing) {
+				UpdatingNode *new_node = new UpdatingNode();
+
+				if (new_node) {
+					memcpy(new_node->unique_id, uid, 16);
+					_updating_nodes.add(new_node);
+				}
+			}
+
+		} else {
+			if (existing) {
+				_updating_nodes.deleteNode(existing);
+			}
+		}
+
 		return rv;
 	}
 
@@ -260,6 +304,18 @@ out_close:
 
 	const BasePathString &getFirmwareAltBasePath() const { return alt_base_path_; }
 
+	void setArmed(bool armed) { _armed = armed; }
+
+	~FirmwareVersionChecker()
+	{
+		_updating_nodes.clear();
+	}
+
+	bool hasUpdatingNodes() const
+	{
+		return !_updating_nodes.empty();
+	}
+
 	const BasePathString &getFirmwareNfsBasePath() const { return nfs_base_path_; }
 
 	void setFirmwareNfsBasePath(const char *path)

src/drivers/uavcan/uavcan_main.cpp

@@ -752,6 +752,13 @@ UavcanNode::Run()
 		_node_info_retriever.invalidateAll();
 	}
 
+	// propagate armed state to firmware version checker
+	if (_actuator_armed_sub.updated() && _servers != nullptr) {
+		actuator_armed_s actuator_armed{};
+		_actuator_armed_sub.copy(&actuator_armed);
+		_servers->setArmed(actuator_armed.armed || actuator_armed.prearmed);
+	}
+
 #ifdef CONFIG_MODULES_NFS_MOUNT
 
 	if (_servers != nullptr) {
@@ -766,6 +773,18 @@ UavcanNode::Run()
 
 	publish_node_statuses();
 
+	if (_servers != nullptr) {
+		const bool pending = _servers->hasPendingFirmwareUpdates();
+
+		if (pending != _fw_update_pending_last) {
+			_fw_update_pending_last = pending;
+			uavcan_firmware_update_s fw_update{};
+			fw_update.timestamp = hrt_absolute_time();
+			fw_update.pending_updates = pending;
+			_fw_update_pub.publish(fw_update);
+		}
+	}
+
 	// check for parameter updates
 	if (_parameter_update_sub.updated()) {
 		// clear update

src/drivers/uavcan/uavcan_main.hpp

@@ -102,7 +102,9 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/can_interface_status.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/dronecan_node_status.h>
+#include <uORB/topics/uavcan_firmware_update.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/uavcan_parameter_request.h>
 #include <uORB/topics/uavcan_parameter_value.h>
@@ -320,9 +322,12 @@ private:
 	uORB::SubscriptionInterval	_parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _vcmd_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _param_request_sub{ORB_ID(uavcan_parameter_request)};
+	uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
 
 	uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
 	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<uavcan_firmware_update_s> _fw_update_pub{ORB_ID(uavcan_firmware_update)};
+	bool _fw_update_pending_last{false};
 
 	orb_advert_t _can_status_pub_handles[UAVCAN_NUM_IFACES] = {nullptr};
 

src/drivers/uavcan/uavcan_servers.hpp

@@ -77,6 +77,13 @@ public:
 
 	bool guessIfAllDynamicNodesAreAllocated() { return _server_instance.guessIfAllDynamicNodesAreAllocated(); }
 
+	void setArmed(bool armed) { _fw_version_checker.setArmed(armed); }
+
+	bool hasPendingFirmwareUpdates() const
+	{
+		return _fw_version_checker.hasUpdatingNodes();
+	}
+
 private:
 
 	void unpackFwFromROMFS(const char *sd_path, const char *romfs_path);

src/include/containers/List.hpp

@@ -144,6 +144,16 @@ public:
 
 	bool empty() const { return getHead() == nullptr; }
 
+	template<typename Predicate>
+	T find(Predicate pred) const
+	{
+		for (auto n = getHead(); n != nullptr; n = n->getSibling()) {
+			if (pred(n)) { return n; }
+		}
+
+		return nullptr;
+	}
+
 	size_t size() const
 	{
 		size_t sz = 0;

src/lib/circuit_breaker/circuit_breaker.h

@@ -56,6 +56,7 @@
 #define CBRK_FLIGHTTERM_KEY	121212
 #define CBRK_USB_CHK_KEY	197848
 #define CBRK_VTOLARMING_KEY	159753
+#define CBRK_UAVCAN_FW_KEY	5318008
 
 #include <stdint.h>
 

src/lib/circuit_breaker/circuit_breaker_params.yaml

@@ -81,3 +81,15 @@ parameters:
       default: 0
       min: 0
       max: 159753
+    CBRK_UAVCAN_FW:
+      description:
+        short: Circuit breaker for UAVCAN firmware update arming check
+        long: |-
+          Setting this parameter to 5318008 will allow arming even when a UAVCAN
+          node firmware update is pending or a node went offline mid-update.
+          WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
+      category: Developer
+      type: int32
+      default: 0
+      min: 0
+      max: 5318008

src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp

@@ -160,4 +160,24 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
 						    events::Log::Error, "Arm authorization denied");
 		}
 	}
+
+	// Block arming while a UAVCAN node firmware update is pending
+	uavcan_firmware_update_s uavcan_fw_update{};
+
+	if (!circuit_breaker_enabled_by_val(_param_cbrk_uavcan_fw.get(), CBRK_UAVCAN_FW_KEY)
+	    && _uavcan_fw_update_sub.copy(&uavcan_fw_update) && uavcan_fw_update.pending_updates) {
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * This check can be configured via <param>CBRK_UAVCAN_FW</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(NavModes::All, health_component_t::system,
+					    events::ID("check_system_uavcan_fw_update_pending"),
+					    events::Log::Warning, "UAVCAN firmware update in progress");
+
+		if (reporter.mavlink_log_pub()) {
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: UAVCAN firmware update in progress");
+		}
+	}
 }

src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp

@@ -37,6 +37,7 @@
 
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/uavcan_firmware_update.h>
 
 class SystemChecks : public HealthAndArmingCheckBase
 {
@@ -48,10 +49,11 @@ public:
 
 private:
 	uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
+	uORB::Subscription _uavcan_fw_update_sub{ORB_ID(uavcan_firmware_update)};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
-					(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
+					(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,	(ParamInt<px4::params::CBRK_UAVCAN_FW>) _param_cbrk_uavcan_fw,
 					(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
 					(ParamInt<px4::params::COM_ARM_AUTH_REQ>) _param_com_arm_auth_req
 				       )