fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 00:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavsdk_tests: update to MAVSDK 3.0.0

Browse Source
This commit is contained in:
Julian Oes
2025-05-19 07:37:02 -07:00
committed by Ramon Roche
parent e50d5d8567
commit 66deae172d
6 changed files with 53 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/mavsdk_tests/MAVSDK_VERSION
+1 -1
View File
@@ -1 +1 @@
1.3.1 3.0.0
test/mavsdk_tests/autopilot_tester.cpp
+28 -24
View File
@@ -215,9 +215,10 @@ void AutopilotTester::wait_until_altitude(float rel_altitude_m, std::chrono::sec
auto prom = std::promise<void> {}; auto prom = std::promise<void> {};
auto fut = prom.get_future(); 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) { if (fabs(rel_altitude_m - new_position.relative_altitude_m) <= delta) {
_telemetry->subscribe_position(nullptr); _telemetry->unsubscribe_position(handle);
prom.set_value(); prom.set_value();
} }
}); });
@@ -629,7 +630,8 @@ void AutopilotTester::start_checking_altitude(const float max_deviation_m)
std::array<float, 3> initial_position = get_current_position_ned(); std::array<float, 3> initial_position = get_current_position_ned();
float target_altitude = initial_position[2]; 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); const float current_deviation = fabs((-target_altitude) - new_position.relative_altitude_m);
CHECK(current_deviation <= max_deviation_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() 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) 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); _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, void AutopilotTester::add_mavlink_message_callback(uint16_t message_id,
std::function< void(const mavlink_message_t &)> callback) 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<float, 3> AutopilotTester::get_current_position_ned() std::array<float, 3> AutopilotTester::get_current_position_ned()
@@ -867,11 +864,12 @@ void AutopilotTester::start_and_wait_for_mission_sequence(int sequence_number)
auto prom = std::promise<void> {}; auto prom = std::promise<void> {};
auto fut = prom.get_future(); 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; std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl;
if (progress.current >= sequence_number) { if (progress.current >= sequence_number) {
_mission->subscribe_mission_progress(nullptr); _mission->unsubscribe_mission_progress(handle);
prom.set_value(); prom.set_value();
} }
}); });
@@ -886,11 +884,12 @@ void AutopilotTester::start_and_wait_for_mission_sequence_raw(int sequence_numbe
auto prom = std::promise<void> {}; auto prom = std::promise<void> {};
auto fut = prom.get_future(); 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; std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl;
if (progress.current >= sequence_number) { if (progress.current >= sequence_number) {
_mission_raw->subscribe_mission_progress(nullptr); _mission_raw->unsubscribe_mission_progress(handle);
prom.set_value(); prom.set_value();
} }
}); });
@@ -905,9 +904,10 @@ void AutopilotTester::wait_for_flight_mode(Telemetry::FlightMode flight_mode, st
auto prom = std::promise<void> {}; auto prom = std::promise<void> {};
auto fut = prom.get_future(); 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) { if (new_flight_mode == flight_mode) {
_telemetry->subscribe_flight_mode(nullptr); _telemetry->unsubscribe_flight_mode(handle);
prom.set_value(); prom.set_value();
} }
}); });
@@ -920,9 +920,10 @@ void AutopilotTester::wait_for_landed_state(Telemetry::LandedState landed_state,
auto prom = std::promise<void> {}; auto prom = std::promise<void> {};
auto fut = prom.get_future(); 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) { if (new_landed_state == landed_state) {
_telemetry->subscribe_landed_state(nullptr); _telemetry->unsubscribe_landed_state(handle);
prom.set_value(); prom.set_value();
} }
}); });
@@ -935,7 +936,8 @@ void AutopilotTester::wait_until_speed_lower_than(float speed, std::chrono::seco
auto prom = std::promise<void> {}; auto prom = std::promise<void> {};
auto fut = prom.get_future(); 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<float, 3> current_velocity; std::array<float, 3> current_velocity;
current_velocity[0] = position_velocity_ned.velocity.north_m_s; current_velocity[0] = position_velocity_ned.velocity.north_m_s;
current_velocity[1] = position_velocity_ned.velocity.east_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); const float current_speed = norm(current_velocity);
if (current_speed <= speed) { if (current_speed <= speed) {
_telemetry->subscribe_position_velocity_ned(nullptr); _telemetry->unsubscribe_position_velocity_ned(handle);
prom.set_value(); prom.set_value();
} }
}); });
@@ -956,9 +958,10 @@ void AutopilotTester::wait_for_mission_finished(std::chrono::seconds timeout)
auto prom = std::promise<void> {}; auto prom = std::promise<void> {};
auto fut = prom.get_future(); 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) { if (progress.current == progress.total) {
_mission->subscribe_mission_progress(nullptr); _mission->unsubscribe_mission_progress(handle);
prom.set_value(); prom.set_value();
} }
}); });
@@ -971,9 +974,10 @@ void AutopilotTester::wait_for_mission_raw_finished(std::chrono::seconds timeout
auto prom = std::promise<void> {}; auto prom = std::promise<void> {};
auto fut = prom.get_future(); 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) { if (progress.current == progress.total) {
_mission_raw->subscribe_mission_progress(nullptr); _mission_raw->unsubscribe_mission_progress(handle);
prom.set_value(); prom.set_value();
} }
}); });
test/mavsdk_tests/autopilot_tester.h
+3 -2
View File
@@ -149,7 +149,6 @@ public:
void check_current_altitude(float target_rel_altitude_m, float max_distance_m = 1.5f); 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 execute_rtl_when_reaching_mission_sequence(int sequence_number);
void send_custom_mavlink_command(const MavlinkPassthrough::CommandInt &command); 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 add_mavlink_message_callback(uint16_t message_id, std::function< void(const mavlink_message_t &)> callback);
void enable_fixedwing_mectrics(); void enable_fixedwing_mectrics();
@@ -282,7 +281,7 @@ private:
} }
mavsdk::Mavsdk _mavsdk{}; mavsdk::Mavsdk _mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}};
std::unique_ptr<mavsdk::Action> _action{}; std::unique_ptr<mavsdk::Action> _action{};
std::unique_ptr<mavsdk::Failure> _failure{}; std::unique_ptr<mavsdk::Failure> _failure{};
std::unique_ptr<mavsdk::Info> _info{}; std::unique_ptr<mavsdk::Info> _info{};
@@ -296,6 +295,8 @@ private:
Telemetry::GroundTruth _home{NAN, NAN, NAN}; Telemetry::GroundTruth _home{NAN, NAN, NAN};
mavsdk::Telemetry::PositionHandle _check_altitude_handle{};
std::atomic<bool> _should_exit {false}; std::atomic<bool> _should_exit {false};
std::thread _real_time_report_thread {}; std::thread _real_time_report_thread {};
}; };
test/mavsdk_tests/autopilot_tester_figure_eight.cpp
+4 -3
View File
@@ -103,7 +103,8 @@ void AutopilotTesterFigureEight::check_tracks_figure_eight(std::chrono::seconds
order_to_fly = std::vector<int32_t> {0, 3, 2, 1, 0, 6, 5, 4, 0}; order_to_fly = std::vector<int32_t> {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) { &order_to_fly, this](Telemetry::PositionVelocityNed position_velocity_ned) {
static size_t index{0}; static size_t index{0};
int32_t close_index{-1}; 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 } else { // reached an out of order point
if (index > 0U) { // only set to false if we already hve passed the first center 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); prom.set_value(false);
} }
} }
} }
if (index + 1 == order_to_fly.size()) { if (index + 1 == order_to_fly.size()) {
getTelemetry()->subscribe_position_velocity_ned(nullptr); getTelemetry()->unsubscribe_position_velocity_ned(handle);
prom.set_value(true); prom.set_value(true);
} }
}); });
test/mavsdk_tests/autopilot_tester_rtl.cpp
+16 -8
View File
@@ -79,10 +79,12 @@ void AutopilotTesterRtl::upload_custom_mission(std::chrono::seconds timeout)
if (request_int_message.mission_type == mission_type) { if (request_int_message.mission_type == mission_type) {
// send requested element. // send requested element.
mavlink_message_t mission_item_int_mavlink_message; getMavlinkPassthrough()->queue_message([&](MavlinkAddress mavlink_address, uint8_t channel) {
mavlink_msg_mission_item_int_encode(getMavlinkPassthrough()->get_our_sysid(), getMavlinkPassthrough()->get_our_compid(), mavlink_message_t message;
&mission_item_int_mavlink_message, &(_custom_mission[request_int_message.seq])); mavlink_msg_mission_item_int_encode_chan(mavlink_address.system_id, mavlink_address.component_id, channel,
send_custom_mavlink_message(mission_item_int_mavlink_message); &message, &(_custom_mission[request_int_message.seq]));
return message;
});
if (request_int_message.seq + 1U == _custom_mission.size()) { if (request_int_message.seq + 1U == _custom_mission.size()) {
add_mavlink_message_callback(MAVLINK_MSG_ID_MISSION_REQUEST_INT, nullptr); 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(), mavlink_msg_mission_count_encode(getMavlinkPassthrough()->get_our_sysid(), getMavlinkPassthrough()->get_our_compid(),
&mission_count_mavlink_message, &mission_count_message); &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); 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(return_rtl_alt.first == Param::Result::Success);
REQUIRE(descend_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) { this](Telemetry::PositionVelocityNed position_velocity_ned) {
if ((-position_velocity_ned.position.down_m < return_rtl_alt.second - 3.) 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.) { if (-position_velocity_ned.position.down_m < descend_rtl_alt.second + 3.) {
// We reached the altitude // We reached the altitude
getTelemetry()->subscribe_position_velocity_ned(nullptr); getTelemetry()->unsubscribe_position_velocity_ned(handle);
prom.set_value(true); prom.set_value(true);
return; return;
@@ -252,7 +260,7 @@ void AutopilotTesterRtl::check_rtl_approaches(float acceptance_radius_m, std::ch
} }
if (!on_approach_loiter) { if (!on_approach_loiter) {
getTelemetry()->subscribe_position_velocity_ned(nullptr); getTelemetry()->unsubscribe_position_velocity_ned(handle);
prom.set_value(false); prom.set_value(false);
} }
test/mavsdk_tests/configs/sitl.json
+1 -1
View File
@@ -1,7 +1,7 @@
{ {
"mode": "sitl", "mode": "sitl",
"simulator": "gazebo", "simulator": "gazebo",
"mavlink_connection": "udp://:14540", "mavlink_connection": "udpin://0.0.0.0:14540",
"tests": "tests":
[ [
{ {