From 66deae172d7d7db04cf7c80dff8acc2bf66af4f2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 7 May 2025 14:44:14 +1200 Subject: [PATCH] mavsdk_tests: update to MAVSDK 3.0.0 --- test/mavsdk_tests/MAVSDK_VERSION | 2 +- test/mavsdk_tests/autopilot_tester.cpp | 52 ++++++++++--------- test/mavsdk_tests/autopilot_tester.h | 5 +- .../autopilot_tester_figure_eight.cpp | 7 +-- test/mavsdk_tests/autopilot_tester_rtl.cpp | 24 ++++++--- test/mavsdk_tests/configs/sitl.json | 2 +- 6 files changed, 53 insertions(+), 39 deletions(-) diff --git a/test/mavsdk_tests/MAVSDK_VERSION b/test/mavsdk_tests/MAVSDK_VERSION index 3a3cd8cc8b..4a36342fca 100644 --- a/test/mavsdk_tests/MAVSDK_VERSION +++ b/test/mavsdk_tests/MAVSDK_VERSION @@ -1 +1 @@ -1.3.1 +3.0.0 diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 4344a35ae8..64aaf05614 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -215,9 +215,10 @@ void AutopilotTester::wait_until_altitude(float rel_altitude_m, std::chrono::sec auto prom = std::promise {}; auto fut = prom.get_future(); - _telemetry->subscribe_position([&prom, rel_altitude_m, delta, this](Telemetry::Position new_position) { + Telemetry::PositionHandle handle = _telemetry->subscribe_position([&prom, rel_altitude_m, delta, &handle, + this](Telemetry::Position new_position) { if (fabs(rel_altitude_m - new_position.relative_altitude_m) <= delta) { - _telemetry->subscribe_position(nullptr); + _telemetry->unsubscribe_position(handle); prom.set_value(); } }); @@ -629,7 +630,8 @@ void AutopilotTester::start_checking_altitude(const float max_deviation_m) std::array initial_position = get_current_position_ned(); float target_altitude = initial_position[2]; - _telemetry->subscribe_position([target_altitude, max_deviation_m, this](Telemetry::Position new_position) { + _check_altitude_handle = _telemetry->subscribe_position([target_altitude, max_deviation_m, + this](Telemetry::Position new_position) { const float current_deviation = fabs((-target_altitude) - new_position.relative_altitude_m); CHECK(current_deviation <= max_deviation_m); }); @@ -637,7 +639,7 @@ void AutopilotTester::start_checking_altitude(const float max_deviation_m) void AutopilotTester::stop_checking_altitude() { - _telemetry->subscribe_position(nullptr); + _telemetry->unsubscribe_position(_check_altitude_handle); } void AutopilotTester::check_tracks_mission_raw(float corridor_radius_m, bool reverse) @@ -744,15 +746,10 @@ void AutopilotTester::send_custom_mavlink_command(const MavlinkPassthrough::Comm _mavlink_passthrough->send_command_int(command); } -void AutopilotTester::send_custom_mavlink_message(mavlink_message_t &message) -{ - _mavlink_passthrough->send_message(message); -} - void AutopilotTester::add_mavlink_message_callback(uint16_t message_id, std::function< void(const mavlink_message_t &)> callback) { - _mavlink_passthrough->subscribe_message_async(message_id, std::move(callback)); + _mavlink_passthrough->subscribe_message(message_id, std::move(callback)); } std::array AutopilotTester::get_current_position_ned() @@ -867,11 +864,12 @@ void AutopilotTester::start_and_wait_for_mission_sequence(int sequence_number) auto prom = std::promise {}; auto fut = prom.get_future(); - _mission->subscribe_mission_progress([&prom, this, sequence_number](Mission::MissionProgress progress) { + Mission::MissionProgressHandle handle = _mission->subscribe_mission_progress( + [&prom, &handle, this, sequence_number](Mission::MissionProgress progress) { std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; if (progress.current >= sequence_number) { - _mission->subscribe_mission_progress(nullptr); + _mission->unsubscribe_mission_progress(handle); prom.set_value(); } }); @@ -886,11 +884,12 @@ void AutopilotTester::start_and_wait_for_mission_sequence_raw(int sequence_numbe auto prom = std::promise {}; auto fut = prom.get_future(); - _mission_raw->subscribe_mission_progress([&prom, this, sequence_number](MissionRaw::MissionProgress progress) { + MissionRaw::MissionProgressHandle handle = _mission_raw->subscribe_mission_progress( + [&prom, &handle, this, sequence_number](MissionRaw::MissionProgress progress) { std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; if (progress.current >= sequence_number) { - _mission_raw->subscribe_mission_progress(nullptr); + _mission_raw->unsubscribe_mission_progress(handle); prom.set_value(); } }); @@ -905,9 +904,10 @@ void AutopilotTester::wait_for_flight_mode(Telemetry::FlightMode flight_mode, st auto prom = std::promise {}; auto fut = prom.get_future(); - _telemetry->subscribe_flight_mode([&prom, flight_mode, this](Telemetry::FlightMode new_flight_mode) { + Telemetry::FlightModeHandle handle = _telemetry->subscribe_flight_mode( + [&prom, &handle, flight_mode, this](Telemetry::FlightMode new_flight_mode) { if (new_flight_mode == flight_mode) { - _telemetry->subscribe_flight_mode(nullptr); + _telemetry->unsubscribe_flight_mode(handle); prom.set_value(); } }); @@ -920,9 +920,10 @@ void AutopilotTester::wait_for_landed_state(Telemetry::LandedState landed_state, auto prom = std::promise {}; auto fut = prom.get_future(); - _telemetry->subscribe_landed_state([&prom, landed_state, this](Telemetry::LandedState new_landed_state) { + Telemetry::LandedStateHandle handle = _telemetry->subscribe_landed_state( + [&prom, &handle, landed_state, this](Telemetry::LandedState new_landed_state) { if (new_landed_state == landed_state) { - _telemetry->subscribe_landed_state(nullptr); + _telemetry->unsubscribe_landed_state(handle); prom.set_value(); } }); @@ -935,7 +936,8 @@ void AutopilotTester::wait_until_speed_lower_than(float speed, std::chrono::seco auto prom = std::promise {}; auto fut = prom.get_future(); - _telemetry->subscribe_position_velocity_ned([&prom, speed, this](Telemetry::PositionVelocityNed position_velocity_ned) { + Telemetry::PositionVelocityNedHandle handle = _telemetry->subscribe_position_velocity_ned( + [&prom, &handle, speed, this](Telemetry::PositionVelocityNed position_velocity_ned) { std::array current_velocity; current_velocity[0] = position_velocity_ned.velocity.north_m_s; current_velocity[1] = position_velocity_ned.velocity.east_m_s; @@ -943,7 +945,7 @@ void AutopilotTester::wait_until_speed_lower_than(float speed, std::chrono::seco const float current_speed = norm(current_velocity); if (current_speed <= speed) { - _telemetry->subscribe_position_velocity_ned(nullptr); + _telemetry->unsubscribe_position_velocity_ned(handle); prom.set_value(); } }); @@ -956,9 +958,10 @@ void AutopilotTester::wait_for_mission_finished(std::chrono::seconds timeout) auto prom = std::promise {}; auto fut = prom.get_future(); - _mission->subscribe_mission_progress([&prom, this](Mission::MissionProgress progress) { + Mission::MissionProgressHandle handle = _mission->subscribe_mission_progress( + [&prom, &handle, this](Mission::MissionProgress progress) { if (progress.current == progress.total) { - _mission->subscribe_mission_progress(nullptr); + _mission->unsubscribe_mission_progress(handle); prom.set_value(); } }); @@ -971,9 +974,10 @@ void AutopilotTester::wait_for_mission_raw_finished(std::chrono::seconds timeout auto prom = std::promise {}; auto fut = prom.get_future(); - _mission_raw->subscribe_mission_progress([&prom, this](MissionRaw::MissionProgress progress) { + MissionRaw::MissionProgressHandle handle = _mission_raw->subscribe_mission_progress( + [&prom, &handle, this](MissionRaw::MissionProgress progress) { if (progress.current == progress.total) { - _mission_raw->subscribe_mission_progress(nullptr); + _mission_raw->unsubscribe_mission_progress(handle); prom.set_value(); } }); diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 802bc819a3..8fcb48b58c 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -149,7 +149,6 @@ public: void check_current_altitude(float target_rel_altitude_m, float max_distance_m = 1.5f); void execute_rtl_when_reaching_mission_sequence(int sequence_number); void send_custom_mavlink_command(const MavlinkPassthrough::CommandInt &command); - void send_custom_mavlink_message(mavlink_message_t &message); void add_mavlink_message_callback(uint16_t message_id, std::function< void(const mavlink_message_t &)> callback); void enable_fixedwing_mectrics(); @@ -282,7 +281,7 @@ private: } - mavsdk::Mavsdk _mavsdk{}; + mavsdk::Mavsdk _mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; std::unique_ptr _action{}; std::unique_ptr _failure{}; std::unique_ptr _info{}; @@ -296,6 +295,8 @@ private: Telemetry::GroundTruth _home{NAN, NAN, NAN}; + mavsdk::Telemetry::PositionHandle _check_altitude_handle{}; + std::atomic _should_exit {false}; std::thread _real_time_report_thread {}; }; diff --git a/test/mavsdk_tests/autopilot_tester_figure_eight.cpp b/test/mavsdk_tests/autopilot_tester_figure_eight.cpp index 402c72d3a2..2d6e8f0b85 100644 --- a/test/mavsdk_tests/autopilot_tester_figure_eight.cpp +++ b/test/mavsdk_tests/autopilot_tester_figure_eight.cpp @@ -103,7 +103,8 @@ void AutopilotTesterFigureEight::check_tracks_figure_eight(std::chrono::seconds order_to_fly = std::vector {0, 3, 2, 1, 0, 6, 5, 4, 0}; } - getTelemetry()->subscribe_position_velocity_ned([&figure_eight_point_of_interest, &prom, corridor_radius_m, + Telemetry::PositionVelocityNedHandle handle = getTelemetry()->subscribe_position_velocity_ned( + [&figure_eight_point_of_interest, &prom, &handle, corridor_radius_m, &order_to_fly, this](Telemetry::PositionVelocityNed position_velocity_ned) { static size_t index{0}; int32_t close_index{-1}; @@ -129,14 +130,14 @@ void AutopilotTesterFigureEight::check_tracks_figure_eight(std::chrono::seconds } else { // reached an out of order point if (index > 0U) { // only set to false if we already hve passed the first center point - getTelemetry()->subscribe_position_velocity_ned(nullptr); + getTelemetry()->unsubscribe_position_velocity_ned(handle); prom.set_value(false); } } } if (index + 1 == order_to_fly.size()) { - getTelemetry()->subscribe_position_velocity_ned(nullptr); + getTelemetry()->unsubscribe_position_velocity_ned(handle); prom.set_value(true); } }); diff --git a/test/mavsdk_tests/autopilot_tester_rtl.cpp b/test/mavsdk_tests/autopilot_tester_rtl.cpp index d24b59e90d..df2bd946ea 100644 --- a/test/mavsdk_tests/autopilot_tester_rtl.cpp +++ b/test/mavsdk_tests/autopilot_tester_rtl.cpp @@ -79,10 +79,12 @@ void AutopilotTesterRtl::upload_custom_mission(std::chrono::seconds timeout) if (request_int_message.mission_type == mission_type) { // send requested element. - mavlink_message_t mission_item_int_mavlink_message; - mavlink_msg_mission_item_int_encode(getMavlinkPassthrough()->get_our_sysid(), getMavlinkPassthrough()->get_our_compid(), - &mission_item_int_mavlink_message, &(_custom_mission[request_int_message.seq])); - send_custom_mavlink_message(mission_item_int_mavlink_message); + 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); @@ -102,7 +104,12 @@ void AutopilotTesterRtl::upload_custom_mission(std::chrono::seconds timeout) mavlink_msg_mission_count_encode(getMavlinkPassthrough()->get_our_sysid(), getMavlinkPassthrough()->get_our_compid(), &mission_count_mavlink_message, &mission_count_message); - send_custom_mavlink_message(mission_count_mavlink_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); } @@ -222,7 +229,8 @@ void AutopilotTesterRtl::check_rtl_approaches(float acceptance_radius_m, std::ch REQUIRE(return_rtl_alt.first == Param::Result::Success); REQUIRE(descend_rtl_alt.first == Param::Result::Success); - getTelemetry()->subscribe_position_velocity_ned([&prom, acceptance_radius_m, return_rtl_alt, descend_rtl_alt, ct, + Telemetry::PositionVelocityNedHandle handle = getTelemetry()->subscribe_position_velocity_ned( + [&prom, &handle, acceptance_radius_m, return_rtl_alt, descend_rtl_alt, ct, this](Telemetry::PositionVelocityNed position_velocity_ned) { if ((-position_velocity_ned.position.down_m < return_rtl_alt.second - 3.) @@ -242,7 +250,7 @@ void AutopilotTesterRtl::check_rtl_approaches(float acceptance_radius_m, std::ch if (-position_velocity_ned.position.down_m < descend_rtl_alt.second + 3.) { // We reached the altitude - getTelemetry()->subscribe_position_velocity_ned(nullptr); + getTelemetry()->unsubscribe_position_velocity_ned(handle); prom.set_value(true); return; @@ -252,7 +260,7 @@ void AutopilotTesterRtl::check_rtl_approaches(float acceptance_radius_m, std::ch } if (!on_approach_loiter) { - getTelemetry()->subscribe_position_velocity_ned(nullptr); + getTelemetry()->unsubscribe_position_velocity_ned(handle); prom.set_value(false); } diff --git a/test/mavsdk_tests/configs/sitl.json b/test/mavsdk_tests/configs/sitl.json index e14b73689c..a281c2fe2a 100644 --- a/test/mavsdk_tests/configs/sitl.json +++ b/test/mavsdk_tests/configs/sitl.json @@ -1,7 +1,7 @@ { "mode": "sitl", "simulator": "gazebo", - "mavlink_connection": "udp://:14540", + "mavlink_connection": "udpin://0.0.0.0:14540", "tests": [ {