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: remove hacks for rally points

Browse Source 
We can now just use MAVSDK to upload rally points.
This commit is contained in:
Julian Oes 2025-05-07 20:57:27 +12:00 committed by Ramon Roche
parent 66deae172d
commit e19d245355
4 changed files with 28 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
test/mavsdk_tests/autopilot_tester.h
View File

@ -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<System> get_system() { return _mavsdk.systems().at(0);}

81
test/mavsdk_tests/autopilot_tester_rtl.cpp
View File

@ -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<void> 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<int32_t>(pos.latitude_deg * 1E7);
tmp_mission_item.y = static_cast<int32_t>(pos.longitude_deg * 1E7);
tmp_mission_item.z = 0.f;
tmp_mission_item.seq = static_cast<uint16_t>(_custom_mission.size());
tmp_mission_item.seq = static_cast<uint16_t>(_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<int32_t>(pos.latitude_deg * 1E7);
tmp_mission_item.y = static_cast<int32_t>(pos.longitude_deg * 1E7);
tmp_mission_item.z = 15.f;
tmp_mission_item.seq = static_cast<uint16_t>(_custom_mission.size());
tmp_mission_item.seq = static_cast<uint16_t>(_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<int32_t>(pos.latitude_deg * 1E7);
tmp_mission_item.y = static_cast<int32_t>(pos.longitude_deg * 1E7);
tmp_mission_item.seq = static_cast<uint16_t>(_custom_mission.size());
tmp_mission_item.seq = static_cast<uint16_t>(_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<double>(mission_item.x) / 1E7, static_cast<double>(mission_item.y) / 1E7}));
double rel_distance_to_center = sqrt(sq(position_velocity_ned.position.north_m - pos.north_m) + sq(

8
test/mavsdk_tests/autopilot_tester_rtl.h
View File

@ -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<mavsdk::Failure> _failure{};
std::vector<mavlink_mission_item_int_t> _custom_mission{};
std::vector<mavsdk::geometry::CoordinateTransformation::LocalCoordinate> _rally_points{};
std::vector<mavsdk::MissionRaw::MissionItem> _rally_points{};
std::vector<mavsdk::geometry::CoordinateTransformation::LocalCoordinate> _local_rally_points{};
};

14
test/mavsdk_tests/test_vtol_rtl.cpp
View File

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