From 985aa09b7133ef3433485ab8a62eb57f67e9fd1e Mon Sep 17 00:00:00 2001 From: Phil Date: Tue, 11 Aug 2026 01:57:58 +0200 Subject: [PATCH] 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 | 1 + msg/UavcanFirmwareUpdate.msg | 2 + .../uavcan_posix/firmware_version_checker.hpp | 56 +++++++++++++++++++ src/drivers/uavcan/uavcan_main.cpp | 19 +++++++ src/drivers/uavcan/uavcan_main.hpp | 5 ++ src/drivers/uavcan/uavcan_servers.hpp | 7 +++ src/include/containers/List.hpp | 10 ++++ src/lib/circuit_breaker/circuit_breaker.h | 1 + .../circuit_breaker_params.yaml | 12 ++++ .../checks/systemCheck.cpp | 20 +++++++ .../checks/systemCheck.hpp | 4 +- 11 files changed, 136 insertions(+), 1 deletion(-) create mode 100644 msg/UavcanFirmwareUpdate.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 9cf0a982fea..375d53bc8c8 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -229,6 +229,7 @@ set(msg_files TrajectorySetpoint6dof.msg TransponderReport.msg TuneControl.msg + UavcanFirmwareUpdate.msg UavcanParameterRequest.msg UavcanParameterValue.msg UlogStream.msg diff --git a/msg/UavcanFirmwareUpdate.msg b/msg/UavcanFirmwareUpdate.msg new file mode 100644 index 00000000000..57d15ef433c --- /dev/null +++ b/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 diff --git a/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index 1cf25cdab0d..b948b57cba2 100644 --- a/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -17,6 +17,7 @@ #include #include +#include // 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::Type PathString; + struct UpdatingNode : public ListNode { + 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 _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) diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 66e2575d7bd..31d3f6ba6f3 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/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 diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 4f84ff783e0..76286f76c51 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -102,7 +102,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -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 _param_response_pub{ORB_ID(uavcan_parameter_value)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::Publication _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}; diff --git a/src/drivers/uavcan/uavcan_servers.hpp b/src/drivers/uavcan/uavcan_servers.hpp index 6613dc42605..33ca24f3e36 100644 --- a/src/drivers/uavcan/uavcan_servers.hpp +++ b/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); diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 97cd483f4d0..aa3e53642e1 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -144,6 +144,16 @@ public: bool empty() const { return getHead() == nullptr; } + template + 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; diff --git a/src/lib/circuit_breaker/circuit_breaker.h b/src/lib/circuit_breaker/circuit_breaker.h index dbc54edede6..62cf8f20084 100644 --- a/src/lib/circuit_breaker/circuit_breaker.h +++ b/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 diff --git a/src/lib/circuit_breaker/circuit_breaker_params.yaml b/src/lib/circuit_breaker/circuit_breaker_params.yaml index 6ace492dd3a..2b1cfe886b6 100644 --- a/src/lib/circuit_breaker/circuit_breaker_params.yaml +++ b/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 diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp index 81878d98449..afa82c1d169 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp +++ b/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 + * + * This check can be configured via CBRK_UAVCAN_FW parameter. + * + */ + 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"); + } + } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp index 954d7b79f71..7cc968c07a0 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp @@ -37,6 +37,7 @@ #include #include +#include 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) _param_cbrk_vtolarming, - (ParamInt) _param_cbrk_usb_chk, + (ParamInt) _param_cbrk_usb_chk, (ParamInt) _param_cbrk_uavcan_fw, (ParamBool) _param_com_arm_wo_gps, (ParamInt) _param_com_arm_auth_req )