From 93ab80220252e58f6a2b43155dcff402699444b8 Mon Sep 17 00:00:00 2001
From: Dmitry Ponomarev <36133264+PonomarevDA@users.noreply.github.com>
Date: Fri, 23 Jan 2026 00:43:18 +0300
Subject: [PATCH] add ability to control uavcan hardpoint by MAV_CMD_DO_GRIPPER
 (#19124)

* update uavcan hardpoint: add ability to use it via mavlink cmd and during mission and removed send_command() dedicated for usage via mavlink console

* uavcan hardpoint: allow gripper ID 0

---------

Co-authored-by: Andrew Brahim <35986980+dirksavage88@users.noreply.github.com>
---
 src/drivers/uavcan/actuators/hardpoint.cpp | 47 +++++++++++-----------
 src/drivers/uavcan/actuators/hardpoint.hpp | 17 ++++----
 2 files changed, 31 insertions(+), 33 deletions(-)

diff --git a/src/drivers/uavcan/actuators/hardpoint.cpp b/src/drivers/uavcan/actuators/hardpoint.cpp
index e1d9e52115..2e0a4a6743 100644
--- a/src/drivers/uavcan/actuators/hardpoint.cpp
+++ b/src/drivers/uavcan/actuators/hardpoint.cpp
@@ -38,14 +38,13 @@
  */
 
 #include "hardpoint.hpp"
-#include <systemlib/err.h>
 
 UavcanHardpointController::UavcanHardpointController(uavcan::INode &node) :
 	_node(node),
-	_uavcan_pub_raw_cmd(node),
+	_uavcan_pub_hardpoint(node),
 	_timer(node)
 {
-	_uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::MiddleLower);
+	_uavcan_pub_hardpoint.setPriority(uavcan::TransferPriority::MiddleLower);
 }
 
 UavcanHardpointController::~UavcanHardpointController()
@@ -59,33 +58,33 @@ UavcanHardpointController::init()
 	/*
 	 * Setup timer and call back function for periodic updates
 	 */
-	_timer.setCallback(TimerCbBinder(this, &UavcanHardpointController::periodic_update));
-	return 0;
-}
-
-void
-UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command)
-{
-	_cmd.command = command;
-	_cmd.hardpoint_id = hardpoint_id;
-
-	/*
-	 * Publish the command message to the bus
-	 */
-	_uavcan_pub_raw_cmd.broadcast(_cmd);
-
-	/*
-	 * Start the periodic update timer after a command is set
-	 */
 	if (!_timer.isRunning()) {
-		_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ));
+		_timer.setCallback(TimerCbBinder(this, &UavcanHardpointController::periodic_update));
+		_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_UPDATE_RATE_HZ));
 	}
 
+	return 0;
 }
 
 void
 UavcanHardpointController::periodic_update(const uavcan::TimerEvent &)
 {
-	// Broadcast command at MAX_RATE_HZ
-	_uavcan_pub_raw_cmd.broadcast(_cmd);
+	if (_vehicle_command_sub.updated()) {
+		vehicle_command_s vehicle_command;
+
+		if (_vehicle_command_sub.copy(&vehicle_command)) {
+			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GRIPPER) {
+				_cmd.hardpoint_id = vehicle_command.param1;
+				_cmd.command = vehicle_command.param2;
+				_next_publish_time = 0;
+			}
+		}
+	}
+
+	const hrt_abstime timestamp_now = hrt_absolute_time();
+
+	if (timestamp_now > _next_publish_time) {
+		_next_publish_time = timestamp_now + 1000000 / PUBLISH_RATE_HZ;
+		_uavcan_pub_hardpoint.broadcast(_cmd);
+	}
 }
diff --git a/src/drivers/uavcan/actuators/hardpoint.hpp b/src/drivers/uavcan/actuators/hardpoint.hpp
index 9587b60a3e..e5be4224fa 100644
--- a/src/drivers/uavcan/actuators/hardpoint.hpp
+++ b/src/drivers/uavcan/actuators/hardpoint.hpp
@@ -42,7 +42,8 @@
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/hardpoint/Command.hpp>
 #include <uavcan/equipment/hardpoint/Status.hpp>
-#include <perf/perf_counter.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_command.h>
 
 /**
  * @brief The UavcanHardpointController class
@@ -59,17 +60,12 @@ public:
 	*/
 	int init();
 
-
-	/*
-	 * set command
-	 */
-	void set_command(uint8_t hardpoint_id, uint16_t command);
-
 private:
 	/*
 	 * Max update rate to avoid exessive bus traffic
 	 */
-	static constexpr unsigned			MAX_RATE_HZ = 1;	///< XXX make this configurable
+	static constexpr unsigned			MAX_UPDATE_RATE_HZ = 10;
+	static constexpr unsigned			PUBLISH_RATE_HZ = 1;
 
 	uavcan::equipment::hardpoint::Command		_cmd;
 
@@ -83,7 +79,10 @@ private:
 	 * libuavcan related things
 	 */
 	uavcan::INode							&_node;
-	uavcan::Publisher<uavcan::equipment::hardpoint::Command>	_uavcan_pub_raw_cmd;
+	uavcan::Publisher<uavcan::equipment::hardpoint::Command>	_uavcan_pub_hardpoint;
 	uavcan::TimerEventForwarder<TimerCbBinder>			_timer;
 
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+
+	hrt_abstime _next_publish_time = 0;
 };