Only command Gripper grab when we are actually initializing gripper

- Previously, on every parameter update, gripper grab was being
commanded
- This commit narrows that scope to only when we are actually
initializing the gripper

src/modules/payload_deliverer/gripper.cpp

@@ -68,6 +68,10 @@ void Gripper::init(const GripperConfig &config)
 
 void Gripper::grab()
 {
+	if (_state == GripperState::GRABBING || _state == GripperState::GRABBED) {
+		return;
+	}
+
 	publish_gripper_command(gripper_s::COMMAND_GRAB);
 	_state = GripperState::GRABBING;
 	_last_command_time = hrt_absolute_time();
@@ -75,6 +79,10 @@ void Gripper::grab()
 
 void Gripper::release()
 {
+	if (_state == GripperState::RELEASING || _state == GripperState::RELEASED) {
+		return;
+	}
+
 	publish_gripper_command(gripper_s::COMMAND_RELEASE);
 	_state = GripperState::RELEASING;
 	_last_command_time = hrt_absolute_time();
@@ -89,11 +97,13 @@ void Gripper::update()
 	case GripperState::GRABBING:
 		if (_has_feedback_sensor) {
 			// Handle feedback sensor input, return true for now (not supported)
+			_grabbed_state_cache = true;
 			_state = GripperState::GRABBED;
 			break;
 		}
 
 		if (command_timed_out) {
+			_grabbed_state_cache = true;
 			_state = GripperState::GRABBED;
 		}
 

src/modules/payload_deliverer/gripper.h

@@ -99,9 +99,17 @@ public:
 	// Returns true if in released position either sensed or timeout based
 	bool released() { return _state == GripperState::RELEASED; }
 
-	// Returns true only once after the state transitioned into released
-	// Used for sending vehicle command ack only when the gripper actually releases
-	// in payload deliverer
+	/**
+	 * @brief Returns true once after gripper succeededs in releasing the payload
+	 *
+	 * Note, that this is a single-read for a one successful release operation. Meaning that
+	 * if you command `release()` and then call this function multiple times in the future,
+	 * it will only return `true` only once, and then it will return `false` after it returns
+	 * `true` once.
+	 *
+	 * This is used to detect a single event of successful releasing of a gripper in `payload_deliverer`
+	 * module, which then would send `vehicle_command_ack` message, indicating the successful release.
+	 */
 	bool released_read_once()
 	{
 		if (_released_state_cache) {
@@ -113,6 +121,20 @@ public:
 		}
 	}
 
+	/**
+	 * @brief Returns true once after gripper succeededs in grabbing the payload
+	 */
+	bool grabbed_read_once()
+	{
+		if (_grabbed_state_cache) {
+			_grabbed_state_cache = false;
+			return true;
+
+		} else {
+			return false;
+		}
+	}
+
 	// Returns true if Gripper output function is assigned properly
 	bool is_valid() const { return _valid; }
 
@@ -128,7 +150,10 @@ private:
 	// Internal state
 	GripperState _state{GripperState::IDLE};
 	hrt_abstime _last_command_time{0};
-	bool _released_state_cache{false}; // Cached flag that is set once gripper release was successful, gets reset once read
+
+	// Cached flag that is set once gripper release/grab was successful, it must get reset when read.
+	bool _released_state_cache{false};
+	bool _grabbed_state_cache{false};
 
 	uORB::Publication<gripper_s> _gripper_pub{ORB_ID(gripper)};
 };

src/modules/payload_deliverer/payload_deliverer.cpp

@@ -61,28 +61,30 @@ bool PayloadDeliverer::initialize_gripper()
 		config.sensor = GripperConfig::GripperSensorType::NONE; // Feedback sensor isn't supported for now
 		config.timeout_us = hrt_abstime(_param_gripper_timeout_s.get() * 1000000ULL);
 		_gripper.init(config);
+
+		if (!_gripper.is_valid()) {
+			PX4_DEBUG("Gripper object initialization failed!");
+			return false;
+
+		} else {
+			// Command the gripper to grab position by default
+			if (!_gripper.grabbed() && !_gripper.grabbing()) {
+				PX4_DEBUG("Gripper intialize: putting to grab position!");
+				send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_GRAB);
+			}
+
+			return true;
+		}
 	}
 
 	// NOTE: Support for changing gripper sensor type / gripper type configuration when
 	// the parameter change is detected isn't added as we don't have actual use case for that
 	// yet!
 
-	if (!_gripper.is_valid()) {
-		PX4_DEBUG("Gripper object initialization invalid!");
-		return false;
-
-	} else {
-		_gripper.update();
-
-		// If gripper wasn't commanded to go to grab position, command.
-		if (!_gripper.grabbed() && !_gripper.grabbing()) {
-			PX4_DEBUG("Gripper intialize: putting to grab position!");
-			send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_GRAB);
-		}
-
-		return true;
-	}
+	// NOTE: De-initializing the gripper when `PD_GRIPPER_EN` param gets set to 0
+	// is also not implemented yet (assuming users won't be disabling the gripper)!
 
+	return false;
 }
 
 void PayloadDeliverer::parameter_update()
@@ -131,14 +133,27 @@ void PayloadDeliverer::gripper_update(const hrt_abstime &now)
 
 	_gripper.update();
 
-	// Publish a successful gripper release acknowledgement
+	// Publish a successful gripper release / grab acknowledgement
 	if (_gripper.released_read_once()) {
 		vehicle_command_ack_s vcmd_ack{};
 		vcmd_ack.timestamp = now;
 		vcmd_ack.command = vehicle_command_s::VEHICLE_CMD_DO_GRIPPER;
 		vcmd_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+		// Ideally, we need to fill out target_system and target_component to match the vehicle_command we are acknowledging for
+		// But since we are not tracking the vehicle command's source system & component, we don't fill it out for now.
 		_vehicle_command_ack_pub.publish(vcmd_ack);
-		PX4_DEBUG("Payload Drop Successful Ack Sent!");
+		PX4_DEBUG("Payload Release Successful Ack Sent!");
+
+	} else if (_gripper.grabbed_read_once()) {
+		vehicle_command_ack_s vcmd_ack{};
+		vcmd_ack.timestamp = now;
+		vcmd_ack.command = vehicle_command_s::VEHICLE_CMD_DO_GRIPPER;
+		vcmd_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+		// Ideally, we need to fill out target_system and target_component to match the vehicle_command we are acknowledging for
+		// But since we are not tracking the vehicle command's source system & component, we don't fill it out for now.
+		_vehicle_command_ack_pub.publish(vcmd_ack);
+		PX4_DEBUG("Payload Grab Successful Ack Sent!");
+
 	}
 }
 
@@ -151,13 +166,23 @@ void PayloadDeliverer::handle_vehicle_command(const hrt_abstime &now,  const veh
 
 	// Process DO_GRIPPER vehicle command
 	if (vehicle_command->command == vehicle_command_s::VEHICLE_CMD_DO_GRIPPER) {
-		// If we received Gripper command and gripper isn't valid, warn the user
 		if (!_gripper.is_valid()) {
+			// If gripper instance is invalid, send ACK command with FAILED result, and warn the user
+			vehicle_command_ack_s vcmd_ack{};
+			vcmd_ack.timestamp = now;
+			vcmd_ack.command = vehicle_command_s::VEHICLE_CMD_DO_GRIPPER;
+			vcmd_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
+			vcmd_ack.target_system = vehicle_command->source_system;
+			vcmd_ack.target_component = vehicle_command->source_component;
+			_vehicle_command_ack_pub.publish(vcmd_ack);
 			PX4_WARN("Gripper instance not valid but DO_GRIPPER vehicle command was received. Gripper won't work!");
 			return;
 		}
 
-		const int32_t gripper_action = *(int32_t *)&vehicle_command->param2; // Convert the action to integer
+		const int32_t gripper_action = (int32_t)vehicle_command->param2;
+		// Note: although the 'param2' is a floating point type, the enums for GRIPPER_ACTION are converted into
+		// floating point, then gets sent by the Ground Control Station to trigger gripper action for example.
+		// This is an inherent MAVLink standard limitation (converting enums into floats)!
 
 		switch (gripper_action) {
 		case vehicle_command_s::GRIPPER_ACTION_GRAB:
@@ -168,6 +193,18 @@ void PayloadDeliverer::handle_vehicle_command(const hrt_abstime &now,  const veh
 			_gripper.release();
 			break;
 		}
+
+		// Send IN_PROGRESS vehicle_command_ack message to alert that DO_GRIPPER command is getting processed
+		// Since QGC/AMC doesn't impose timeout for IN_PROGRESS vehicle command acks, it is suffice to send it only once.
+		vehicle_command_ack_s vcmd_ack{};
+		vcmd_ack.timestamp = now;
+		vcmd_ack.command = vehicle_command_s::VEHICLE_CMD_DO_GRIPPER;
+		vcmd_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS;
+		vcmd_ack.result_param1 = UINT8_MAX; // Set progress percentage to UINT8_MAX, indicating it is unkonwn
+		vcmd_ack.target_system = vehicle_command->source_system;
+		vcmd_ack.target_component = vehicle_command->source_component;
+		_vehicle_command_ack_pub.publish(vcmd_ack);
+		PX4_DEBUG("Payload Drop In-Progress Ack Sent!");
 	}
 }
 
@@ -176,7 +213,8 @@ bool PayloadDeliverer::send_gripper_vehicle_command(const int32_t gripper_action
 	vehicle_command_s vcmd;
 	vcmd.timestamp = hrt_absolute_time();
 	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GRIPPER;
-	*(int32_t *)&vcmd.param2 = gripper_action;
+	vcmd.param2 = gripper_action;
+	// Note: Integer type GRIPPER_ACTION gets formatted into a floating point here.
 	return _vehicle_command_pub.publish(vcmd);
 }