From e19d245355725abd634635de0990dabb8ca447dc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 7 May 2025 20:57:27 +1200 Subject: [PATCH] mavsdk_tests: remove hacks for rally points We can now just use MAVSDK to upload rally points. --- test/mavsdk_tests/autopilot_tester.h | 1 + test/mavsdk_tests/autopilot_tester_rtl.cpp | 81 +++++----------------- test/mavsdk_tests/autopilot_tester_rtl.h | 8 +-- test/mavsdk_tests/test_vtol_rtl.cpp | 14 ++-- 4 files changed, 28 insertions(+), 76 deletions(-) diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 8fcb48b58c..e7f5f895b7 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -191,6 +191,7 @@ public: protected: mavsdk::Param *getParams() const { return _param.get();} mavsdk::Telemetry *getTelemetry() const { return _telemetry.get();} + mavsdk::MissionRaw *getMissionRaw() const { return _mission_raw.get();} mavsdk::ManualControl *getManualControl() const { return _manual_control.get();} MavlinkPassthrough *getMavlinkPassthrough() const { return _mavlink_passthrough.get();} std::shared_ptr get_system() { return _mavsdk.systems().at(0);} diff --git a/test/mavsdk_tests/autopilot_tester_rtl.cpp b/test/mavsdk_tests/autopilot_tester_rtl.cpp index df2bd946ea..200cf66a24 100644 --- a/test/mavsdk_tests/autopilot_tester_rtl.cpp +++ b/test/mavsdk_tests/autopilot_tester_rtl.cpp @@ -64,54 +64,9 @@ void AutopilotTesterRtl::set_takeoff_land_requirements(int req) CHECK(getParams()->set_param_int("MIS_TKO_LAND_REQ", req) == Param::Result::Success); } -void AutopilotTesterRtl::upload_custom_mission(std::chrono::seconds timeout) +void AutopilotTesterRtl::upload_rally_points() { - std::promise prom; - auto fut = prom.get_future(); - - uint8_t mission_type = _custom_mission[0].mission_type; - // Register callback to mission item request. - add_mavlink_message_callback(MAVLINK_MSG_ID_MISSION_REQUEST_INT, [this, mission_type, - &prom](const mavlink_message_t &message) { - - mavlink_mission_request_int_t request_int_message; - mavlink_msg_mission_request_int_decode(&message, &request_int_message); - - if (request_int_message.mission_type == mission_type) { - // send requested element. - getMavlinkPassthrough()->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { - mavlink_message_t message; - mavlink_msg_mission_item_int_encode_chan(mavlink_address.system_id, mavlink_address.component_id, channel, - &message, &(_custom_mission[request_int_message.seq])); - return message; - }); - - if (request_int_message.seq + 1U == _custom_mission.size()) { - add_mavlink_message_callback(MAVLINK_MSG_ID_MISSION_REQUEST_INT, nullptr); - prom.set_value(); - } - } - }); - - // send mission count - mavlink_mission_count_t mission_count_message; - mission_count_message.count = _custom_mission.size(); - mission_count_message.target_system = getMavlinkPassthrough()->get_target_sysid(); - mission_count_message.target_component = getMavlinkPassthrough()->get_target_compid(); - mission_count_message.mission_type = mission_type; - - mavlink_message_t mission_count_mavlink_message; - mavlink_msg_mission_count_encode(getMavlinkPassthrough()->get_our_sysid(), getMavlinkPassthrough()->get_our_compid(), - &mission_count_mavlink_message, &mission_count_message); - - getMavlinkPassthrough()->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) { - mavlink_message_t message; - mavlink_msg_mission_count_encode_chan(mavlink_address.system_id, mavlink_address.component_id, channel, - &message, &mission_count_message); - return message; - }); - - REQUIRE(fut.wait_for(timeout) == std::future_status::ready); + REQUIRE(getMissionRaw()->upload_rally_points(_rally_points) == MissionRaw::Result::Success); } void AutopilotTesterRtl::add_home_to_rally_point() @@ -128,13 +83,13 @@ void AutopilotTesterRtl::add_home_with_approaches_to_rally_point() void AutopilotTesterRtl::add_local_rally_point(mavsdk::geometry::CoordinateTransformation::LocalCoordinate local_coordinate) { - _rally_points.push_back(local_coordinate); + _local_rally_points.push_back(local_coordinate); mavsdk::geometry::CoordinateTransformation ct(get_coordinate_transformation()); mavsdk::geometry::CoordinateTransformation::GlobalCoordinate pos(ct.global_from_local(local_coordinate)); // Set rally point - mavlink_mission_item_int_t tmp_mission_item; + mavsdk::MissionRaw::MissionItem tmp_mission_item; tmp_mission_item.param1 = 0.f; tmp_mission_item.param2 = 0.f; tmp_mission_item.param3 = 0.f; @@ -142,16 +97,15 @@ void AutopilotTesterRtl::add_local_rally_point(mavsdk::geometry::CoordinateTrans tmp_mission_item.x = static_cast(pos.latitude_deg * 1E7); tmp_mission_item.y = static_cast(pos.longitude_deg * 1E7); tmp_mission_item.z = 0.f; - tmp_mission_item.seq = static_cast(_custom_mission.size()); + tmp_mission_item.seq = static_cast(_rally_points.size()); tmp_mission_item.command = MAV_CMD_NAV_RALLY_POINT; - tmp_mission_item.target_system = getMavlinkPassthrough()->get_target_sysid(); - tmp_mission_item.target_component = getMavlinkPassthrough()->get_target_compid(); tmp_mission_item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - tmp_mission_item.current = 0; + // FIXME: this is currently required with MAVSDK even though it doesn't make much sense for rally points + tmp_mission_item.current = _rally_points.empty() ? 1 : 0; tmp_mission_item.autocontinue = 0; tmp_mission_item.mission_type = MAV_MISSION_TYPE_RALLY; - _custom_mission.push_back(tmp_mission_item); + _rally_points.push_back(tmp_mission_item); } void AutopilotTesterRtl::add_local_rally_with_approaches_point( @@ -171,7 +125,7 @@ void AutopilotTesterRtl::add_approaches_to_point(mavsdk::geometry::CoordinateTra mavsdk::geometry::CoordinateTransformation::LocalCoordinate tmp_coordinate{local_coordinate}; tmp_coordinate.north_m += 200.; mavsdk::geometry::CoordinateTransformation::GlobalCoordinate pos(ct.global_from_local(tmp_coordinate)); - mavlink_mission_item_int_t tmp_mission_item; + mavsdk::MissionRaw::MissionItem tmp_mission_item; tmp_mission_item.param1 = 0.f; tmp_mission_item.param2 = 80.f; tmp_mission_item.param3 = 0.f; @@ -179,16 +133,15 @@ void AutopilotTesterRtl::add_approaches_to_point(mavsdk::geometry::CoordinateTra tmp_mission_item.x = static_cast(pos.latitude_deg * 1E7); tmp_mission_item.y = static_cast(pos.longitude_deg * 1E7); tmp_mission_item.z = 15.f; - tmp_mission_item.seq = static_cast(_custom_mission.size()); + tmp_mission_item.seq = static_cast(_rally_points.size()); tmp_mission_item.command = MAV_CMD_NAV_LOITER_TO_ALT; - tmp_mission_item.target_system = getMavlinkPassthrough()->get_target_sysid(); - tmp_mission_item.target_component = getMavlinkPassthrough()->get_target_compid(); tmp_mission_item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - tmp_mission_item.current = 0; + // FIXME: this is currently required with MAVSDK even though it doesn't make much sense for rally points + tmp_mission_item.current = _rally_points.empty() ? 1 : 0; tmp_mission_item.autocontinue = 0; tmp_mission_item.mission_type = MAV_MISSION_TYPE_RALLY; - _custom_mission.push_back(tmp_mission_item); + _rally_points.push_back(tmp_mission_item); // Set east loiter to alt tmp_coordinate = local_coordinate; @@ -196,9 +149,9 @@ void AutopilotTesterRtl::add_approaches_to_point(mavsdk::geometry::CoordinateTra pos = ct.global_from_local(tmp_coordinate); tmp_mission_item.x = static_cast(pos.latitude_deg * 1E7); tmp_mission_item.y = static_cast(pos.longitude_deg * 1E7); - tmp_mission_item.seq = static_cast(_custom_mission.size()); + tmp_mission_item.seq = static_cast(_rally_points.size()); - _custom_mission.push_back(tmp_mission_item); + _rally_points.push_back(tmp_mission_item); } void AutopilotTesterRtl::check_rally_point_within(float acceptance_radius_m) @@ -209,7 +162,7 @@ void AutopilotTesterRtl::check_rally_point_within(float acceptance_radius_m) mavsdk::geometry::CoordinateTransformation::GlobalCoordinate pos; bool within_rally_point{false}; - for (const auto &rally_point : _rally_points) { + for (const auto &rally_point : _local_rally_points) { pos = ct.global_from_local(rally_point); land_coord.latitude_deg = pos.latitude_deg; land_coord.longitude_deg = pos.longitude_deg; @@ -238,7 +191,7 @@ void AutopilotTesterRtl::check_rtl_approaches(float acceptance_radius_m, std::ch // We started to loiter down so we should be on the approach loiter bool on_approach_loiter(false); - for (const auto mission_item : _custom_mission) { + for (const auto mission_item : _rally_points) { if (mission_item.command == MAV_CMD_NAV_LOITER_TO_ALT) { mavsdk::geometry::CoordinateTransformation::LocalCoordinate pos(ct.local_from_global({static_cast(mission_item.x) / 1E7, static_cast(mission_item.y) / 1E7})); double rel_distance_to_center = sqrt(sq(position_velocity_ned.position.north_m - pos.north_m) + sq( diff --git a/test/mavsdk_tests/autopilot_tester_rtl.h b/test/mavsdk_tests/autopilot_tester_rtl.h index eaa3c5fd9d..8efbe7ecc4 100644 --- a/test/mavsdk_tests/autopilot_tester_rtl.h +++ b/test/mavsdk_tests/autopilot_tester_rtl.h @@ -59,15 +59,13 @@ public: void connect(const std::string uri); void check_rally_point_within(float acceptance_radius_m); void check_rtl_approaches(float acceptance_radius_m, std::chrono::seconds timeout); - /* NOTE mavsdk mission upload should be used when possible. Only use this when uploading a mission which is not yet suppported by mavsdk. - * Used here to to test the new way of uploading approaches for rally points. */ - void upload_custom_mission(std::chrono::seconds timeout); + void upload_rally_points(); private: void add_approaches_to_point(mavsdk::geometry::CoordinateTransformation::LocalCoordinate local_coordinate); std::unique_ptr _failure{}; - std::vector _custom_mission{}; - std::vector _rally_points{}; + std::vector _rally_points{}; + std::vector _local_rally_points{}; }; diff --git a/test/mavsdk_tests/test_vtol_rtl.cpp b/test/mavsdk_tests/test_vtol_rtl.cpp index c98a3cd3ef..75e28d3820 100644 --- a/test/mavsdk_tests/test_vtol_rtl.cpp +++ b/test/mavsdk_tests/test_vtol_rtl.cpp @@ -105,7 +105,7 @@ TEST_CASE("RTL direct home without approaches", "[vtol]") tester.set_rtl_appr_force(0); // reupload rally points with approaches tester.add_home_to_rally_point(); - tester.upload_custom_mission(std::chrono::seconds(10)); + tester.upload_rally_points(); tester.arm(); tester.execute_rtl_when_reaching_mission_sequence(4); tester.wait_until_disarmed(std::chrono::seconds(150)); @@ -124,7 +124,7 @@ TEST_CASE("RTL direct home without approaches forced", "[vtol]") tester.set_rtl_appr_force(1); // reupload rally points with approaches tester.add_home_to_rally_point(); - tester.upload_custom_mission(std::chrono::seconds(10)); + tester.upload_rally_points(); tester.arm(); tester.execute_rtl_when_reaching_mission_sequence(4); tester.wait_until_disarmed(std::chrono::seconds(150)); @@ -142,7 +142,7 @@ TEST_CASE("RTL direct home with approaches", "[vtol]") tester.set_rtl_type(0); // reupload rally points with approaches tester.add_home_with_approaches_to_rally_point(); - tester.upload_custom_mission(std::chrono::seconds(10)); + tester.upload_rally_points(); tester.arm(); tester.execute_rtl_when_reaching_mission_sequence(4); tester.check_rtl_approaches(5., std::chrono::seconds(60)); @@ -161,7 +161,7 @@ TEST_CASE("RTL direct home not as rally point", "[vtol]") tester.set_rtl_type(1); // reupload rally points with approaches tester.add_home_with_approaches_to_rally_point(); - tester.upload_custom_mission(std::chrono::seconds(10)); + tester.upload_rally_points(); tester.arm(); tester.execute_rtl_when_reaching_mission_sequence(2); tester.wait_until_disarmed(std::chrono::seconds(150)); @@ -180,7 +180,7 @@ TEST_CASE("RTL direct rally without approaches", "[vtol]") tester.set_rtl_appr_force(0); // reupload rally points with approaches tester.add_local_rally_point({100., -200.}); - tester.upload_custom_mission(std::chrono::seconds(10)); + tester.upload_rally_points(); tester.arm(); tester.execute_rtl_when_reaching_mission_sequence(3); tester.wait_until_disarmed(std::chrono::seconds(150)); @@ -200,7 +200,7 @@ TEST_CASE("RTL direct rally without approaches forced", "[vtol]") tester.set_rtl_appr_force(1); // reupload rally points with approaches tester.add_local_rally_point({100., -2000.}); - tester.upload_custom_mission(std::chrono::seconds(10)); + tester.upload_rally_points(); tester.arm(); tester.execute_rtl_when_reaching_mission_sequence(3); tester.wait_until_disarmed(std::chrono::seconds(150)); @@ -219,7 +219,7 @@ TEST_CASE("RTL direct rally with approaches", "[vtol]") tester.set_rtl_appr_force(0); // reupload rally points with approaches tester.add_local_rally_with_approaches_point({100., -200.}); - tester.upload_custom_mission(std::chrono::seconds(10)); + tester.upload_rally_points(); tester.arm(); tester.execute_rtl_when_reaching_mission_sequence(3); tester.check_rtl_approaches(5., std::chrono::seconds(60));