mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
payload_deliverer: Add gripper state output to "status" CLI output
- Previously it was hard to check which state the gripper was in (GRAB/GRABBED/ etc), not even whether it was correctly initialized, this commit adds that functionality
This commit is contained in:
parent
e65d48c070
commit
13d565d707
@ -127,3 +127,26 @@ void Gripper::publish_gripper_command(const int8_t gripper_command)
|
||||
gripper.command = gripper_command;
|
||||
_gripper_pub.publish(gripper);
|
||||
}
|
||||
|
||||
const char *Gripper::get_state_str() const
|
||||
{
|
||||
switch (_state) {
|
||||
case GripperState::GRABBING:
|
||||
return "GRABBING";
|
||||
|
||||
case GripperState::GRABBED:
|
||||
return "GRABBED";
|
||||
|
||||
case GripperState::RELEASING:
|
||||
return "RELEASING";
|
||||
|
||||
case GripperState::RELEASED:
|
||||
return "RELEASED";
|
||||
|
||||
case GripperState::IDLE:
|
||||
return "IDLE";
|
||||
|
||||
default:
|
||||
return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
@ -87,8 +87,13 @@ public:
|
||||
// Update gripper state
|
||||
void update();
|
||||
|
||||
// Return griper's state in string format
|
||||
const char *get_state_str() const;
|
||||
|
||||
// Returns true if in grabbed position either sensed or timeout based
|
||||
bool grabbed() { return _state == GripperState::GRABBED; }
|
||||
|
||||
// Returns true if in grabbing position either sensed or timeout based
|
||||
bool grabbing() { return _state == GripperState::GRABBING; }
|
||||
|
||||
// Returns true if in released position either sensed or timeout based
|
||||
|
||||
@ -216,6 +216,18 @@ void PayloadDeliverer::gripper_close()
|
||||
send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_GRAB);
|
||||
}
|
||||
|
||||
int PayloadDeliverer::print_status()
|
||||
{
|
||||
// Gripper status
|
||||
PX4_INFO("Gripper valid: %s", _gripper.is_valid() ? "True" : "False");
|
||||
|
||||
if (_gripper.is_valid()) {
|
||||
PX4_INFO("Gripper state: %s", _gripper.get_state_str());
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int PayloadDeliverer::custom_command(int argc, char *argv[])
|
||||
{
|
||||
if (argc >= 1) {
|
||||
|
||||
@ -77,6 +77,9 @@ public:
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
/** @see ModuleBase. Override "status" output when invoked via Commandline, to give detailed status **/
|
||||
int print_status() override;
|
||||
|
||||
// Initializes the module
|
||||
bool init();
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user