yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add straight-line MAVSDK test

Browse Source
This commit is contained in:
Julian Kent
2020-07-09 17:36:24 +02:00
committed by Julian Kent
parent 624d8a58e9
commit db361d7a59
3 changed files with 59 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
test/mavsdk_tests/autopilot_tester.cpp
View File

@@ -164,6 +164,30 @@ void AutopilotTester::prepare_square_mission(MissionOptions mission_options)
REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready); REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready);
} }
void AutopilotTester::prepare_straight_mission(MissionOptions mission_options)
{
const auto ct = get_coordinate_transformation();
Mission::MissionPlan mission_plan {};
mission_plan.mission_items.push_back(create_mission_item({0, 0.}, mission_options, ct));
mission_plan.mission_items.push_back(create_mission_item({mission_options.leg_length_m, 0}, mission_options, ct));
mission_plan.mission_items.push_back(create_mission_item({2 * mission_options.leg_length_m, 0}, mission_options, ct));
mission_plan.mission_items.push_back(create_mission_item({3 * mission_options.leg_length_m, 0}, mission_options, ct));
mission_plan.mission_items.push_back(create_mission_item({4 * mission_options.leg_length_m, 0}, mission_options, ct));
_mission->set_return_to_launch_after_mission(mission_options.rtl_at_end);
std::promise<void> prom;
auto fut = prom.get_future();
_mission->upload_mission_async(mission_plan, [&prom](Mission::Result result) {
REQUIRE(Mission::Result::Success == result);
prom.set_value();
});
REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready);
}
void AutopilotTester::execute_mission() void AutopilotTester::execute_mission()
{ {
std::promise<void> prom; std::promise<void> prom;
@@ -203,6 +227,7 @@ Mission::MissionItem AutopilotTester::create_mission_item(
mission_item.latitude_deg = pos_north.latitude_deg; mission_item.latitude_deg = pos_north.latitude_deg;
mission_item.longitude_deg = pos_north.longitude_deg; mission_item.longitude_deg = pos_north.longitude_deg;
mission_item.relative_altitude_m = mission_options.relative_altitude_m; mission_item.relative_altitude_m = mission_options.relative_altitude_m;
mission_item.is_fly_through = mission_options.fly_through;
return mission_item; return mission_item;
} }
@@ -221,6 +246,18 @@ void AutopilotTester::offboard_goto(const Offboard::PositionNedYaw &target, floa
std::cout << "Target position reached" << std::endl; std::cout << "Target position reached" << std::endl;
} }
void AutopilotTester::check_mission_item_speed_above(int item_index, float min_speed)
{
_telemetry->subscribe_velocity_ned([item_index, min_speed, this](Telemetry::VelocityNed velocity) {
float horizontal = std::hypot(velocity.north_m_s, velocity.east_m_s);
auto progress = _mission->mission_progress();
if (progress.current == item_index) {
CHECK(horizontal > min_speed);
}
});
}
void AutopilotTester::offboard_land() void AutopilotTester::offboard_land()
{ {
Offboard::VelocityNedYaw land_velocity; Offboard::VelocityNedYaw land_velocity;

3
test/mavsdk_tests/autopilot_tester.h
View File

@@ -56,6 +56,7 @@ public:
double leg_length_m {20.0}; double leg_length_m {20.0};
double relative_altitude_m {10.0}; double relative_altitude_m {10.0};
bool rtl_at_end {false}; bool rtl_at_end {false};
bool fly_through {false};
}; };
void connect(const std::string uri); void connect(const std::string uri);
@@ -72,12 +73,14 @@ public:
void wait_until_disarmed(std::chrono::seconds timeout_duration = std::chrono::seconds(90)); void wait_until_disarmed(std::chrono::seconds timeout_duration = std::chrono::seconds(90));
void wait_until_hovering(); void wait_until_hovering();
void prepare_square_mission(MissionOptions mission_options); void prepare_square_mission(MissionOptions mission_options);
void prepare_straight_mission(MissionOptions mission_options);
void execute_mission(); void execute_mission();
void execute_rtl(); void execute_rtl();
void offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m = 0.3f, void offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m = 0.3f,
std::chrono::seconds timeout_duration = std::chrono::seconds(60)); std::chrono::seconds timeout_duration = std::chrono::seconds(60));
void offboard_land(); void offboard_land();
void request_ground_truth(); void request_ground_truth();
void check_mission_item_speed_above(int item_index, float min_speed);
private: private:

19
test/mavsdk_tests/test_multicopter_mission.cpp
View File

@@ -78,3 +78,22 @@ TEST_CASE("Fly square Multicopter Missions", "[multicopter][vtol]")
tester.wait_until_disarmed(); tester.wait_until_disarmed();
} }
} }
TEST_CASE("Fly straight Multicopter Mission", "[multicopter]")
{
AutopilotTester tester;
tester.connect(connection_url);
tester.wait_until_ready();
AutopilotTester::MissionOptions mission_options;
mission_options.rtl_at_end = false;
mission_options.fly_through = true;
tester.prepare_straight_mission(mission_options);
tester.check_mission_item_speed_above(2, 4.5);
tester.arm();
tester.execute_mission();
tester.wait_until_hovering();
tester.execute_rtl();
tester.wait_until_disarmed();
}