|
|
|
@ -40,7 +40,7 @@ std::string connection_url {"udp://"};
@@ -40,7 +40,7 @@ std::string connection_url {"udp://"};
|
|
|
|
|
void AutopilotTester::connect(const std::string uri) |
|
|
|
|
{ |
|
|
|
|
ConnectionResult ret = _mavsdk.add_any_connection(uri); |
|
|
|
|
REQUIRE(ret == ConnectionResult::SUCCESS); |
|
|
|
|
REQUIRE(ret == ConnectionResult::Success); |
|
|
|
|
|
|
|
|
|
std::cout << "Waiting for system connect" << std::endl; |
|
|
|
|
REQUIRE(poll_condition_with_timeout( |
|
|
|
@ -67,11 +67,11 @@ void AutopilotTester::wait_until_ready_local_position_only()
@@ -67,11 +67,11 @@ void AutopilotTester::wait_until_ready_local_position_only()
|
|
|
|
|
CHECK(poll_condition_with_timeout( |
|
|
|
|
[this]() { |
|
|
|
|
return |
|
|
|
|
(_telemetry->health().gyrometer_calibration_ok && |
|
|
|
|
_telemetry->health().accelerometer_calibration_ok && |
|
|
|
|
_telemetry->health().magnetometer_calibration_ok && |
|
|
|
|
_telemetry->health().level_calibration_ok && |
|
|
|
|
_telemetry->health().local_position_ok); |
|
|
|
|
(_telemetry->health().is_gyrometer_calibration_ok && |
|
|
|
|
_telemetry->health().is_accelerometer_calibration_ok && |
|
|
|
|
_telemetry->health().is_magnetometer_calibration_ok && |
|
|
|
|
_telemetry->health().is_level_calibration_ok && |
|
|
|
|
_telemetry->health().is_local_position_ok); |
|
|
|
|
}, std::chrono::seconds(20))); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -93,40 +93,40 @@ void AutopilotTester::check_home_within(float acceptance_radius_m)
@@ -93,40 +93,40 @@ void AutopilotTester::check_home_within(float acceptance_radius_m)
|
|
|
|
|
|
|
|
|
|
void AutopilotTester::set_takeoff_altitude(const float altitude_m) |
|
|
|
|
{ |
|
|
|
|
CHECK(Action::Result::SUCCESS == _action->set_takeoff_altitude(altitude_m)); |
|
|
|
|
CHECK(Action::Result::Success == _action->set_takeoff_altitude(altitude_m)); |
|
|
|
|
const auto result = _action->get_takeoff_altitude(); |
|
|
|
|
CHECK(result.first == Action::Result::SUCCESS); |
|
|
|
|
CHECK(result.first == Action::Result::Success); |
|
|
|
|
CHECK(result.second == Approx(altitude_m)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void AutopilotTester::arm() |
|
|
|
|
{ |
|
|
|
|
const auto result = _action->arm(); |
|
|
|
|
REQUIRE(result == Action::Result::SUCCESS); |
|
|
|
|
REQUIRE(result == Action::Result::Success); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void AutopilotTester::takeoff() |
|
|
|
|
{ |
|
|
|
|
const auto result = _action->takeoff(); |
|
|
|
|
REQUIRE(result == Action::Result::SUCCESS); |
|
|
|
|
REQUIRE(result == Action::Result::Success); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void AutopilotTester::land() |
|
|
|
|
{ |
|
|
|
|
const auto result = _action->land(); |
|
|
|
|
REQUIRE(result == Action::Result::SUCCESS); |
|
|
|
|
REQUIRE(result == Action::Result::Success); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void AutopilotTester::transition_to_fixedwing() |
|
|
|
|
{ |
|
|
|
|
const auto result = _action->transition_to_fixedwing(); |
|
|
|
|
REQUIRE(result == Action::Result::SUCCESS); |
|
|
|
|
REQUIRE(result == Action::Result::Success); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void AutopilotTester::transition_to_multicopter() |
|
|
|
|
{ |
|
|
|
|
const auto result = _action->transition_to_multicopter(); |
|
|
|
|
REQUIRE(result == Action::Result::SUCCESS); |
|
|
|
|
REQUIRE(result == Action::Result::Success); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void AutopilotTester::wait_until_disarmed(std::chrono::seconds timeout_duration) |
|
|
|
@ -138,26 +138,26 @@ void AutopilotTester::wait_until_disarmed(std::chrono::seconds timeout_duration)
@@ -138,26 +138,26 @@ void AutopilotTester::wait_until_disarmed(std::chrono::seconds timeout_duration)
|
|
|
|
|
void AutopilotTester::wait_until_hovering() |
|
|
|
|
{ |
|
|
|
|
REQUIRE(poll_condition_with_timeout( |
|
|
|
|
[this]() { return _telemetry->landed_state() == Telemetry::LandedState::IN_AIR; }, std::chrono::seconds(20))); |
|
|
|
|
[this]() { return _telemetry->landed_state() == Telemetry::LandedState::InAir; }, std::chrono::seconds(20))); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void AutopilotTester::prepare_square_mission(MissionOptions mission_options) |
|
|
|
|
{ |
|
|
|
|
const auto ct = get_coordinate_transformation(); |
|
|
|
|
|
|
|
|
|
std::vector<std::shared_ptr<MissionItem>> mission_items {}; |
|
|
|
|
mission_items.push_back(create_mission_item({mission_options.leg_length_m, 0.}, mission_options, ct)); |
|
|
|
|
mission_items.push_back(create_mission_item({mission_options.leg_length_m, mission_options.leg_length_m}, |
|
|
|
|
mission_options, ct)); |
|
|
|
|
mission_items.push_back(create_mission_item({0., mission_options.leg_length_m}, mission_options, ct)); |
|
|
|
|
Mission::MissionPlan mission_plan {}; |
|
|
|
|
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({mission_options.leg_length_m, mission_options.leg_length_m}, |
|
|
|
|
mission_options, ct)); |
|
|
|
|
mission_plan.mission_items.push_back(create_mission_item({0., mission_options.leg_length_m}, 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_items, [&prom](Mission::Result result) { |
|
|
|
|
REQUIRE(Mission::Result::SUCCESS == result); |
|
|
|
|
_mission->upload_mission_async(mission_plan, [&prom](Mission::Result result) { |
|
|
|
|
REQUIRE(Mission::Result::Success == result); |
|
|
|
|
prom.set_value(); |
|
|
|
|
}); |
|
|
|
|
|
|
|
|
@ -170,48 +170,52 @@ void AutopilotTester::execute_mission()
@@ -170,48 +170,52 @@ void AutopilotTester::execute_mission()
|
|
|
|
|
auto fut = prom.get_future(); |
|
|
|
|
|
|
|
|
|
_mission->start_mission_async([&prom](Mission::Result result) { |
|
|
|
|
REQUIRE(Mission::Result::SUCCESS == result); |
|
|
|
|
REQUIRE(Mission::Result::Success == result); |
|
|
|
|
prom.set_value(); |
|
|
|
|
}); |
|
|
|
|
|
|
|
|
|
// TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc.
|
|
|
|
|
|
|
|
|
|
REQUIRE(poll_condition_with_timeout( |
|
|
|
|
[this]() { return _mission->mission_finished(); }, std::chrono::seconds(60))); |
|
|
|
|
[this]() { |
|
|
|
|
auto result = _mission->is_mission_finished(); |
|
|
|
|
return result.first == Mission::Result::Success && result.second; |
|
|
|
|
}, std::chrono::seconds(60))); |
|
|
|
|
|
|
|
|
|
REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
CoordinateTransformation AutopilotTester::get_coordinate_transformation() |
|
|
|
|
{ |
|
|
|
|
const auto home = _telemetry->home_position(); |
|
|
|
|
const auto home = _telemetry->home(); |
|
|
|
|
CHECK(std::isfinite(home.latitude_deg)); |
|
|
|
|
CHECK(std::isfinite(home.longitude_deg)); |
|
|
|
|
return CoordinateTransformation({home.latitude_deg, home.longitude_deg}); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
std::shared_ptr<MissionItem> AutopilotTester::create_mission_item( |
|
|
|
|
Mission::MissionItem AutopilotTester::create_mission_item( |
|
|
|
|
const CoordinateTransformation::LocalCoordinate &local_coordinate, |
|
|
|
|
const MissionOptions &mission_options, |
|
|
|
|
const CoordinateTransformation &ct) |
|
|
|
|
{ |
|
|
|
|
auto mission_item = std::make_shared<MissionItem>(); |
|
|
|
|
auto mission_item = Mission::MissionItem{}; |
|
|
|
|
const auto pos_north = ct.global_from_local(local_coordinate); |
|
|
|
|
mission_item->set_position(pos_north.latitude_deg, pos_north.longitude_deg); |
|
|
|
|
mission_item->set_relative_altitude(mission_options.relative_altitude_m); |
|
|
|
|
mission_item.latitude_deg = pos_north.latitude_deg; |
|
|
|
|
mission_item.longitude_deg = pos_north.longitude_deg; |
|
|
|
|
mission_item.relative_altitude_m = mission_options.relative_altitude_m; |
|
|
|
|
return mission_item; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void AutopilotTester::execute_rtl() |
|
|
|
|
{ |
|
|
|
|
REQUIRE(Action::Result::SUCCESS == _action->return_to_launch()); |
|
|
|
|
REQUIRE(Action::Result::Success == _action->return_to_launch()); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void AutopilotTester::offboard_goto(const Offboard::PositionNEDYaw &target, float acceptance_radius_m, |
|
|
|
|
void AutopilotTester::offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m, |
|
|
|
|
std::chrono::seconds timeout_duration) |
|
|
|
|
{ |
|
|
|
|
_offboard->set_position_ned(target); |
|
|
|
|
REQUIRE(_offboard->start() == Offboard::Result::SUCCESS); |
|
|
|
|
REQUIRE(_offboard->start() == Offboard::Result::Success); |
|
|
|
|
CHECK(poll_condition_with_timeout( |
|
|
|
|
[ = ]() { return estimated_position_close_to(target, acceptance_radius_m); }, timeout_duration)); |
|
|
|
|
std::cout << "Target position reached" << std::endl; |
|
|
|
@ -219,7 +223,7 @@ void AutopilotTester::offboard_goto(const Offboard::PositionNEDYaw &target, floa
@@ -219,7 +223,7 @@ void AutopilotTester::offboard_goto(const Offboard::PositionNEDYaw &target, floa
|
|
|
|
|
|
|
|
|
|
void AutopilotTester::offboard_land() |
|
|
|
|
{ |
|
|
|
|
Offboard::VelocityNEDYaw land_velocity; |
|
|
|
|
Offboard::VelocityNedYaw land_velocity; |
|
|
|
|
land_velocity.north_m_s = 0.0f; |
|
|
|
|
land_velocity.east_m_s = 0.0f; |
|
|
|
|
land_velocity.down_m_s = 1.0f; |
|
|
|
@ -227,25 +231,25 @@ void AutopilotTester::offboard_land()
@@ -227,25 +231,25 @@ void AutopilotTester::offboard_land()
|
|
|
|
|
_offboard->set_velocity_ned(land_velocity); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool AutopilotTester::estimated_position_close_to(const Offboard::PositionNEDYaw &target_pos, float acceptance_radius_m) |
|
|
|
|
bool AutopilotTester::estimated_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m) |
|
|
|
|
{ |
|
|
|
|
Telemetry::PositionNED est_pos = _telemetry->position_velocity_ned().position; |
|
|
|
|
Telemetry::PositionNed est_pos = _telemetry->position_velocity_ned().position; |
|
|
|
|
return sq(est_pos.north_m - target_pos.north_m) + |
|
|
|
|
sq(est_pos.east_m - target_pos.east_m) + |
|
|
|
|
sq(est_pos.down_m - target_pos.down_m) < sq(acceptance_radius_m); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool AutopilotTester::estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw &target_pos, |
|
|
|
|
bool AutopilotTester::estimated_horizontal_position_close_to(const Offboard::PositionNedYaw &target_pos, |
|
|
|
|
float acceptance_radius_m) |
|
|
|
|
{ |
|
|
|
|
Telemetry::PositionNED est_pos = _telemetry->position_velocity_ned().position; |
|
|
|
|
Telemetry::PositionNed est_pos = _telemetry->position_velocity_ned().position; |
|
|
|
|
return sq(est_pos.north_m - target_pos.north_m) + |
|
|
|
|
sq(est_pos.east_m - target_pos.east_m) < sq(acceptance_radius_m); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void AutopilotTester::request_ground_truth() |
|
|
|
|
{ |
|
|
|
|
CHECK(_telemetry->set_rate_ground_truth(15) == Telemetry::Result::SUCCESS); |
|
|
|
|
CHECK(_telemetry->set_rate_ground_truth(15) == Telemetry::Result::Success); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, |
|
|
|
@ -260,7 +264,10 @@ bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry:
@@ -260,7 +264,10 @@ bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry:
|
|
|
|
|
Telemetry::GroundTruth current_pos = _telemetry->ground_truth(); |
|
|
|
|
CHECK(std::isfinite(current_pos.latitude_deg)); |
|
|
|
|
CHECK(std::isfinite(current_pos.longitude_deg)); |
|
|
|
|
LocalCoordinate local_pos = ct.local_from_global(GlobalCoordinate{current_pos.latitude_deg, current_pos.longitude_deg}); |
|
|
|
|
GlobalCoordinate global_current; |
|
|
|
|
global_current.latitude_deg = current_pos.latitude_deg; |
|
|
|
|
global_current.longitude_deg = current_pos.longitude_deg; |
|
|
|
|
LocalCoordinate local_pos = ct.local_from_global(global_current); |
|
|
|
|
const double distance = sqrt(sq(local_pos.north_m) + sq(local_pos.east_m)); |
|
|
|
|
const bool pass = distance < acceptance_radius_m; |
|
|
|
|
|
|
|
|
|