diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index ef1200da46..c26bc1ad8a 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -284,6 +284,9 @@ protected: virtual void handle_command_ack(const mavlink_message_t* msg); void handle_set_mode(mavlink_message_t* msg); + void handle_command_int(mavlink_message_t* msg); + virtual MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet); + void handle_mission_request_list(AP_Mission &mission, mavlink_message_t *msg); void handle_mission_request(AP_Mission &mission, mavlink_message_t *msg); void handle_mission_clear_all(AP_Mission &mission, mavlink_message_t *msg); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 3c982e47f5..22068e7297 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2318,6 +2318,10 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg) handle_common_mission_message(msg); break; + case MAVLINK_MSG_ID_COMMAND_INT: + handle_command_int(msg); + break; + case MAVLINK_MSG_ID_SERIAL_CONTROL: handle_serial_control(msg); break; @@ -2769,6 +2773,23 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_message(mavlink_command_long_t &pack return result; } +MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &packet) +{ + return MAV_RESULT_UNSUPPORTED; +} + +void GCS_MAVLINK::handle_command_int(mavlink_message_t *msg) +{ + // decode packet + mavlink_command_int_t packet; + mavlink_msg_command_int_decode(msg, &packet); + + const MAV_RESULT result = handle_command_int_packet(packet); + + // send ACK or NAK + mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result); +} + bool GCS_MAVLINK::try_send_compass_message(const enum ap_message id) { Compass *compass = get_compass();