fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 07:33:33 +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-19 07:37:02 -07:00
committed by Ramon Roche
parent 66deae172d
commit e19d245355
4 changed files with 28 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/mavsdk_tests/autopilot_tester_rtl.cpp
+17 -64
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(