From e65d48c0708798c05dc06947913de4dffd013ce8 Mon Sep 17 00:00:00 2001
From: Junwoo Hwang <junwoo091400@gmail.com>
Date: Wed, 21 Sep 2022 13:52:43 +0200
Subject: [PATCH] payload_deliverer: Fix Gripper unitialized message spamming

- When the gripper was uninitialized, the error message popped up
everytime the module had 'any' vehicle command received. But we should
be spamming only when we do actually get 'DO_GRIPPER' command received,
this commit adds that feature
- Also converted error messages for CLI when gripper test functinality
fails, so that user would get a feedback (originally, the PX4_DEBUG was
of course not outputting everything)
---
 .../payload_deliverer/payload_deliverer.cpp   | 26 ++++++++++++++-----
 .../payload_deliverer/payload_deliverer.h     | 10 +++++++
 2 files changed, 29 insertions(+), 7 deletions(-)

diff --git a/src/modules/payload_deliverer/payload_deliverer.cpp b/src/modules/payload_deliverer/payload_deliverer.cpp
index 0d402f3892..0de0af86a4 100644
--- a/src/modules/payload_deliverer/payload_deliverer.cpp
+++ b/src/modules/payload_deliverer/payload_deliverer.cpp
@@ -83,7 +83,6 @@ bool PayloadDeliverer::initialize_gripper()
 		return true;
 	}
 
-
 }
 
 void PayloadDeliverer::parameter_update()
@@ -110,6 +109,9 @@ void PayloadDeliverer::Run()
 		parameter_update();
 	}
 
+	// Update delivery mechanism's state
+	gripper_update(now);
+
 	if (_vehicle_command_sub.update(&vcmd)) {
 		handle_vehicle_command(now, &vcmd);
 
@@ -119,16 +121,17 @@ void PayloadDeliverer::Run()
 	}
 }
 
-void PayloadDeliverer::handle_vehicle_command(const hrt_abstime &now,  const vehicle_command_s *vehicle_command)
+void PayloadDeliverer::gripper_update(const hrt_abstime &now)
 {
 	if (!_gripper.is_valid()) {
-		PX4_WARN("Gripper instance not valid but vehicle command was received. Gripper won't work!");
+		// Try initializing gripper
+		initialize_gripper();
 		return;
 	}
 
 	_gripper.update();
 
-	// Process successful gripper release acknowledgement
+	// Publish a successful gripper release acknowledgement
 	if (_gripper.released_read_once()) {
 		vehicle_command_ack_s vcmd_ack{};
 		vcmd_ack.timestamp = now;
@@ -137,7 +140,10 @@ void PayloadDeliverer::handle_vehicle_command(const hrt_abstime &now,  const veh
 		_vehicle_command_ack_pub.publish(vcmd_ack);
 		PX4_DEBUG("Payload Drop Successful Ack Sent!");
 	}
+}
 
+void PayloadDeliverer::handle_vehicle_command(const hrt_abstime &now,  const vehicle_command_s *vehicle_command)
+{
 	// If there's no vehicle command to process, just return
 	if (vehicle_command == nullptr) {
 		return;
@@ -145,6 +151,12 @@ 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()) {
+			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
 
 		switch (gripper_action) {
@@ -171,7 +183,7 @@ bool PayloadDeliverer::send_gripper_vehicle_command(const int32_t gripper_action
 void PayloadDeliverer::gripper_test()
 {
 	if (!_gripper.is_valid()) {
-		PX4_DEBUG("Gripper is not initialized correctly!");
+		PX4_INFO("Gripper is not initialized correctly!");
 		return;
 	}
 
@@ -187,7 +199,7 @@ void PayloadDeliverer::gripper_test()
 void PayloadDeliverer::gripper_open()
 {
 	if (!_gripper.is_valid()) {
-		PX4_DEBUG("Gripper is not initialized correctly!");
+		PX4_INFO("Gripper is not initialized correctly!");
 		return;
 	}
 
@@ -197,7 +209,7 @@ void PayloadDeliverer::gripper_open()
 void PayloadDeliverer::gripper_close()
 {
 	if (!_gripper.is_valid()) {
-		PX4_DEBUG("Gripper is not initialized correctly!");
+		PX4_INFO("Gripper is not initialized correctly!");
 		return;
 	}
 
diff --git a/src/modules/payload_deliverer/payload_deliverer.h b/src/modules/payload_deliverer/payload_deliverer.h
index 4bd18eb1eb..584c2a9fe1 100644
--- a/src/modules/payload_deliverer/payload_deliverer.h
+++ b/src/modules/payload_deliverer/payload_deliverer.h
@@ -101,6 +101,16 @@ private:
 	 */
 	bool initialize_gripper();
 
+	/**
+	 * @brief Update gipper instance's state and send vehicle command ack
+	 *
+	 * This updates the gripper instance to check if the gripper has reached the desired state.
+	 * And if so, it sends a vehicle command ack to the navigator.
+	 *
+	 * If the gripper instance isn't valid (i.e. not initialized), it doesn't do anything
+	 */
+	void gripper_update(const hrt_abstime &now);
+
 	/**
 	 * @brief Commands the payload delivery mechanism based on the received vehicle command
 	 *