update uavcan hardpoint: add ability to use it via mavlink cmd and du…

…ring mission and removed send_command() dedicated for usage via mavlink console

msg/vehicle_command.msg

@@ -54,6 +54,7 @@ uint16 VEHICLE_CMD_DO_FENCE_ENABLE=207			# Mission command to enable the geofenc
 uint16 VEHICLE_CMD_DO_PARACHUTE=208			# Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_MOTOR_TEST=209			# motor test command |Instance (1, ...)| throttle type| throttle| timeout [s]| Motor count | Test order| Empty|
 uint16 VEHICLE_CMD_DO_INVERTED_FLIGHT=210		# Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty|
+uint16 VEHICLE_CMD_DO_GRIPPER=211 			# Mission command to operate a gripper.
 uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL=214		# Mission command to set TRIG_INTERVAL for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220		# Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_GUIDED_MASTER=221			# set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty|

src/drivers/uavcan/actuators/hardpoint.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2014-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,7 +38,6 @@
  */
 
 #include "hardpoint.hpp"
-#include <systemlib/err.h>
 
 UavcanHardpointController::UavcanHardpointController(uavcan::INode &node) :
 	_node(node),
@@ -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_raw_cmd.broadcast(_cmd);
+		}
+	}
 }

src/drivers/uavcan/actuators/hardpoint.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2014-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -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;
 
@@ -86,4 +82,7 @@ class UavcanHardpointController
 	uavcan::Publisher<uavcan::equipment::hardpoint::Command>	_uavcan_pub_raw_cmd;
 	uavcan::TimerEventForwarder<TimerCbBinder>			_timer;
 
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+
+	hrt_abstime _next_publish_time = 0;
 };

src/modules/commander/Commander.cpp

@@ -1539,6 +1539,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN:
 	case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
 	case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
+	case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:
 	case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR:
 		/* ignore commands that are handled by other parts of the system */

src/modules/mavlink/mavlink_mission.cpp

@@ -1429,6 +1429,12 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 			mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
 			break;
 
+		case MAV_CMD_DO_GRIPPER:
+			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+			mission_item->params[1] = mavlink_mission_item->param1;
+			mission_item->params[2] = mavlink_mission_item->param2;
+			break;
+
 		case MAV_CMD_CONDITION_GATE:
 			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
 			break;
@@ -1537,6 +1543,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		case MAV_CMD_DO_SET_ROI_NONE:
 		case MAV_CMD_CONDITION_DELAY:
 		case MAV_CMD_CONDITION_DISTANCE:
+		case MAV_CMD_DO_GRIPPER:
 			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
 			break;
 
@@ -1629,6 +1636,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 		case NAV_CMD_SET_CAMERA_ZOOM:
 		case NAV_CMD_SET_CAMERA_FOCUS:
 		case NAV_CMD_DO_VTOL_TRANSITION:
+		case NAV_CMD_DO_GRIPPER:
 			break;
 
 		default:

src/modules/navigator/mission_block.cpp

@@ -126,6 +126,7 @@ MissionBlock::is_mission_item_reached()
 		}
 
 	case NAV_CMD_DO_CHANGE_SPEED:
+	case NAV_CMD_DO_GRIPPER:
 	case NAV_CMD_DO_SET_HOME:
 		return true;
 

src/modules/navigator/mission_feasibility_checker.cpp

@@ -278,6 +278,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
 		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
 		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
 		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
+		    missionitem.nav_cmd != NAV_CMD_DO_GRIPPER &&
 		    missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
 
 			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d\t",
@@ -422,6 +423,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
 					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
 					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
 					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
+					  missionitem.nav_cmd != NAV_CMD_DO_GRIPPER &&
 					  missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION);
 		}
 	}

src/modules/navigator/navigation.h

@@ -83,6 +83,7 @@ enum NAV_CMD {
 	NAV_CMD_DO_DIGICAM_CONTROL = 203,
 	NAV_CMD_DO_MOUNT_CONFIGURE = 204,
 	NAV_CMD_DO_MOUNT_CONTROL = 205,
+	NAV_CMD_DO_GRIPPER = 211,
 	NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL = 214,
 	NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206,
 	NAV_CMD_OBLIQUE_SURVEY = 260,

src/modules/navigator/navigator_main.cpp

@@ -234,6 +234,9 @@ void Navigator::run()
 				// TODO: move DO_GO_AROUND handling to navigator
 				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_GRIPPER) {
+				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
 
 				bool reposition_valid = true;