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

mavsdk_tests: move waypoints to vehicle position

Browse Source 
This way it shouldn't matter where the test is run.
This commit is contained in:
Julian Oes
2021-04-06 11:35:18 -04:00
committed by Daniel Agar
parent 9eadabcb02
commit cd66a79747
3 changed files with 25 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/mavsdk_tests/autopilot_tester.cpp
+22 -2
View File
@@ -326,11 +326,13 @@ Mission::MissionItem AutopilotTester::create_mission_item(
return mission_item;
}
void AutopilotTester::load_qgc_mission_raw(const std::string &plan_file)
void AutopilotTester::load_qgc_mission_raw_and_move_here(const std::string &plan_file)
{
const auto import_result = _mission_raw->import_qgroundcontrol_mission(plan_file);
auto import_result = _mission_raw->import_qgroundcontrol_mission(plan_file);
REQUIRE(import_result.first == MissionRaw::Result::Success);
move_mission_raw_here(import_result.second.mission_items);
REQUIRE(_mission_raw->upload_mission(import_result.second.mission_items) == MissionRaw::Result::Success);
}
@@ -654,6 +656,24 @@ void AutopilotTester::wait_for_mission_raw_finished(std::chrono::seconds timeout
REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
}
void AutopilotTester::move_mission_raw_here(std::vector<MissionRaw::MissionItem> &mission_items)
{
const auto position = _telemetry->position();
REQUIRE(std::isfinite(position.latitude_deg));
REQUIRE(std::isfinite(position.longitude_deg));
auto offset_x = mission_items[0].x - static_cast<int32_t>(1e7 * position.latitude_deg);
auto offset_y = mission_items[1].y - static_cast<int32_t>(1e7 * position.longitude_deg);
for (auto &item : mission_items) {
if (item.frame == 3) { // MAV_FRAME_GLOBAL_RELATIVE_ALT
item.x -= offset_x;
}
item.y -= offset_y;
}
}
void AutopilotTester::report_speed_factor()
{
// We check the exit flag more often than the speed factor.