diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 695cf157ef..7dbd32ef4b 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -657,7 +657,7 @@ protected: MAV_RESULT handle_command_camera(const mavlink_command_int_t &packet); MAV_RESULT handle_command_do_set_roi(const mavlink_command_int_t &packet); virtual MAV_RESULT handle_command_do_set_roi(const Location &roi_loc); - MAV_RESULT handle_command_do_gripper(const mavlink_command_long_t &packet); + MAV_RESULT handle_command_do_gripper(const mavlink_command_int_t &packet); MAV_RESULT handle_command_do_sprayer(const mavlink_command_int_t &packet); MAV_RESULT handle_command_do_set_mode(const mavlink_command_int_t &packet); MAV_RESULT handle_command_get_home_position(const mavlink_command_int_t &packet); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 35bfe5159a..fb5615cf35 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4552,7 +4552,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_set_ekf_source_set(const mavlink_command_ } #if AP_GRIPPER_ENABLED -MAV_RESULT GCS_MAVLINK::handle_command_do_gripper(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK::handle_command_do_gripper(const mavlink_command_int_t &packet) { AP_Gripper *gripper = AP::gripper(); if (gripper == nullptr) { @@ -4726,12 +4726,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t switch (packet.command) { -#if AP_GRIPPER_ENABLED - case MAV_CMD_DO_GRIPPER: - result = handle_command_do_gripper(packet); - break; -#endif - #if AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { result = handle_command_request_autopilot_capabilities(packet); @@ -5048,6 +5042,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p case MAV_CMD_DO_FLIGHTTERMINATION: return handle_flight_termination(packet); +#if AP_GRIPPER_ENABLED + case MAV_CMD_DO_GRIPPER: + return handle_command_do_gripper(packet); +#endif + case MAV_CMD_DO_SET_MODE: return handle_command_do_set_mode(packet);