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

payload_deliverer & gripper: Improve intermediate state & vcmd_ack

Browse Source 
Gripper:
- Don't command gripper (via uORB `gripper` topic, which maps into an
actuator via Control Allocation) if we are already at the state we want
(e.g. Grabbed / Released) or in the intermediate state to the state we
want -> This prevents spamming on `gripper` topic

Payload Deliverer:
- Add read-once function for Gripper's released / grabbed state
- Send vehicle_command_ack for both release/grab actions.

TODO: target_system & target_component for the released/grabbed vcmd_ack
is incomplete, since we are not keeping track of the vehicle_command
that corresponds to this. This needs to be dealt with in the future, not
sure what the best solution it is for now.

Possible solutions:
- Queue-ing the vehicle command?
- Tying the gripper's action to specific vehicle command one-on-one, to make sure if we send multiple vehicle commands, we know
which command resulted in the action exactly?)

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

Handle gripper de-initialization upon parameter change

- Also added some local state initialization code to init() function of
Gripper
- This will now make init / de-init more graceful & controlled compared
to before
This commit is contained in:
Junwoo Hwang
2022-10-27 07:51:17 +02:00
committed by Beat Küng
parent 36a3c716d6
commit 6529e39f8b
4 changed files with 107 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/payload_deliverer/gripper.cpp
+32
View File
@@ -62,12 +62,38 @@ void Gripper::init(const GripperConfig &config)
_timeout_us = config.timeout_us;
// Reset internal states
_state = GripperState::IDLE;
_last_command_time = 0;
_released_state_cache = false;
_grabbed_state_cache = false;
// We have valid gripper type & sensor configuration
_valid = true;
}
void Gripper::deinit()
{
// Reset Config variables
_has_feedback_sensor = false;
_timeout_us = 0;
// Reset internal states
_state = GripperState::IDLE;
_last_command_time = 0;
_released_state_cache = false;
_grabbed_state_cache = false;
// Mark gripper instance as invalid
_valid = false;
}
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 +101,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 +119,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;
}