From 0b25d8e49ae9dc7e9337afb1ffec1a46c2a8010a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 11 Oct 2023 22:32:36 +1100 Subject: [PATCH] GCS_MAVLink: handle MAV_CMD_DEBUG_TRAP as both long and int --- libraries/GCS_MAVLink/GCS.h | 2 +- libraries/GCS_MAVLink/GCS_Common.cpp | 9 ++++----- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 3dd7839fa0e2e1..ab28d0d007b0c0 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -658,7 +658,7 @@ class GCS_MAVLINK MAV_RESULT handle_command_do_set_mode(const mavlink_command_long_t &packet); MAV_RESULT handle_command_get_home_position(const mavlink_command_long_t &packet); MAV_RESULT handle_command_do_fence_enable(const mavlink_command_long_t &packet); - MAV_RESULT handle_command_debug_trap(const mavlink_command_long_t &packet); + MAV_RESULT handle_command_debug_trap(const mavlink_command_int_t &packet); MAV_RESULT handle_command_set_ekf_source_set(const mavlink_command_int_t &packet); MAV_RESULT handle_command_airframe_configuration(const mavlink_command_int_t &packet); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index ca232be85329f8..1099047d3e3995 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4535,7 +4535,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_get_home_position(const mavlink_command_l return MAV_RESULT_ACCEPTED; } -MAV_RESULT GCS_MAVLINK::handle_command_debug_trap(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK::handle_command_debug_trap(const mavlink_command_int_t &packet) { // magic number must be supplied to trap; you must *really* mean it. if (uint32_t(packet.param1) != 32451) { @@ -4828,10 +4828,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t result = handle_command_get_home_position(packet); break; - case MAV_CMD_DEBUG_TRAP: - result = handle_command_debug_trap(packet); - break; - default: result = try_command_long_as_command_int(packet, msg); break; @@ -5096,6 +5092,9 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p return handle_can_forward(packet, msg); #endif + case MAV_CMD_DEBUG_TRAP: + return handle_command_debug_trap(packet); + case MAV_CMD_DO_AUX_FUNCTION: return handle_command_do_aux_function(packet);