fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-07 14:44:14 +12:00 committed by Ramon Roche
parent e50d5d8567
commit 66deae172d
6 changed files with 53 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
test/mavsdk_tests/MAVSDK_VERSION
View File

@ -1 +1 @@
1.3.1
3.0.0

52
test/mavsdk_tests/autopilot_tester.cpp
View File

@ -215,9 +215,10 @@ void AutopilotTester::wait_until_altitude(float rel_altitude_m, std::chrono::sec
auto prom = std::promise<void> {};
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<float, 3> 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<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 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<void> {};
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<void> {};
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<void> {};
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<void> {};
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;
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<void> {};
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<void> {};
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();
}
});

5
test/mavsdk_tests/autopilot_tester.h
View File

@ -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<mavsdk::Action> _action{};
std::unique_ptr<mavsdk::Failure> _failure{};
std::unique_ptr<mavsdk::Info> _info{};
@ -296,6 +295,8 @@ private:
Telemetry::GroundTruth _home{NAN, NAN, NAN};
mavsdk::Telemetry::PositionHandle _check_altitude_handle{};
std::atomic<bool> _should_exit {false};
std::thread _real_time_report_thread {};
};

7
test/mavsdk_tests/autopilot_tester_figure_eight.cpp
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};
}
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);
}
});

24
test/mavsdk_tests/autopilot_tester_rtl.cpp
View File

@ -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);
}

2
test/mavsdk_tests/configs/sitl.json
View File

@ -1,7 +1,7 @@
{
"mode": "sitl",
"simulator": "gazebo",
"mavlink_connection": "udp://:14540",
"mavlink_connection": "udpin://0.0.0.0:14540",
"tests":
[
{