diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 800de56fce..36df8ad8a8 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -580,10 +580,10 @@ protected: void deadlock_sem(void); // reset a message interval via mavlink: - MAV_RESULT handle_command_set_message_interval(const mavlink_command_long_t &packet); - MAV_RESULT handle_command_get_message_interval(const mavlink_command_long_t &packet); + MAV_RESULT handle_command_set_message_interval(const mavlink_command_int_t &packet); + MAV_RESULT handle_command_get_message_interval(const mavlink_command_int_t &packet); bool get_ap_message_interval(ap_message id, uint16_t &interval_ms) const; - MAV_RESULT handle_command_request_message(const mavlink_command_long_t &packet); + MAV_RESULT handle_command_request_message(const mavlink_command_int_t &packet); MAV_RESULT handle_rc_bind(const mavlink_command_long_t &packet); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 3c0b3febf5..2286831185 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2847,7 +2847,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_aux_function(const mavlink_command_lon return MAV_RESULT_ACCEPTED; } -MAV_RESULT GCS_MAVLINK::handle_command_set_message_interval(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK::handle_command_set_message_interval(const mavlink_command_int_t &packet) { return set_message_interval((uint32_t)packet.param1, (int32_t)packet.param2); } @@ -2917,7 +2917,7 @@ uint8_t GCS::get_channel_from_port_number(uint8_t port_num) return UINT8_MAX; } -MAV_RESULT GCS_MAVLINK::handle_command_request_message(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK::handle_command_request_message(const mavlink_command_int_t &packet) { const uint32_t mavlink_id = (uint32_t)packet.param1; const ap_message id = mavlink_id_to_ap_message_id(mavlink_id); @@ -2949,7 +2949,7 @@ bool GCS_MAVLINK::get_ap_message_interval(ap_message id, uint16_t &interval_ms) return false; } -MAV_RESULT GCS_MAVLINK::handle_command_get_message_interval(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK::handle_command_get_message_interval(const mavlink_command_int_t &packet) { if (txspace() < PAYLOAD_SIZE(chan, MESSAGE_INTERVAL) + PAYLOAD_SIZE(chan, COMMAND_ACK)) { return MAV_RESULT_TEMPORARILY_REJECTED; @@ -4840,18 +4840,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t result = handle_command_do_aux_function(packet); break; - case MAV_CMD_SET_MESSAGE_INTERVAL: - result = handle_command_set_message_interval(packet); - break; - - case MAV_CMD_GET_MESSAGE_INTERVAL: - result = handle_command_get_message_interval(packet); - break; - - case MAV_CMD_REQUEST_MESSAGE: - result = handle_command_request_message(packet); - break; - default: result = try_command_long_as_command_int(packet, msg); break; @@ -5167,6 +5155,18 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p case MAV_CMD_STORAGE_FORMAT: return handle_command_storage_format(packet, msg); #endif + + // support for dealing with streamrate for a specific message and + // requesting a message instance: + case MAV_CMD_SET_MESSAGE_INTERVAL: + return handle_command_set_message_interval(packet); + + case MAV_CMD_GET_MESSAGE_INTERVAL: + return handle_command_get_message_interval(packet); + + case MAV_CMD_REQUEST_MESSAGE: + return handle_command_request_message(packet); + } return MAV_RESULT_UNSUPPORTED;