diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 22a2933992..1072b644e4 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -625,6 +625,11 @@ bool AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item_t& packet, AP cmd.p1 = packet.param1; // normal=0 inverted=1 break; + case MAV_CMD_DO_GRIPPER: // MAV ID: 211 + cmd.content.gripper.num = packet.param1; // gripper number + cmd.content.gripper.action = packet.param2; // action 0=release, 1=grab. See GRIPPER_ACTION enum + break; + default: // unrecognised command return false; @@ -888,6 +893,11 @@ bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd, packet.param1 = cmd.p1; // normal=0 inverted=1 break; + case MAV_CMD_DO_GRIPPER: // MAV ID: 211 + packet.param1 = cmd.content.gripper.num; // gripper number + packet.param2 = cmd.content.gripper.action; // action 0=release, 1=grab. See GRIPPER_ACTION enum + break; + default: // unrecognised command return false; diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index bfd04261ac..b0e0374b6d 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -121,6 +121,12 @@ public: float meters; // distance }; + // gripper command structure + struct PACKED Gripper_Command { + uint8_t num; // gripper number + uint8_t action; // action (0 = release, 1 = grab) + }; + union PACKED Content { // Nav_Guided_Command Nav_Guided_Command nav_guided; @@ -158,6 +164,9 @@ public: // cam trigg distance Cam_Trigg_Distance cam_trigg_dist; + // do-gripper + Gripper_Command gripper; + // location Location location; // Waypoint location