diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index ab9db2ce81..3dd7839fa0 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -659,7 +659,7 @@ protected: 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_set_ekf_source_set(const mavlink_command_long_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 df7294f5ea..7da2f54178 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4541,7 +4541,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_debug_trap(const mavlink_command_long_t & return MAV_RESULT_UNSUPPORTED; } -MAV_RESULT GCS_MAVLINK::handle_command_set_ekf_source_set(const mavlink_command_long_t &packet) +MAV_RESULT GCS_MAVLINK::handle_command_set_ekf_source_set(const mavlink_command_int_t &packet) { // source set must be between 1 and 3 uint32_t source_set = uint32_t(packet.param1); @@ -4826,10 +4826,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t result = handle_command_debug_trap(packet); break; - case MAV_CMD_SET_EKF_SOURCE_SET: - result = handle_command_set_ekf_source_set(packet); - break; - default: result = try_command_long_as_command_int(packet, msg); break; @@ -5163,6 +5159,9 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p } #endif // AP_SCRIPTING_ENABLED + case MAV_CMD_SET_EKF_SOURCE_SET: + return handle_command_set_ekf_source_set(packet); + #if AP_FILESYSTEM_FORMAT_ENABLED case MAV_CMD_STORAGE_FORMAT: return handle_command_storage_format(packet, msg);