add ability to control uavcan hardpoint by MAV_CMD_DO_GRIPPER

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,38 @@ UavcanHardpointController::init()
 	/*
 	 * Setup timer and call back function for periodic updates
 	 */
-	_timer.setCallback(TimerCbBinder(this, &UavcanHardpointController::periodic_update));
+	if (!_timer.isRunning()) {
+		_timer.setCallback(TimerCbBinder(this, &UavcanHardpointController::periodic_update));
+		_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_UPDATE_RATE_HZ));
+	}
+
 	return 0;
 }
 
 void
-UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command)
+UavcanHardpointController::periodic_update(const uavcan::TimerEvent &)
 {
-	_cmd.command = command;
-	_cmd.hardpoint_id = hardpoint_id;
+	if (_vehicle_command_sub.updated()) {
+		vehicle_command_s vehicle_command;
 
-	/*
-	 * Publish the command message to the bus
-	 */
-	_uavcan_pub_raw_cmd.broadcast(_cmd);
+		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;
+			}
+		}
+	}
 
 	/*
-	 * Start the periodic update timer after a command is set
+	 * According to the MAV_CMD_DO_GRIPPER cmd, Instance (hardpoint_id) should be at least 1
 	 */
-	if (!_timer.isRunning()) {
-		_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ));
-	}
-
-}
+	if (_cmd.hardpoint_id >= 1) {
+		const hrt_abstime timestamp_now = hrt_absolute_time();
 
-void
-UavcanHardpointController::periodic_update(const uavcan::TimerEvent &)
-{
-	// Broadcast command at MAX_RATE_HZ
-	_uavcan_pub_raw_cmd.broadcast(_cmd);
+		if (timestamp_now > _next_publish_time) {
+			_next_publish_time = timestamp_now + 1000000 / PUBLISH_RATE_HZ;
+			_uavcan_pub_hardpoint.broadcast(_cmd);
+		}
+	}
 }

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 @@ class UavcanHardpointController
 	*/
 	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 @@ class UavcanHardpointController
 	 * 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;
 };