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

payload_deliverer: Refactor & Handle vehicle command conflicts

Browse Source 
Refactor
- Require reboot for PD_GRIPPER_EN parameter change
- Define gripper ACTION_NONE for readability. This makes implicit assumption that -1 equals no-action commanded more explicit
- Tidy the scattered vcmd_ack struct handling cases into a single function
- Refactor to remove return in the middle of function: avoids future complications where a programmer may expect the logic at the end of the function to be executed, but isn't

Vehicle Command Handling
- Cancel the previous running vehicle command if we receive a different vehicle command
- Reject vehicle command if we get a same one that is getting executed
- Save the source system & component of currently running vehicle command
- Added note about new discovered edge case of having same entity sending different gripper commands consequently, where an unexpected ack result may be received
This commit is contained in:
Junwoo Hwang
2022-10-27 07:51:17 +02:00
committed by Beat Küng
parent 6529e39f8b
commit c2b2ae55d9
3 changed files with 124 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/payload_deliverer/payload_deliverer.h
+19 -6
View File
@@ -55,6 +55,9 @@ using namespace time_literals;
extern "C" __EXPORT int payload_deliverer_main(int argc, char *argv[]);
// If cached gripper action is set to this value, it means we aren't running any vehicle command
static constexpr int8_t GRIPPER_ACTION_NONE = -1;
/**
* @brief Payload Deliverer Module
*
@@ -125,17 +128,27 @@ private:
void handle_vehicle_command(const hrt_abstime &now, const vehicle_command_s *vehicle_command = nullptr);
/**
* @brief Send DO_GRIPPER vehicle command with specified gripper action correctly formatted
*
* This is useful since vehicle command uses float types for param2, where the gripper action is
* specified, but we want to use int32_t data type for it instead, hence filling out the floating
* point data structure like integer type is necessary.
* @brief Send DO_GRIPPER vehicle command with specified gripper action
*
* @param gripper_command GRIPPER_ACTION_GRAB or GRIPPER_ACTION_RELEASE
*/
bool send_gripper_vehicle_command(const int32_t gripper_action);
Gripper _gripper; // Gripper object to handle gripper action
/**
* @brief Send ack response to DO_GRIPPER vehicle command with specified parameters
*
* For the case of VEHICLE_CMD_RESULT_IN_PROGRESS, progress percentage parameter will be filled out
*/
bool send_gripper_vehicle_command_ack(const hrt_abstime now, const uint8_t command_result, const uint8_t target_system,
const uint8_t target_component);
Gripper _gripper;
// Cached values of the currently running vehicle command for the gripper action
// used for conflicting vehicle commands & successful vehicle command acknowledgements
int8_t _cur_vcmd_gripper_action{GRIPPER_ACTION_NONE};
uint8_t _cur_vcmd_target_system{0};
uint8_t _cur_vcmd_target_component{0};
// Subscription
uORB::SubscriptionCallbackWorkItem _vehicle_command_sub{this, ORB_ID(vehicle_command)};