From ede4551291d83bef4ebdb3235e0ea66e7cef6365 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 23 Jul 2026 10:49:50 +0300 Subject: [PATCH] test(mavsdk): add multicopter DO_JUMP set-current regression test Add an integration test for an external MISSION_SET_CURRENT that targets a DO_JUMP item mid-mission - the scenario that used to strand the vehicle on an invalid IDLE setpoint (fixed in the following navigator commit). The test flies a multicopter mission containing a DO_JUMP that loops back to an earlier waypoint, commands the current item to the DO_JUMP index while airborne, and checks that the mission still resolves the jump, runs to completion, lands and disarms within the timeout. Send the set-current as a single raw MISSION_SET_CURRENT via MavlinkPassthrough (fire-and-forget) rather than MissionRaw::set_current_mission_item(). The latter retransmits until the autopilot echoes back the exact requested seq; since the navigator resolves the DO_JUMP to a different seq, that call would re-send indefinitely and continuously reset the mission, which is not the one-shot operator action we want to exercise. Signed-off-by: RomanBapst --- test/mavsdk_tests/CMakeLists.txt | 1 + test/mavsdk_tests/autopilot_tester.cpp | 92 +++++++++++++++++++ test/mavsdk_tests/autopilot_tester.h | 8 ++ .../test_multicopter_mission_do_jump.cpp | 70 ++++++++++++++ 4 files changed, 171 insertions(+) create mode 100644 test/mavsdk_tests/test_multicopter_mission_do_jump.cpp diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 4bcccda57f3..acce31ff41a 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -28,6 +28,7 @@ if(MAVSDK_FOUND) test_multicopter_failure_injection.cpp test_multicopter_failsafe.cpp test_multicopter_mission.cpp + test_multicopter_mission_do_jump.cpp test_multicopter_offboard.cpp test_multicopter_manual.cpp test_multicopter_rtl_with_geofence.cpp diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 10b3b65b915..4f4299ae5d1 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -407,6 +407,98 @@ void AutopilotTester::execute_mission_raw() wait_for_mission_raw_finished(std::chrono::seconds(300)); } +namespace +{ +MissionRaw::MissionItem make_raw_mission_item(uint32_t seq, uint32_t command, uint32_t frame, + float param1, float param2, int32_t x, int32_t y, float z, uint32_t current) +{ + MissionRaw::MissionItem item{}; + item.seq = seq; + item.frame = frame; + item.command = command; + item.current = current; + item.autocontinue = 1; + item.param1 = param1; + item.param2 = param2; + item.param3 = 0.f; + item.param4 = 0.f; + item.x = x; + item.y = y; + item.z = z; + item.mission_type = MAV_MISSION_TYPE_MISSION; + return item; +} +} // namespace + +int AutopilotTester::prepare_multicopter_mission_with_do_jump(const MissionOptions &mission_options, int jump_repeats) +{ + const auto ct = get_coordinate_transformation(); + const float altitude_m = static_cast(mission_options.relative_altitude_m); + const double leg_m = mission_options.leg_length_m; + + // Convert a local (north, east) offset from home into a GLOBAL_RELATIVE_ALT mission item. + auto position_item = [&](uint32_t seq, uint32_t command, double north_m, double east_m, float z, + uint32_t current) { + const auto global = ct.global_from_local({north_m, east_m}); + return make_raw_mission_item(seq, command, MAV_FRAME_GLOBAL_RELATIVE_ALT, 0.f, 0.f, + static_cast(std::lround(global.latitude_deg * 1e7)), + static_cast(std::lround(global.longitude_deg * 1e7)), z, current); + }; + + // A simple square-ish path with a DO_JUMP that loops back to waypoint 1. The DO_JUMP is at seq 3. + const int jump_index = 3; + std::vector items; + items.push_back(position_item(0, MAV_CMD_NAV_TAKEOFF, 0., 0., altitude_m, /*current*/ 1)); + items.push_back(position_item(1, MAV_CMD_NAV_WAYPOINT, leg_m, 0., altitude_m, 0)); + items.push_back(position_item(2, MAV_CMD_NAV_WAYPOINT, leg_m, leg_m, altitude_m, 0)); + items.push_back(make_raw_mission_item(jump_index, MAV_CMD_DO_JUMP, MAV_FRAME_MISSION, + /*param1: target seq*/ 1.f, /*param2: repeats*/ static_cast(jump_repeats), + 0, 0, 0.f, 0)); + items.push_back(position_item(4, MAV_CMD_NAV_WAYPOINT, 0., leg_m, altitude_m, 0)); + items.push_back(position_item(5, MAV_CMD_NAV_LAND, 0., 0., 0.f, 0)); + + REQUIRE(_mission_raw->upload_mission(items) == MissionRaw::Result::Success); + // PX4 needs time to realize that it now has a mission available, so we need to wait a bit here. + sleep_for(std::chrono::seconds(1)); + + return jump_index; +} + +void AutopilotTester::start_mission_raw_and_wait_for_sequence(int sequence_number) +{ + start_and_wait_for_mission_sequence_raw(sequence_number); +} + +void AutopilotTester::send_set_current_mission_item(int index) +{ + // Send a single raw MISSION_SET_CURRENT, fire-and-forget - this mimics a one-shot operator/GCS + // action. We deliberately do NOT use MissionRaw::set_current_mission_item(): that call keeps + // retransmitting MISSION_SET_CURRENT until the autopilot echoes back MISSION_CURRENT with the + // exact requested seq. When the requested item is a DO_JUMP the navigator resolves it to the + // jump target and reports a different seq, so MAVSDK would re-send indefinitely and continuously + // reset the mission, which is not the scenario we want to exercise here. + const uint8_t target_sysid = _mavlink_passthrough->get_target_sysid(); + const uint16_t seq = static_cast(index); + + const MavlinkPassthrough::Result result = _mavlink_passthrough->queue_message( + [target_sysid, seq](MavlinkAddress mavlink_address, uint8_t channel) { + mavlink_message_t message; + mavlink_msg_mission_set_current_pack_chan( + mavlink_address.system_id, + mavlink_address.component_id, + channel, + &message, + target_sysid, + MAV_COMP_ID_AUTOPILOT1, + seq); + return message; + }); + + // Extra parentheses stop Catch2 from decomposing the expression: stringifying + // MavlinkPassthrough::Result pulls in an operator<< that the MAVSDK lib does not export. + REQUIRE((result == MavlinkPassthrough::Result::Success)); +} + void AutopilotTester::execute_rtl() { REQUIRE(Action::Result::Success == _action->return_to_launch()); diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 3a779a5c4c6..93b9a8bfb8a 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -132,6 +132,14 @@ public: void execute_mission_and_get_baro_stuck(); void load_qgc_mission_raw_and_move_here(const std::string &plan_file); void execute_mission_raw(); + // Build and upload a multicopter mission (relative to home) that contains a DO_JUMP item looping + // back to an earlier waypoint. The mission lands at the end so it terminates on its own. Returns + // the sequence index of the DO_JUMP item. + int prepare_multicopter_mission_with_do_jump(const MissionOptions &mission_options, int jump_repeats); + // Start the (raw) mission and block until the vehicle reaches the given sequence number. + void start_mission_raw_and_wait_for_sequence(int sequence_number); + // Send MISSION_SET_CURRENT for the given sequence index (mimics an operator/GCS command). + void send_set_current_mission_item(int index); void execute_rtl(); void execute_land(); void offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m = 0.3f, diff --git a/test/mavsdk_tests/test_multicopter_mission_do_jump.cpp b/test/mavsdk_tests/test_multicopter_mission_do_jump.cpp new file mode 100644 index 00000000000..682290b813a --- /dev/null +++ b/test/mavsdk_tests/test_multicopter_mission_do_jump.cpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "autopilot_tester.h" +#include + +// Regression test for the DO_JUMP "idle setpoint" hang. +// +// Background: an external MISSION_SET_CURRENT sets the navigator's current sequence verbatim, with +// no jump resolution. If it points directly at a DO_JUMP item, the navigator used to load that jump +// as the current item, publish an invalid IDLE position setpoint, and hang there indefinitely +// (observed in a real flight). The fix resolves the jump when loading the current item. +// +// Setup: a multicopter flies a mission that contains a DO_JUMP (item 3, looping back to waypoint 1). +// Mid-mission, we send MISSION_SET_CURRENT pointing at the DO_JUMP index. +// +// Pass: the navigator resolves the jump instead of hanging, and the mission runs to completion, +// landing and disarming within the timeout. +TEST_CASE("Fly Multicopter Mission with external set-current onto a DO_JUMP item", "[multicopter]") +{ + AutopilotTester tester; + tester.connect(connection_url); + tester.wait_until_ready(); + + AutopilotTester::MissionOptions mission_options; + mission_options.leg_length_m = 100.0; + mission_options.relative_altitude_m = 15.0; + const int jump_index = tester.prepare_multicopter_mission_with_do_jump(mission_options, /*jump_repeats*/ 1); + tester.arm(); + + // Let the mission get underway, then - in the middle of the mission - command the current item to + // be the DO_JUMP index. This is the exact scenario that used to strand the vehicle on an IDLE + // setpoint. + tester.start_mission_raw_and_wait_for_sequence(2); + tester.send_set_current_mission_item(jump_index); + + // With the bug the vehicle would hang and never land; with the fix the jump resolves and the + // mission completes normally. + tester.wait_until_disarmed(std::chrono::seconds(240)); +}