fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-10 03:41:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

[NEED-UPSTREAMING] Send IN_PROGRESS command ack when actuating gripper

Browse Source 
- This hopefully then alerts the GCS that the command is getting
processed
- Referenced commander's `handle_command` function to implement this. As
it seems like GCS needs the acknowledgement of the command being
processed to execute such commands properly
- Also send FAILED command ack if we can't actuate the gripper
This commit is contained in:
Junwoo Hwang
2022-09-30 10:33:50 +02:00
parent 34b6786f79
commit 634b650b9c
1 changed files with 22 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/payload_deliverer/payload_deliverer.cpp
+22 -1
View File
@@ -137,6 +137,8 @@ void PayloadDeliverer::gripper_update(const hrt_abstime &now)
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!");
}
@@ -151,8 +153,15 @@ 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_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;
}
@@ -168,6 +177,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_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!");
}
}