Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 06:36:45 +08:00
Code Issues Actions 17 Packages Projects Releases Wiki Activity

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

Fix wrong GRIPPER_ACTION conversion from floating point to int32_t

- Due to the MAVLink spec, we actually just convert enums into floating
point, so in PX4 we need to convert the float directly into integer as
well (although there can be precision issues on large numbers)
- This is a limitation in MAVLink spec, and should hopefully be
changed in MAVLink v2
This commit is contained in:
Junwoo Hwang
2022-09-27 16:24:19 +02:00
committed by Beat Küng
parent eb16730400
commit 36a3c716d6
1 changed files with 28 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/payload_deliverer/payload_deliverer.cpp
+28 -3
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,13 +153,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 +180,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 +200,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);
}