fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-13 04:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Showcase clang-format google style

Browse Source
This commit is contained in:
Matthias Grob
2022-06-30 16:10:40 +02:00
parent 437148f8f3
commit 25ed6b67fb
2694 changed files with 103014 additions and 118862 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/mavsdk_tests/autopilot_tester.cpp
+200 -245
View File
@@ -32,38 +32,33 @@
****************************************************************************/
#include "autopilot_tester.h"
#include "math_helpers.h"
#include <iostream>
#include <future>
#include <thread>
#include <unistd.h>
#include <cmath>
#include <future>
#include <iostream>
#include <thread>
std::string connection_url {"udp://"};
std::optional<float> speed_factor {std::nullopt};
#include "math_helpers.h"
AutopilotTester::AutopilotTester() :
_real_time_report_thread([this]()
{
report_speed_factor();
})
{
}
std::string connection_url{"udp://"};
std::optional<float> speed_factor{std::nullopt};
AutopilotTester::~AutopilotTester()
{
AutopilotTester::AutopilotTester() : _real_time_report_thread([this]() { report_speed_factor(); }) {}
AutopilotTester::~AutopilotTester() {
_should_exit = true;
_real_time_report_thread.join();
}
void AutopilotTester::connect(const std::string uri)
{
void AutopilotTester::connect(const std::string uri) {
ConnectionResult ret = _mavsdk.add_any_connection(uri);
REQUIRE(ret == ConnectionResult::Success);
std::cout << time_str() << "Waiting for system connect" << std::endl;
REQUIRE(poll_condition_with_timeout(
[this]() { return _mavsdk.systems().size() > 0; }, std::chrono::seconds(25)));
REQUIRE(poll_condition_with_timeout([this]() { return _mavsdk.systems().size() > 0; },
std::chrono::seconds(25)));
auto system = get_system();
@@ -78,11 +73,9 @@ void AutopilotTester::connect(const std::string uri)
_telemetry.reset(new Telemetry(system));
}
void AutopilotTester::wait_until_ready()
{
void AutopilotTester::wait_until_ready() {
std::cout << time_str() << "Waiting for system to be ready" << std::endl;
CHECK(poll_condition_with_timeout(
[this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(30)));
CHECK(poll_condition_with_timeout([this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(30)));
// FIXME: workaround to prevent race between PX4 switching to Hold mode
// and us trying to arm and take off. If PX4 is not in Hold mode yet,
@@ -90,128 +83,112 @@ void AutopilotTester::wait_until_ready()
std::this_thread::sleep_for(std::chrono::seconds(1));
}
void AutopilotTester::wait_until_ready_local_position_only()
{
void AutopilotTester::wait_until_ready_local_position_only() {
std::cout << time_str() << "Waiting for system to be ready" << std::endl;
CHECK(poll_condition_with_timeout(
[this]() {
return
(_telemetry->health().is_gyrometer_calibration_ok &&
_telemetry->health().is_accelerometer_calibration_ok &&
_telemetry->health().is_magnetometer_calibration_ok &&
_telemetry->health().is_local_position_ok);
}, std::chrono::seconds(20)));
[this]() {
return (_telemetry->health().is_gyrometer_calibration_ok &&
_telemetry->health().is_accelerometer_calibration_ok &&
_telemetry->health().is_magnetometer_calibration_ok &&
_telemetry->health().is_local_position_ok);
},
std::chrono::seconds(20)));
}
void AutopilotTester::store_home()
{
void AutopilotTester::store_home() {
request_ground_truth();
std::cout << time_str() << "Waiting to get home position" << std::endl;
CHECK(poll_condition_with_timeout(
[this]() {
_home = _telemetry->ground_truth();
return std::isfinite(_home.latitude_deg) && std::isfinite(_home.longitude_deg);
}, std::chrono::seconds(10)));
[this]() {
_home = _telemetry->ground_truth();
return std::isfinite(_home.latitude_deg) && std::isfinite(_home.longitude_deg);
},
std::chrono::seconds(10)));
}
void AutopilotTester::check_home_within(float acceptance_radius_m)
{
void AutopilotTester::check_home_within(float acceptance_radius_m) {
CHECK(ground_truth_horizontal_position_close_to(_home, acceptance_radius_m));
}
void AutopilotTester::check_home_not_within(float min_distance_m)
{
void AutopilotTester::check_home_not_within(float min_distance_m) {
CHECK(ground_truth_horizontal_position_far_from(_home, min_distance_m));
}
void AutopilotTester::set_takeoff_altitude(const float altitude_m)
{
void AutopilotTester::set_takeoff_altitude(const float 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.second == Approx(altitude_m));
}
void AutopilotTester::set_rtl_altitude(const float altitude_m)
{
void AutopilotTester::set_rtl_altitude(const float altitude_m) {
CHECK(Action::Result::Success == _action->set_return_to_launch_altitude(altitude_m));
const auto result = _action->get_return_to_launch_altitude();
CHECK(result.first == Action::Result::Success);
CHECK(result.second == Approx(altitude_m));
}
void AutopilotTester::set_height_source(AutopilotTester::HeightSource height_source)
{
void AutopilotTester::set_height_source(AutopilotTester::HeightSource height_source) {
switch (height_source) {
case HeightSource::Baro:
CHECK(_param->set_param_int("EKF2_HGT_MODE", 0) == Param::Result::Success);
break;
case HeightSource::Baro:
CHECK(_param->set_param_int("EKF2_HGT_MODE", 0) == Param::Result::Success);
break;
case HeightSource::Gps:
CHECK(_param->set_param_int("EKF2_HGT_MODE", 1) == Param::Result::Success);
case HeightSource::Gps:
CHECK(_param->set_param_int("EKF2_HGT_MODE", 1) == Param::Result::Success);
}
}
void AutopilotTester::set_rc_loss_exception(AutopilotTester::RcLossException mask)
{
void AutopilotTester::set_rc_loss_exception(AutopilotTester::RcLossException mask) {
switch (mask) {
case RcLossException::Mission:
CHECK(_param->set_param_int("COM_RCL_EXCEPT", 1 << 0) == Param::Result::Success);
break;
case RcLossException::Mission:
CHECK(_param->set_param_int("COM_RCL_EXCEPT", 1 << 0) == Param::Result::Success);
break;
case RcLossException::Hold:
CHECK(_param->set_param_int("COM_RCL_EXCEPT", 1 << 1) == Param::Result::Success);
break;
case RcLossException::Hold:
CHECK(_param->set_param_int("COM_RCL_EXCEPT", 1 << 1) == Param::Result::Success);
break;
case RcLossException::Offboard:
CHECK(_param->set_param_int("COM_RCL_EXCEPT", 1 << 2) == Param::Result::Success);
case RcLossException::Offboard:
CHECK(_param->set_param_int("COM_RCL_EXCEPT", 1 << 2) == Param::Result::Success);
}
}
void AutopilotTester::arm()
{
void AutopilotTester::arm() {
const auto result = _action->arm();
REQUIRE(result == Action::Result::Success);
}
void AutopilotTester::takeoff()
{
void AutopilotTester::takeoff() {
const auto result = _action->takeoff();
REQUIRE(result == Action::Result::Success);
}
void AutopilotTester::land()
{
void AutopilotTester::land() {
const auto result = _action->land();
REQUIRE(result == Action::Result::Success);
}
void AutopilotTester::transition_to_fixedwing()
{
void AutopilotTester::transition_to_fixedwing() {
const auto result = _action->transition_to_fixedwing();
REQUIRE(result == Action::Result::Success);
}
void AutopilotTester::transition_to_multicopter()
{
void AutopilotTester::transition_to_multicopter() {
const auto result = _action->transition_to_multicopter();
REQUIRE(result == Action::Result::Success);
}
void AutopilotTester::wait_until_disarmed(std::chrono::seconds timeout_duration)
{
REQUIRE(poll_condition_with_timeout(
[this]() { return !_telemetry->armed(); }, timeout_duration));
void AutopilotTester::wait_until_disarmed(std::chrono::seconds timeout_duration) {
REQUIRE(poll_condition_with_timeout([this]() { return !_telemetry->armed(); }, timeout_duration));
}
void AutopilotTester::wait_until_hovering()
{
void AutopilotTester::wait_until_hovering() {
wait_for_landed_state(Telemetry::LandedState::InAir, std::chrono::seconds(45));
}
void AutopilotTester::wait_until_altitude(float rel_altitude_m, std::chrono::seconds timeout)
{
auto prom = std::promise<void> {};
void AutopilotTester::wait_until_altitude(float rel_altitude_m, std::chrono::seconds timeout) {
auto prom = std::promise<void>{};
auto fut = prom.get_future();
_telemetry->subscribe_position([&prom, rel_altitude_m, this](Telemetry::Position new_position) {
@@ -224,140 +201,143 @@ void AutopilotTester::wait_until_altitude(float rel_altitude_m, std::chrono::sec
REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
}
void AutopilotTester::prepare_square_mission(MissionOptions mission_options)
{
void AutopilotTester::prepare_square_mission(MissionOptions mission_options) {
const auto ct = get_coordinate_transformation();
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::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);
REQUIRE(_mission->upload_mission(mission_plan) == Mission::Result::Success);
}
void AutopilotTester::prepare_straight_mission(MissionOptions mission_options)
{
void AutopilotTester::prepare_straight_mission(MissionOptions mission_options) {
const auto ct = get_coordinate_transformation();
Mission::MissionPlan mission_plan {};
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_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);
REQUIRE(_mission->upload_mission(mission_plan) == Mission::Result::Success);
}
void AutopilotTester::execute_mission()
{
void AutopilotTester::execute_mission() {
std::promise<void> prom;
auto fut = prom.get_future();
REQUIRE(poll_condition_with_timeout(
[this]() { return _mission->start_mission() == Mission::Result::Success; }, std::chrono::seconds(3)));
REQUIRE(poll_condition_with_timeout([this]() { return _mission->start_mission() == Mission::Result::Success; },
std::chrono::seconds(3)));
// TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc.
wait_for_mission_finished(std::chrono::seconds(90));
}
void AutopilotTester::execute_mission_and_lose_gps()
{
void AutopilotTester::execute_mission_and_lose_gps() {
CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
start_and_wait_for_first_mission_item();
CHECK(_failure->inject(Failure::FailureUnit::SensorGps, Failure::FailureType::Off, 0) == Failure::Result::Success);
CHECK(_failure->inject(Failure::FailureUnit::SensorGps, Failure::FailureType::Off, 0) ==
Failure::Result::Success);
// We expect that a blind land is performed.
wait_for_flight_mode(Telemetry::FlightMode::Land, std::chrono::seconds(30));
}
void AutopilotTester::execute_mission_and_lose_mag()
{
void AutopilotTester::execute_mission_and_lose_mag() {
CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
start_and_wait_for_first_mission_item();
CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Off, 0) == Failure::Result::Success);
CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Off, 0) ==
Failure::Result::Success);
// We except the mission to continue without mag just fine.
REQUIRE(poll_condition_with_timeout(
[this]() {
auto progress = _mission->mission_progress();
return progress.current == progress.total;
}, std::chrono::seconds(90)));
[this]() {
auto progress = _mission->mission_progress();
return progress.current == progress.total;
},
std::chrono::seconds(90)));
}
void AutopilotTester::execute_mission_and_lose_baro()
{
void AutopilotTester::execute_mission_and_lose_baro() {
CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
start_and_wait_for_first_mission_item();
CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Off, 0) == Failure::Result::Success);
CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Off, 0) ==
Failure::Result::Success);
// We except the mission to continue without baro just fine.
REQUIRE(poll_condition_with_timeout(
[this]() {
auto progress = _mission->mission_progress();
return progress.current == progress.total;
}, std::chrono::seconds(90)));
[this]() {
auto progress = _mission->mission_progress();
return progress.current == progress.total;
},
std::chrono::seconds(90)));
}
void AutopilotTester::execute_mission_and_get_baro_stuck()
{
void AutopilotTester::execute_mission_and_get_baro_stuck() {
CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
start_and_wait_for_first_mission_item();
CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Stuck, 0) == Failure::Result::Success);
CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Stuck, 0) ==
Failure::Result::Success);
// We except the mission to continue with a stuck baro just fine.
REQUIRE(poll_condition_with_timeout(
[this]() {
auto progress = _mission->mission_progress();
return progress.current == progress.total;
}, std::chrono::seconds(90)));
[this]() {
auto progress = _mission->mission_progress();
return progress.current == progress.total;
},
std::chrono::seconds(90)));
}
void AutopilotTester::execute_mission_and_get_mag_stuck()
{
void AutopilotTester::execute_mission_and_get_mag_stuck() {
CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
start_and_wait_for_first_mission_item();
CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Stuck, 0) == Failure::Result::Success);
CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Stuck, 0) ==
Failure::Result::Success);
// We except the mission to continue with a stuck mag just fine.
REQUIRE(poll_condition_with_timeout(
[this]() {
auto progress = _mission->mission_progress();
return progress.current == progress.total;
}, std::chrono::seconds(120)));
[this]() {
auto progress = _mission->mission_progress();
return progress.current == progress.total;
},
std::chrono::seconds(120)));
}
CoordinateTransformation AutopilotTester::get_coordinate_transformation()
{
CoordinateTransformation AutopilotTester::get_coordinate_transformation() {
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});
}
Mission::MissionItem AutopilotTester::create_mission_item(
const CoordinateTransformation::LocalCoordinate &local_coordinate,
const MissionOptions &mission_options,
const CoordinateTransformation &ct)
{
Mission::MissionItem AutopilotTester::create_mission_item(
const CoordinateTransformation::LocalCoordinate &local_coordinate, const MissionOptions &mission_options,
const CoordinateTransformation &ct) {
auto mission_item = Mission::MissionItem{};
const auto pos_north = ct.global_from_local(local_coordinate);
mission_item.latitude_deg = pos_north.latitude_deg;
@@ -367,8 +347,7 @@ Mission::MissionItem AutopilotTester::create_mission_item(
return mission_item;
}
void AutopilotTester::load_qgc_mission_raw_and_move_here(const std::string &plan_file)
{
void AutopilotTester::load_qgc_mission_raw_and_move_here(const std::string &plan_file) {
auto import_result = _mission_raw->import_qgroundcontrol_mission(plan_file);
REQUIRE(import_result.first == MissionRaw::Result::Success);
@@ -377,8 +356,7 @@ void AutopilotTester::load_qgc_mission_raw_and_move_here(const std::string &plan
REQUIRE(_mission_raw->upload_mission(import_result.second.mission_items) == MissionRaw::Result::Success);
}
void AutopilotTester::execute_mission_raw()
{
void AutopilotTester::execute_mission_raw() {
REQUIRE(_mission->start_mission() == Mission::Result::Success);
// TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc.
@@ -386,29 +364,20 @@ void AutopilotTester::execute_mission_raw()
wait_for_mission_raw_finished(std::chrono::seconds(120));
}
void AutopilotTester::execute_rtl()
{
REQUIRE(Action::Result::Success == _action->return_to_launch());
}
void AutopilotTester::execute_rtl() { REQUIRE(Action::Result::Success == _action->return_to_launch()); }
void AutopilotTester::execute_land()
{
REQUIRE(Action::Result::Success == _action->land());
}
void AutopilotTester::execute_land() { REQUIRE(Action::Result::Success == _action->land()); }
void AutopilotTester::offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m,
std::chrono::seconds timeout_duration)
{
std::chrono::seconds timeout_duration) {
_offboard->set_position_ned(target);
REQUIRE(_offboard->start() == Offboard::Result::Success);
CHECK(poll_condition_with_timeout(
[ = ]() { return estimated_position_close_to(target, acceptance_radius_m); }, timeout_duration));
CHECK(poll_condition_with_timeout([=]() { return estimated_position_close_to(target, acceptance_radius_m); },
timeout_duration));
std::cout << time_str() << "Target position reached" << std::endl;
}
void AutopilotTester::check_mission_item_speed_above(int item_index, float min_speed_m_s)
{
void AutopilotTester::check_mission_item_speed_above(int item_index, float min_speed_m_s) {
_telemetry->set_rate_velocity_ned(10);
_telemetry->subscribe_velocity_ned([item_index, min_speed_m_s, this](Telemetry::VelocityNed velocity) {
float horizontal = std::hypot(velocity.north_m_s, velocity.east_m_s);
@@ -420,8 +389,7 @@ void AutopilotTester::check_mission_item_speed_above(int item_index, float min_s
});
}
void AutopilotTester::fly_forward_in_posctl()
{
void AutopilotTester::fly_forward_in_posctl() {
const unsigned manual_control_rate_hz = 50;
// Send something to make sure RC is available.
@@ -440,7 +408,8 @@ void AutopilotTester::fly_forward_in_posctl()
// Fly forward for 60 seconds
for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) {
CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) ==
ManualControl::Result::Success);
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
}
@@ -455,8 +424,7 @@ void AutopilotTester::fly_forward_in_posctl()
}
}
void AutopilotTester::fly_forward_in_altctl()
{
void AutopilotTester::fly_forward_in_altctl() {
const unsigned manual_control_rate_hz = 50;
// Send something to make sure RC is available.
@@ -475,7 +443,8 @@ void AutopilotTester::fly_forward_in_altctl()
// Fly forward for 60 seconds
for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) {
CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) ==
ManualControl::Result::Success);
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
}
@@ -490,8 +459,7 @@ void AutopilotTester::fly_forward_in_altctl()
}
}
void AutopilotTester::start_checking_altitude(const float max_deviation_m)
{
void AutopilotTester::start_checking_altitude(const float max_deviation_m) {
std::array<float, 3> initial_position = get_current_position_ned();
float target_altitude = initial_position[2];
@@ -501,13 +469,9 @@ void AutopilotTester::start_checking_altitude(const float max_deviation_m)
});
}
void AutopilotTester::stop_checking_altitude()
{
_telemetry->subscribe_position(nullptr);
}
void AutopilotTester::stop_checking_altitude() { _telemetry->subscribe_position(nullptr); }
void AutopilotTester::check_tracks_mission(float corridor_radius_m)
{
void AutopilotTester::check_tracks_mission(float corridor_radius_m) {
auto mission = _mission->download_mission();
CHECK(mission.first == Mission::Result::Success);
@@ -515,40 +479,41 @@ void AutopilotTester::check_tracks_mission(float corridor_radius_m)
auto ct = get_coordinate_transformation();
_telemetry->set_rate_position_velocity_ned(5);
_telemetry->subscribe_position_velocity_ned([ct, mission_items, corridor_radius_m,
this](Telemetry::PositionVelocityNed position_velocity_ned) {
auto progress = _mission->mission_progress();
_telemetry->subscribe_position_velocity_ned(
[ct, mission_items, corridor_radius_m, this](Telemetry::PositionVelocityNed position_velocity_ned) {
auto progress = _mission->mission_progress();
if (progress.current > 0 && progress.current < progress.total) {
// Get shortest distance of current position to 3D line between previous and next waypoint
if (progress.current > 0 && progress.current < progress.total) {
// Get shortest distance of current position to 3D line between previous and next
// waypoint
std::array<float, 3> current { position_velocity_ned.position.north_m,
position_velocity_ned.position.east_m,
position_velocity_ned.position.down_m };
std::array<float, 3> wp_prev = get_local_mission_item<float>(mission_items[progress.current - 1], ct);
std::array<float, 3> wp_next = get_local_mission_item<float>(mission_items[progress.current], ct);
std::array<float, 3> current{position_velocity_ned.position.north_m,
position_velocity_ned.position.east_m,
position_velocity_ned.position.down_m};
std::array<float, 3> wp_prev =
get_local_mission_item<float>(mission_items[progress.current - 1], ct);
std::array<float, 3> wp_next =
get_local_mission_item<float>(mission_items[progress.current], ct);
float distance_to_trajectory = point_to_line_distance(current, wp_prev, wp_next);
float distance_to_trajectory = point_to_line_distance(current, wp_prev, wp_next);
CHECK(distance_to_trajectory < corridor_radius_m);
}
});
CHECK(distance_to_trajectory < corridor_radius_m);
}
});
}
void AutopilotTester::check_current_altitude(float target_rel_altitude_m, float max_distance_m)
{
void AutopilotTester::check_current_altitude(float target_rel_altitude_m, float max_distance_m) {
CHECK(std::abs(_telemetry->position().relative_altitude_m - target_rel_altitude_m) <= max_distance_m);
}
std::array<float, 3> AutopilotTester::get_current_position_ned()
{
std::array<float, 3> AutopilotTester::get_current_position_ned() {
mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned();
std::array<float, 3> position_ned{position_velocity_ned.position.north_m, position_velocity_ned.position.east_m, position_velocity_ned.position.down_m};
std::array<float, 3> position_ned{position_velocity_ned.position.north_m, position_velocity_ned.position.east_m,
position_velocity_ned.position.down_m};
return position_ned;
}
void AutopilotTester::offboard_land()
{
void AutopilotTester::offboard_land() {
Offboard::VelocityNedYaw land_velocity;
land_velocity.north_m_s = 0.0f;
land_velocity.east_m_s = 0.0f;
@@ -557,37 +522,35 @@ 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;
const float distance_m = std::sqrt(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));
const float distance_m =
std::sqrt(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));
const bool pass = distance_m < acceptance_radius_m;
if (!pass) {
std::cout << time_str() << "distance: " << distance_m << ", " << "acceptance: " << acceptance_radius_m << std::endl;
std::cout << time_str() << "distance: " << distance_m << ", "
<< "acceptance: " << acceptance_radius_m << std::endl;
}
return pass;
return pass;
}
bool AutopilotTester::estimated_horizontal_position_close_to(const Offboard::PositionNedYaw &target_pos,
float acceptance_radius_m)
{
float acceptance_radius_m) {
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);
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()
{
void AutopilotTester::request_ground_truth() {
CHECK(_telemetry->set_rate_ground_truth(15) == Telemetry::Result::Success);
}
bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos,
float acceptance_radius_m)
{
float acceptance_radius_m) {
CHECK(std::isfinite(target_pos.latitude_deg));
CHECK(std::isfinite(target_pos.longitude_deg));
using GlobalCoordinate = CoordinateTransformation::GlobalCoordinate;
@@ -617,8 +580,7 @@ bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry:
}
bool AutopilotTester::ground_truth_horizontal_position_far_from(const Telemetry::GroundTruth &target_pos,
float min_distance_m)
{
float min_distance_m) {
CHECK(std::isfinite(target_pos.latitude_deg));
CHECK(std::isfinite(target_pos.longitude_deg));
using GlobalCoordinate = CoordinateTransformation::GlobalCoordinate;
@@ -647,9 +609,8 @@ bool AutopilotTester::ground_truth_horizontal_position_far_from(const Telemetry:
return pass;
}
void AutopilotTester::start_and_wait_for_first_mission_item()
{
auto prom = std::promise<void> {};
void AutopilotTester::start_and_wait_for_first_mission_item() {
auto prom = std::promise<void>{};
auto fut = prom.get_future();
_mission->subscribe_mission_progress([&prom, this](Mission::MissionProgress progress) {
@@ -666,9 +627,8 @@ void AutopilotTester::start_and_wait_for_first_mission_item()
REQUIRE(fut.wait_for(std::chrono::seconds(60)) == std::future_status::ready);
}
void AutopilotTester::wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout)
{
auto prom = std::promise<void> {};
void AutopilotTester::wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout) {
auto prom = std::promise<void>{};
auto fut = prom.get_future();
_telemetry->subscribe_flight_mode([&prom, flight_mode, this](Telemetry::FlightMode new_flight_mode) {
@@ -681,9 +641,8 @@ void AutopilotTester::wait_for_flight_mode(Telemetry::FlightMode flight_mode, st
REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
}
void AutopilotTester::wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout)
{
auto prom = std::promise<void> {};
void AutopilotTester::wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout) {
auto prom = std::promise<void>{};
auto fut = prom.get_future();
_telemetry->subscribe_landed_state([&prom, landed_state, this](Telemetry::LandedState new_landed_state) {
@@ -696,30 +655,29 @@ void AutopilotTester::wait_for_landed_state(Telemetry::LandedState landed_state,
REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
}
void AutopilotTester::wait_until_speed_lower_than(float speed, std::chrono::seconds timeout)
{
auto prom = std::promise<void> {};
void AutopilotTester::wait_until_speed_lower_than(float speed, std::chrono::seconds timeout) {
auto prom = std::promise<void>{};
auto fut = prom.get_future();
_telemetry->subscribe_position_velocity_ned([&prom, speed, this](Telemetry::PositionVelocityNed position_velocity_ned) {
std::array<float, 3> current_velocity;
current_velocity[0] = position_velocity_ned.velocity.north_m_s;
current_velocity[1] = position_velocity_ned.velocity.east_m_s;
current_velocity[2] = position_velocity_ned.velocity.down_m_s;
const float current_speed = norm(current_velocity);
_telemetry->subscribe_position_velocity_ned(
[&prom, speed, this](Telemetry::PositionVelocityNed position_velocity_ned) {
std::array<float, 3> current_velocity;
current_velocity[0] = position_velocity_ned.velocity.north_m_s;
current_velocity[1] = position_velocity_ned.velocity.east_m_s;
current_velocity[2] = position_velocity_ned.velocity.down_m_s;
const float current_speed = norm(current_velocity);
if (current_speed <= speed) {
_telemetry->subscribe_position_velocity_ned(nullptr);
prom.set_value();
}
});
if (current_speed <= speed) {
_telemetry->subscribe_position_velocity_ned(nullptr);
prom.set_value();
}
});
REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
}
void AutopilotTester::wait_for_mission_finished(std::chrono::seconds timeout)
{
auto prom = std::promise<void> {};
void AutopilotTester::wait_for_mission_finished(std::chrono::seconds timeout) {
auto prom = std::promise<void>{};
auto fut = prom.get_future();
_mission->subscribe_mission_progress([&prom, this](Mission::MissionProgress progress) {
@@ -732,9 +690,8 @@ void AutopilotTester::wait_for_mission_finished(std::chrono::seconds timeout)
REQUIRE(fut.wait_for(timeout) == std::future_status::ready);
}
void AutopilotTester::wait_for_mission_raw_finished(std::chrono::seconds timeout)
{
auto prom = std::promise<void> {};
void AutopilotTester::wait_for_mission_raw_finished(std::chrono::seconds timeout) {
auto prom = std::promise<void>{};
auto fut = prom.get_future();
_mission_raw->subscribe_mission_progress([&prom, this](MissionRaw::MissionProgress progress) {
@@ -747,8 +704,7 @@ 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)
{
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));
@@ -757,7 +713,7 @@ void AutopilotTester::move_mission_raw_here(std::vector<MissionRaw::MissionItem>
auto offset_y = mission_items[0].y - static_cast<int32_t>(1e7 * position.longitude_deg);
for (auto &item : mission_items) {
if (item.frame == 3) { // MAV_FRAME_GLOBAL_RELATIVE_ALT
if (item.frame == 3) { // MAV_FRAME_GLOBAL_RELATIVE_ALT
item.x -= offset_x;
}
@@ -765,15 +721,14 @@ void AutopilotTester::move_mission_raw_here(std::vector<MissionRaw::MissionItem>
}
}
void AutopilotTester::report_speed_factor()
{
void AutopilotTester::report_speed_factor() {
// We check the exit flag more often than the speed factor.
unsigned counter = 0;
while (!_should_exit) {
if (counter++ % 10 == 0) {
if (_info != nullptr) {
std::cout << "Current speed factor: " << _info->get_speed_factor().second ;
std::cout << "Current speed factor: " << _info->get_speed_factor().second;
if (speed_factor.has_value()) {
std::cout << " (set: " << speed_factor.value() << ')';
test/mavsdk_tests/autopilot_tester.h
+46 -57
View File
@@ -33,8 +33,8 @@
#pragma once
#include <mavsdk/mavsdk.h>
#include <mavsdk/geometry.h>
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/failure/failure.h>
#include <mavsdk/plugins/info/info.h>
@@ -42,9 +42,9 @@
#include <mavsdk/plugins/mission/mission.h>
#include <mavsdk/plugins/mission_raw/mission_raw.h>
#include <mavsdk/plugins/offboard/offboard.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <mavsdk/plugins/param/param.h>
#include "catch2/catch.hpp"
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <atomic>
#include <chrono>
#include <ctime>
@@ -53,15 +53,15 @@
#include <optional>
#include <thread>
#include "catch2/catch.hpp"
extern std::string connection_url;
extern std::optional<float> speed_factor;
using namespace mavsdk;
using namespace mavsdk::geometry;
inline std::string time_str()
{
inline std::string time_str() {
time_t rawtime;
time(&rawtime);
struct tm *timeinfo = localtime(&rawtime);
@@ -70,26 +70,18 @@ inline std::string time_str()
return time_buffer;
}
class AutopilotTester
{
class AutopilotTester {
public:
struct MissionOptions {
double leg_length_m {20.0};
double relative_altitude_m {10.0};
bool rtl_at_end {false};
bool fly_through {false};
double leg_length_m{20.0};
double relative_altitude_m{10.0};
bool rtl_at_end{false};
bool fly_through{false};
};
enum class HeightSource {
Baro,
Gps
};
enum class HeightSource { Baro, Gps };
enum class RcLossException {
Mission = 0,
Hold = 1,
Offboard = 2
};
enum class RcLossException { Mission = 0, Hold = 1, Offboard = 2 };
AutopilotTester();
~AutopilotTester();
@@ -110,7 +102,8 @@ public:
void transition_to_fixedwing();
void transition_to_multicopter();
void wait_until_disarmed(std::chrono::seconds timeout_duration = std::chrono::seconds(60));
void wait_until_hovering(); // TODO: name suggests, that function waits for drone velocity to be zero and not just drone in the air
void wait_until_hovering(); // TODO: name suggests, that function waits for drone velocity to be zero and not
// just drone in the air
void wait_until_altitude(float rel_altitude_m, std::chrono::seconds timeout);
void wait_until_speed_lower_than(float speed, std::chrono::seconds timeout);
void prepare_square_mission(MissionOptions mission_options);
@@ -140,18 +133,16 @@ public:
// Blocking call to get the drone's current position in NED frame
std::array<float, 3> get_current_position_ned();
void set_param_int(const std::string &param, int32_t value)
{
void set_param_int(const std::string &param, int32_t value) {
CHECK(_param->set_param_int(param, value) == Param::Result::Success);
}
protected:
mavsdk::Param *getParams() const { return _param.get();}
mavsdk::Telemetry *getTelemetry() const { return _telemetry.get();}
mavsdk::ManualControl *getManualControl() const { return _manual_control.get();}
std::shared_ptr<System> get_system() { return _mavsdk.systems().at(0);}
Telemetry::GroundTruth getHome()
{
mavsdk::Param *getParams() const { return _param.get(); }
mavsdk::Telemetry *getTelemetry() const { return _telemetry.get(); }
mavsdk::ManualControl *getManualControl() const { return _manual_control.get(); }
std::shared_ptr<System> get_system() { return _mavsdk.systems().at(0); }
Telemetry::GroundTruth getHome() {
// Check if home was stored before it is accessed
CHECK(_home.absolute_altitude_m != NAN);
CHECK(_home.latitude_deg != NAN);
@@ -159,20 +150,19 @@ protected:
return _home;
}
template<typename Rep, typename Period>
void sleep_for(std::chrono::duration<Rep, Period> duration)
{
template <typename Rep, typename Period>
void sleep_for(std::chrono::duration<Rep, Period> duration) {
const std::chrono::microseconds duration_us(duration);
if (_telemetry && _telemetry->attitude_quaternion().timestamp_us != 0) {
const int64_t start_time_us = _telemetry->attitude_quaternion().timestamp_us;
while (true) {
// Hopefully this is often enough not to have PX4 time out on us.
std::this_thread::sleep_for(std::chrono::milliseconds(1));
const int64_t elapsed_time_us = _telemetry->attitude_quaternion().timestamp_us - start_time_us;
const int64_t elapsed_time_us =
_telemetry->attitude_quaternion().timestamp_us - start_time_us;
if (elapsed_time_us > duration_us.count()) {
return;
@@ -188,13 +178,14 @@ private:
mavsdk::geometry::CoordinateTransformation get_coordinate_transformation();
mavsdk::Mission::MissionItem create_mission_item(
const mavsdk::geometry::CoordinateTransformation::LocalCoordinate &local_coordinate,
const MissionOptions &mission_options,
const mavsdk::geometry::CoordinateTransformation &ct);
const MissionOptions &mission_options, const mavsdk::geometry::CoordinateTransformation &ct);
bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, float acceptance_radius_m);
bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos,
float acceptance_radius_m);
bool ground_truth_horizontal_position_far_from(const Telemetry::GroundTruth &target_pos, float min_distance_m);
bool estimated_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m);
bool estimated_horizontal_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m);
bool estimated_horizontal_position_close_to(const Offboard::PositionNedYaw &target_pos,
float acceptance_radius_m);
void start_and_wait_for_first_mission_item();
void wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout);
void wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout);
@@ -204,10 +195,8 @@ private:
void report_speed_factor();
template<typename Rep, typename Period>
bool poll_condition_with_timeout(
std::function<bool()> fun, std::chrono::duration<Rep, Period> duration)
{
template <typename Rep, typename Period>
bool poll_condition_with_timeout(std::function<bool()> fun, std::chrono::duration<Rep, Period> duration) {
static constexpr unsigned check_resolution = 100;
const std::chrono::microseconds duration_us(duration);
@@ -219,14 +208,16 @@ private:
while (!fun()) {
std::this_thread::sleep_for(duration_us / check_resolution);
// This might potentially loop forever and the test needs to be killed by a watchdog outside.
// The reason not to include an absolute timeout here is that it can happen if the host is
// busy and PX4 doesn't run fast enough.
const int64_t elapsed_time_us = _telemetry->attitude_quaternion().timestamp_us - start_time_us;
// This might potentially loop forever and the test needs to be killed by a watchdog
// outside. The reason not to include an absolute timeout here is that it can happen if
// the host is busy and PX4 doesn't run fast enough.
const int64_t elapsed_time_us =
_telemetry->attitude_quaternion().timestamp_us - start_time_us;
if (elapsed_time_us > duration_us.count()) {
std::cout << time_str() << "Timeout, connected to vehicle but waiting for test for " << static_cast<double>
(elapsed_time_us) / 1e6 << " seconds\n";
std::cout << time_str()
<< "Timeout, connected to vehicle but waiting for test for "
<< static_cast<double>(elapsed_time_us) / 1e6 << " seconds\n";
return false;
}
}
@@ -237,13 +228,13 @@ private:
while (!fun()) {
std::this_thread::sleep_for(duration_us / check_resolution);
const auto elapsed_time_us = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() -
start_time);
const auto elapsed_time_us = std::chrono::duration_cast<std::chrono::microseconds>(
std::chrono::steady_clock::now() - start_time);
if (elapsed_time_us > duration_us) {
std::cout << time_str() << "Timeout, waiting for the vehicle for "
<< elapsed_time_us.count() * std::chrono::steady_clock::period::num
/ static_cast<double>(std::chrono::steady_clock::period::den)
<< elapsed_time_us.count() * std::chrono::steady_clock::period::num /
static_cast<double>(std::chrono::steady_clock::period::den)
<< " seconds\n";
return false;
}
@@ -253,8 +244,6 @@ private:
return true;
}
mavsdk::Mavsdk _mavsdk{};
std::unique_ptr<mavsdk::Action> _action{};
std::unique_ptr<mavsdk::Failure> _failure{};
@@ -268,6 +257,6 @@ private:
Telemetry::GroundTruth _home{NAN, NAN, NAN};
std::atomic<bool> _should_exit {false};
std::thread _real_time_report_thread {};
std::atomic<bool> _should_exit{false};
std::thread _real_time_report_thread{};
};
test/mavsdk_tests/autopilot_tester_failure.cpp
+16 -23
View File
@@ -33,59 +33,52 @@
#include "autopilot_tester_failure.h"
#include "math_helpers.h"
#include <iostream>
#include <future>
#include <thread>
#include <unistd.h>
#include <future>
#include <iostream>
#include <thread>
void AutopilotTesterFailure::connect(const std::string uri)
{
#include "math_helpers.h"
void AutopilotTesterFailure::connect(const std::string uri) {
AutopilotTester::connect(uri);
auto system = get_system();
_failure.reset(new Failure(system));
}
void AutopilotTesterFailure::set_param_sys_failure_en(bool value)
{
void AutopilotTesterFailure::set_param_sys_failure_en(bool value) {
CHECK(getParams()->set_param_int("SYS_FAILURE_EN", value) == Param::Result::Success);
}
void AutopilotTesterFailure::set_param_fd_act_en(bool value)
{
void AutopilotTesterFailure::set_param_fd_act_en(bool value) {
CHECK(getParams()->set_param_int("FD_ACT_EN", value) == Param::Result::Success);
}
void AutopilotTesterFailure::set_param_mc_airmode(int value)
{
void AutopilotTesterFailure::set_param_mc_airmode(int value) {
CHECK(getParams()->set_param_int("MC_AIRMODE", value) == Param::Result::Success);
}
void AutopilotTesterFailure::set_param_ca_failure_mode(int value)
{
void AutopilotTesterFailure::set_param_ca_failure_mode(int value) {
CHECK(getParams()->set_param_int("CA_FAILURE_MODE", value) == Param::Result::Success);
}
void AutopilotTesterFailure::set_param_com_act_fail_act(int value)
{
void AutopilotTesterFailure::set_param_com_act_fail_act(int value) {
CHECK(getParams()->set_param_int("COM_ACT_FAIL_ACT", value) == Param::Result::Success);
}
void AutopilotTesterFailure::inject_failure(mavsdk::Failure::FailureUnit failure_unit,
mavsdk::Failure::FailureType failure_type, int instance, mavsdk::Failure::Result expected_result)
{
mavsdk::Failure::FailureType failure_type, int instance,
mavsdk::Failure::Result expected_result) {
CHECK(_failure->inject(failure_unit, failure_type, instance) == expected_result);
}
void AutopilotTesterFailure::enable_actuator_output_status()
{
void AutopilotTesterFailure::enable_actuator_output_status() {
CHECK(getTelemetry()->set_rate_actuator_output_status(20.f) == Telemetry::Result::Success);
}
void AutopilotTesterFailure::ensure_motor_stopped(unsigned index, unsigned num_motors)
{
void AutopilotTesterFailure::ensure_motor_stopped(unsigned index, unsigned num_motors) {
const Telemetry::ActuatorOutputStatus &status = getTelemetry()->actuator_output_status();
CHECK(status.active >= num_motors);
@@ -93,7 +86,7 @@ void AutopilotTesterFailure::ensure_motor_stopped(unsigned index, unsigned num_m
if (i == index) {
CHECK(status.actuator[i] <= 901.f);
} else { // ensure all others are still running
} else { // ensure all others are still running
CHECK(status.actuator[i] >= 999.f);
}
}
test/mavsdk_tests/autopilot_tester_failure.h
+4 -6
View File
@@ -33,15 +33,13 @@
#pragma once
#include "autopilot_tester.h"
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/failure/failure.h>
#include "autopilot_tester.h"
class AutopilotTesterFailure : public AutopilotTester
{
class AutopilotTesterFailure : public AutopilotTester {
public:
AutopilotTesterFailure() = default;
~AutopilotTesterFailure() = default;
@@ -55,8 +53,8 @@ public:
void enable_actuator_output_status();
void ensure_motor_stopped(unsigned index, unsigned num_motors);
void inject_failure(mavsdk::Failure::FailureUnit failure_unit, mavsdk::Failure::FailureType failure_type, int instance,
mavsdk::Failure::Result expected_result);
void inject_failure(mavsdk::Failure::FailureUnit failure_unit, mavsdk::Failure::FailureType failure_type,
int instance, mavsdk::Failure::Result expected_result);
void connect(const std::string uri);
private:
test/mavsdk_tests/autopilot_tester_follow_me.cpp
+75 -90
View File
@@ -35,32 +35,27 @@
// #include <mavsdk/plugins/follow_me/follow_me.h>
#include <unistd.h>
#include <cmath>
#include <future>
#include <iostream>
#include <random>
#include <thread>
#include "math_helpers.h"
#include <iostream>
#include <future>
#include <thread>
#include <unistd.h>
#include <cmath>
#include <random>
FollowTargetSimulator::FollowTargetSimulator(std::array<float, 3> initial_position_ned,
mavsdk::Telemetry::GroundTruth home) :
_position_ned(initial_position_ned), _home(home)
{
mavsdk::Telemetry::GroundTruth home)
: _position_ned(initial_position_ned), _home(home) {
_velocity_ned[0] = 0.0f;
_velocity_ned[1] = 0.0f;
_velocity_ned[2] = 0.0f;
}
FollowTargetSimulator::~FollowTargetSimulator()
{
FollowTargetSimulator::~FollowTargetSimulator() {}
}
void FollowTargetSimulator::update(float delta_t_s)
{
void FollowTargetSimulator::update(float delta_t_s) {
const float velocity_m_s = 2.0;
_velocity_ned[0] = velocity_m_s;
@@ -73,8 +68,7 @@ void FollowTargetSimulator::update(float delta_t_s)
_udpate_count++;
}
std::array<double, 3> FollowTargetSimulator::get_position_global(bool add_noise)
{
std::array<double, 3> FollowTargetSimulator::get_position_global(bool add_noise) {
std::array<float, 3> pos_ned = _position_ned;
if (add_noise) {
@@ -91,14 +85,14 @@ std::array<double, 3> FollowTargetSimulator::get_position_global(bool add_noise)
const auto ct = CoordinateTransformation({_home.latitude_deg, _home.longitude_deg});
mavsdk::geometry::CoordinateTransformation::LocalCoordinate local_coordinate{pos_ned[0], pos_ned[1]};
mavsdk::geometry::CoordinateTransformation::GlobalCoordinate global_coordinate = ct.global_from_local(local_coordinate);
std::array<double, 3> global_pos{global_coordinate.latitude_deg, global_coordinate.longitude_deg, pos_ned[2] + _home.absolute_altitude_m};
mavsdk::geometry::CoordinateTransformation::GlobalCoordinate global_coordinate =
ct.global_from_local(local_coordinate);
std::array<double, 3> global_pos{global_coordinate.latitude_deg, global_coordinate.longitude_deg,
pos_ned[2] + _home.absolute_altitude_m};
return global_pos;
}
std::array<float, 3> FollowTargetSimulator::get_position_ned(bool add_noise)
{
std::array<float, 3> FollowTargetSimulator::get_position_ned(bool add_noise) {
std::array<float, 3> pos_ned = _position_ned;
if (add_noise) {
@@ -113,18 +107,11 @@ std::array<float, 3> FollowTargetSimulator::get_position_ned(bool add_noise)
return pos_ned;
}
std::array<float, 3> FollowTargetSimulator::get_velocity_ned_noisy()
{
return get_velocity_ned(true);
}
std::array<float, 3> FollowTargetSimulator::get_velocity_ned_noisy() { return get_velocity_ned(true); }
std::array<float, 3> FollowTargetSimulator::get_velocity_ned_ground_truth()
{
return get_velocity_ned(false);
}
std::array<float, 3> FollowTargetSimulator::get_velocity_ned_ground_truth() { return get_velocity_ned(false); }
std::array<float, 3> FollowTargetSimulator::get_velocity_ned(bool add_noise)
{
std::array<float, 3> FollowTargetSimulator::get_velocity_ned(bool add_noise) {
std::array<float, 3> vel_ned = _velocity_ned;
if (add_noise) {
@@ -140,58 +127,45 @@ std::array<float, 3> FollowTargetSimulator::get_velocity_ned(bool add_noise)
return vel_ned;
}
std::array<float, 3> FollowTargetSimulator::get_position_ned_noisy()
{
return get_position_ned(true);
}
std::array<float, 3> FollowTargetSimulator::get_position_ned_noisy() { return get_position_ned(true); }
std::array<float, 3> FollowTargetSimulator::get_position_ground_truth_ned()
{
return get_position_ned(false);
}
std::array<float, 3> FollowTargetSimulator::get_position_ground_truth_ned() { return get_position_ned(false); }
std::array<double, 3> FollowTargetSimulator::get_position_global_noisy()
{
return get_position_global(true);
}
std::array<double, 3> FollowTargetSimulator::get_position_global_noisy() { return get_position_global(true); }
std::array<double, 3> FollowTargetSimulator::get_position_global_ground_truth()
{
return get_position_global(false);
}
std::array<double, 3> FollowTargetSimulator::get_position_global_ground_truth() { return get_position_global(false); }
void FollowTargetSimulator::check_follow_angle(FollowMe::Config config, std::array<float, 3> drone_pos_ned,
std::array<float, 3> target_pos_ned, float tolerance)
{
std::array<float, 3> target_pos_ned, float tolerance) {
// This check assumes that the target is travelling straight on the x-axis
const float target_to_drone_offset_x = drone_pos_ned[0] - target_pos_ned[0];
const float target_to_drone_offset_y = drone_pos_ned[1] - target_pos_ned[1];
// Follow Angle is measured relative from the target's course (direction it is moving towards)
const float target_to_drone_angle_expected_rad = config.follow_angle_deg * (M_PI / 180.0f);
const float target_to_drone_offset_x_expected = config.follow_distance_m * cos(target_to_drone_angle_expected_rad);
const float target_to_drone_offset_y_expected = config.follow_distance_m * sin(target_to_drone_angle_expected_rad);
const float target_to_drone_offset_x_expected =
config.follow_distance_m * cos(target_to_drone_angle_expected_rad);
const float target_to_drone_offset_y_expected =
config.follow_distance_m * sin(target_to_drone_angle_expected_rad);
// Check that drone is following at an expected position within the tolerance error
CHECK(fabsf(target_to_drone_offset_x - target_to_drone_offset_x_expected) < tolerance);
CHECK(fabsf(target_to_drone_offset_y - target_to_drone_offset_y_expected) < tolerance);
}
void AutopilotTesterFollowMe::connect(const std::string uri)
{
void AutopilotTesterFollowMe::connect(const std::string uri) {
AutopilotTester::connect(uri);
auto system = get_system();
_follow_me.reset(new FollowMe(system));
}
void AutopilotTesterFollowMe::straight_line_test(const bool stream_velocity)
{
void AutopilotTesterFollowMe::straight_line_test(const bool stream_velocity) {
// CONFIGURATION for the test
const unsigned location_update_rate = 1; // [Hz] How often the GPS location update samples are generated
const float position_error_tolerance = 11.0f; // [m] Position error tolerance in both X and Y direction
const float follow_me_height_setting = 10.0f; // [m] Height above home position where the Drone will follow from
const unsigned location_update_rate = 1; // [Hz] How often the GPS location update samples are generated
const float position_error_tolerance = 11.0f; // [m] Position error tolerance in both X and Y direction
const float follow_me_height_setting =
10.0f; // [m] Height above home position where the Drone will follow from
// Start with simulated target on the same plane as drone's home position
std::array<float, 3> start_location_ned = get_current_position_ned();
@@ -209,7 +183,7 @@ void AutopilotTesterFollowMe::straight_line_test(const bool stream_velocity)
sleep_for(std::chrono::milliseconds(1000));
// Start Follow Me
CHECK(FollowMe::Result::Success == _follow_me->start());
CHECK(FollowMe::Result::Success == _follow_me->start());
// Allow some time for mode switch
sleep_for(std::chrono::milliseconds(1000));
@@ -290,7 +264,8 @@ void AutopilotTesterFollowMe::straight_line_test(const bool stream_velocity)
check_current_altitude(follow_me_height_setting);
CHECK(distance_to_target <= config.follow_distance_m + position_error_tolerance);
CHECK(distance_to_target >= config.follow_distance_m - position_error_tolerance);
target_simulator.check_follow_angle(config, position_ned, target_pos_ned_ground_truth, position_error_tolerance);
target_simulator.check_follow_angle(config, position_ned, target_pos_ned_ground_truth,
position_error_tolerance);
}
// Construct follow-me message
@@ -313,31 +288,29 @@ void AutopilotTesterFollowMe::straight_line_test(const bool stream_velocity)
target_location.velocity_z_m_s = NAN;
}
// Send message and check result
CHECK(FollowMe::Result::Success == _follow_me->set_target_location(target_location));
sleep_for(std::chrono::milliseconds(1000 / location_update_rate));
}
CHECK(FollowMe::Result::Success == _follow_me->stop());
CHECK(FollowMe::Result::Success == _follow_me->stop());
}
void AutopilotTesterFollowMe::stream_velocity_only()
{
void AutopilotTesterFollowMe::stream_velocity_only() {
const unsigned loop_update_rate = 1;
const float position_tolerance = 4.0f;
// Configure follow-me
FollowMe::Config config;
config.follow_angle_deg = 180.0f; // Follow from behind
config.follow_angle_deg = 180.0f; // Follow from behind
CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
// Allow some time for mode switch
sleep_for(std::chrono::milliseconds(1000));
// Start Follow Me
CHECK(FollowMe::Result::Success == _follow_me->start());
CHECK(FollowMe::Result::Success == _follow_me->start());
// Allow some time for mode switch
sleep_for(std::chrono::milliseconds(1000));
@@ -366,13 +339,12 @@ void AutopilotTesterFollowMe::stream_velocity_only()
CHECK(distance_travelled < position_tolerance);
}
void AutopilotTesterFollowMe::rc_adjustment_test()
{
void AutopilotTesterFollowMe::rc_adjustment_test() {
// CONFIGURATION
const unsigned loop_update_rate = 50; // [Hz]
const float follow_height_setting = 10.0f; // [m]
const float follow_angle_setting = 0.0f; // [deg]
const float follow_distance_setting = 10.0f; // [m]
const unsigned loop_update_rate = 50; // [Hz]
const float follow_height_setting = 10.0f; // [m]
const float follow_angle_setting = 0.0f; // [deg]
const float follow_distance_setting = 10.0f; // [m]
// The constants below are copied from the "FlightTaskAutoFollowTarget.hpp" to get a reference point for
// how much change a RC adjustment is expected to bring for each follow me parameters
@@ -381,7 +353,8 @@ void AutopilotTesterFollowMe::rc_adjustment_test()
static constexpr float FOLLOW_DISTANCE_USER_ADJUST_SPEED = 2.0;
// [m/s] Speed with which the follow height will be adjusted by when commanded with deflection via RC command
static constexpr float FOLLOW_HEIGHT_USER_ADJUST_SPEED = 1.5;
// [rad/s] Angular rate with which the follow distance will be adjusted by when commanded with full deflection via RC command
// [rad/s] Angular rate with which the follow distance will be adjusted by when commanded with full deflection
// via RC command
static constexpr float FOLLOW_ANGLE_USER_ADJUST_SPEED = 1.5;
// Start with simulated target on the same plane as drone's home position
@@ -408,7 +381,7 @@ void AutopilotTesterFollowMe::rc_adjustment_test()
const std::array<float, 3> target_pos = get_current_position_ned();
// Start Follow-me
CHECK(FollowMe::Result::Success == _follow_me->start());
CHECK(FollowMe::Result::Success == _follow_me->start());
std::array<float, 3> drone_initial_pos;
// task loop
@@ -419,7 +392,8 @@ void AutopilotTesterFollowMe::rc_adjustment_test()
if (i < 5 * loop_update_rate) {
// For 5 seconds, give time for the drone to go to it's initial following position (front)
CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) ==
ManualControl::Result::Success);
} else if (i == 5 * loop_update_rate) {
// At 5 second mark, record the current drone position as initial position
@@ -428,40 +402,50 @@ void AutopilotTesterFollowMe::rc_adjustment_test()
} else if (i < 8 * loop_update_rate) {
// FOLLOW HEIGHT ADJUSTMENT
// Command Throttle-up (Z = 1.0f) for 3 seconds
CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 1.0f, 0.f) == ManualControl::Result::Success);
CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 1.0f, 0.f) ==
ManualControl::Result::Success);
} else if (i < 10 * loop_update_rate) {
// Center the throttle for 2 seconds
CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) ==
ManualControl::Result::Success);
} else if (i == 10 * loop_update_rate) {
// Check if altitude has increased at least half of the expected adjustment (Z is directed downwards, so flip the sign)
CHECK(-(current_drone_pos[2] - drone_initial_pos[2]) > FOLLOW_HEIGHT_USER_ADJUST_SPEED * 3.0f * 0.5f);
// Check if altitude has increased at least half of the expected adjustment (Z is directed
// downwards, so flip the sign)
CHECK(-(current_drone_pos[2] - drone_initial_pos[2]) >
FOLLOW_HEIGHT_USER_ADJUST_SPEED * 3.0f * 0.5f);
} else if (i < 13 * loop_update_rate) {
// FOLLOW HEIGHT ADJUSTMENT
// Command Pitch-down (= Forward) (X = 1.0f) for 3 seconds
CHECK(getManualControl()->set_manual_control_input(1.0f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
CHECK(getManualControl()->set_manual_control_input(1.0f, 0.f, 0.5f, 0.f) ==
ManualControl::Result::Success);
} else if (i < 15 * loop_update_rate) {
// Center the Pitch for 2 seconds
CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) ==
ManualControl::Result::Success);
} else if (i == 15 * loop_update_rate) {
// Check if follow distance has increased at least half of the expected adjustment
const float target_to_drone_x_diff = current_drone_pos[0] - target_pos[0];
const float target_to_drone_y_diff = current_drone_pos[1] - target_pos[1];
const float current_follow_distance = sqrt(sq(target_to_drone_x_diff) + sq(target_to_drone_y_diff));
CHECK(current_follow_distance > follow_distance_setting + FOLLOW_DISTANCE_USER_ADJUST_SPEED * 3.0f * 0.5f);
const float current_follow_distance =
sqrt(sq(target_to_drone_x_diff) + sq(target_to_drone_y_diff));
CHECK(current_follow_distance >
follow_distance_setting + FOLLOW_DISTANCE_USER_ADJUST_SPEED * 3.0f * 0.5f);
} else if (i < 18 * loop_update_rate) {
// FOLLOW ANGLE ADJUSTMENT
// Command Roll-right (=Rightwards) (Y = 1.0f) for 3 seconds
CHECK(getManualControl()->set_manual_control_input(0.f, 1.0f, 0.5f, 0.f) == ManualControl::Result::Success);
CHECK(getManualControl()->set_manual_control_input(0.f, 1.0f, 0.5f, 0.f) ==
ManualControl::Result::Success);
} else if (i < 20 * loop_update_rate) {
// Center the Roll for 2 seconds
CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) ==
ManualControl::Result::Success);
} else if (i == 20 * loop_update_rate) {
// Check if follow angle has increased at least half of the expected adjustment
@@ -469,7 +453,8 @@ void AutopilotTesterFollowMe::rc_adjustment_test()
const float target_to_drone_x_diff = current_drone_pos[0] - target_pos[0];
const float target_to_drone_y_diff = current_drone_pos[1] - target_pos[1];
const float current_follow_angle_rad = atan2f(target_to_drone_y_diff, target_to_drone_x_diff);
CHECK(current_follow_angle_rad > follow_angle_setting * M_PI / 180.0f + FOLLOW_ANGLE_USER_ADJUST_SPEED * 3.0f * 0.5f);
CHECK(current_follow_angle_rad >
follow_angle_setting * M_PI / 180.0f + FOLLOW_ANGLE_USER_ADJUST_SPEED * 3.0f * 0.5f);
}
sleep_for(std::chrono::milliseconds(1000 / loop_update_rate));
test/mavsdk_tests/autopilot_tester_follow_me.h
+11 -10
View File
@@ -33,16 +33,14 @@
#pragma once
#include "autopilot_tester.h"
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/follow_me/follow_me.h>
#include "autopilot_tester.h"
// Simulate a target moving on a straight line in X+ direction
class FollowTargetSimulator
{
class FollowTargetSimulator {
public:
FollowTargetSimulator(std::array<float, 3> initial_position_ned, mavsdk::Telemetry::GroundTruth home);
~FollowTargetSimulator();
@@ -72,24 +70,28 @@ public:
// Retrieve ground truth of position state in global coordinate frame (lat, lon, alt)
std::array<double, 3> get_position_global_ground_truth();
// Run checks whether the drone has the correct angle towards the target, specified by the follow-me configuration
// Run checks whether the drone has the correct angle towards the target, specified by the follow-me
// configuration
void check_follow_angle(FollowMe::Config config, std::array<float, 3> drone_pos_ned,
std::array<float, 3> target_pos_ned, float tolerance);
private:
// Retrieve estimate with the option to add deterministic gaussian noise
//
// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of
// update()
std::array<double, 3> get_position_global(bool add_noise);
// Retrieve estimate with the option to add deterministic gaussian noise
//
// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of
// update()
std::array<float, 3> get_position_ned(bool add_noise);
// Retrieve estimate with the option to add deterministic gaussian noise
//
// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of
// update()
std::array<float, 3> get_velocity_ned(bool add_noise);
std::array<float, 3> _position_ned;
@@ -103,8 +105,7 @@ private:
std::unique_ptr<mavsdk::Telemetry> _telemetry{};
};
class AutopilotTesterFollowMe : public AutopilotTester
{
class AutopilotTesterFollowMe : public AutopilotTester {
public:
AutopilotTesterFollowMe() = default;
~AutopilotTesterFollowMe() = default;
test/mavsdk_tests/math_helpers.h
+16 -23
View File
@@ -33,9 +33,8 @@
#include <array>
template<typename T>
std::array<T, 3> get_local_mission_item(const Mission::MissionItem &item, const CoordinateTransformation &ct)
{
template <typename T>
std::array<T, 3> get_local_mission_item(const Mission::MissionItem &item, const CoordinateTransformation &ct) {
using GlobalCoordinate = mavsdk::geometry::CoordinateTransformation::GlobalCoordinate;
GlobalCoordinate global;
global.latitude_deg = item.latitude_deg;
@@ -44,34 +43,28 @@ std::array<T, 3> get_local_mission_item(const Mission::MissionItem &item, const
return {static_cast<T>(local.north_m), static_cast<T>(local.east_m), -item.relative_altitude_m};
}
template<typename T>
T sq(T x)
{
template <typename T>
T sq(T x) {
return x * x;
}
template<typename T>
T norm(const std::array<T, 3> &vec)
{
template <typename T>
T norm(const std::array<T, 3> &vec) {
return std::sqrt(sq(vec[0]) + sq(vec[1]) + sq(vec[2]));
}
template<typename T>
T dot(const std::array<T, 3> &vec1, const std::array<T, 3> &vec2)
{
template <typename T>
T dot(const std::array<T, 3> &vec1, const std::array<T, 3> &vec2) {
return vec1[0] * vec2[0] + vec1[1] * vec2[1] + vec1[2] * vec2[2];
}
template<typename T>
std::array<T, 3> diff(const std::array<T, 3> &vec1, const std::array<T, 3> &vec2)
{
template <typename T>
std::array<T, 3> diff(const std::array<T, 3> &vec1, const std::array<T, 3> &vec2) {
return {vec1[0] - vec2[0], vec1[1] - vec2[1], vec1[2] - vec2[2]};
}
template<typename T>
std::array<T, 3> normalized(const std::array<T, 3> &vec)
{
template <typename T>
std::array<T, 3> normalized(const std::array<T, 3> &vec) {
T n = norm(vec);
if (n > 1e-6f) {
@@ -82,15 +75,15 @@ std::array<T, 3> normalized(const std::array<T, 3> &vec)
}
}
template<typename T>
template <typename T>
T point_to_line_distance(const std::array<T, 3> &point, const std::array<T, 3> &line_start,
const std::array<T, 3> &line_end)
{
const std::array<T, 3> &line_end) {
std::array<T, 3> norm_dir = normalized(diff(line_end, line_start));
T t = dot(norm_dir, diff(point, line_start));
// closest_on_line = line_start + t * norm_dir;
std::array<T, 3> closest_on_line { line_start[0] + t *norm_dir[0], line_start[1] + t *norm_dir[1], line_start[2] + t *norm_dir[2]};
std::array<T, 3> closest_on_line{line_start[0] + t * norm_dir[0], line_start[1] + t * norm_dir[1],
line_start[2] + t * norm_dir[2]};
return norm(diff(closest_on_line, point));
}
test/mavsdk_tests/test_main.cpp
+8 -12
View File
@@ -32,19 +32,18 @@
****************************************************************************/
#define CATCH_CONFIG_RUNNER
#include "catch2/catch.hpp"
#include <mavsdk/mavsdk.h>
#include <iostream>
#include <string>
#include "autopilot_tester.h"
#include "autopilot_tester.h"
#include "catch2/catch.hpp"
static void usage(const std::string &bin_name);
static void remove_argv(int &argc, char **argv, int pos);
int main(int argc, char **argv)
{
int main(int argc, char **argv) {
for (int i = 0; i < argc; ++i) {
const std::string argv_string(argv[i]);
@@ -65,7 +64,6 @@ int main(int argc, char **argv)
usage(argv[0]);
return -1;
}
}
if (argv_string == "--speed-factor") {
@@ -86,7 +84,6 @@ int main(int argc, char **argv)
remove_argv(argc, argv, i);
--i;
} else {
std::cerr << "No speed factor supplied" << std::endl;
usage(argv[0]);
@@ -105,10 +102,10 @@ int main(int argc, char **argv)
return session.run();
}
void usage(const std::string &bin_name)
{
void usage(const std::string &bin_name) {
std::cout << std::endl
<< "Usage : " << bin_name << " [--url CONNECTION_URL] [--speed-factor SPEED_FACTOR] [catch2 arguments]\n"
<< "Usage : " << bin_name
<< " [--url CONNECTION_URL] [--speed-factor SPEED_FACTOR] [catch2 arguments]\n"
<< "\n"
<< " --url Connection URL format should be :\n"
<< " For TCP : tcp://[server_host][:server_port]\n"
@@ -120,8 +117,7 @@ void usage(const std::string &bin_name)
<< std::flush;
}
void remove_argv(int &argc, char **argv, int pos)
{
void remove_argv(int &argc, char **argv, int pos) {
for (int i = pos; i + 1 < argc; ++i) {
argv[i] = argv[i + 1];
}
test/mavsdk_tests/test_multicopter_control_allocation.cpp
+47 -52
View File
@@ -31,14 +31,14 @@
*
****************************************************************************/
#include <thread>
#include <chrono>
#include <math.h>
#include <chrono>
#include <thread>
#include "autopilot_tester_failure.h"
TEST_CASE("Control Allocation - Remove one motor", "[controlallocation]")
{
TEST_CASE("Control Allocation - Remove one motor", "[controlallocation]") {
const float flight_altitude = 10.0f;
const float altitude_tolerance = 4.0f;
const float hover_speed_tolerance = 1.0f;
@@ -53,17 +53,17 @@ TEST_CASE("Control Allocation - Remove one motor", "[controlallocation]")
// Configuration
tester.set_param_sys_failure_en(true); // Enable failure injection
tester.set_param_fd_act_en(true); // Enable motor failure detection
tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure
tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor
tester.set_param_fd_act_en(true); // Enable motor failure detection
tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure
tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor
tester.prepare_square_mission(mission_options);
tester.set_takeoff_altitude(flight_altitude);
tester.set_rtl_altitude(flight_altitude);
tester.check_tracks_mission(5.f);
tester.store_home();
tester.enable_actuator_output_status();
std::this_thread::sleep_for(std::chrono::seconds(
1)); // This is necessary for the takeoff altitude to be applied properly
std::this_thread::sleep_for(
std::chrono::seconds(1)); // This is necessary for the takeoff altitude to be applied properly
// Takeoff
tester.arm();
@@ -75,15 +75,15 @@ TEST_CASE("Control Allocation - Remove one motor", "[controlallocation]")
// Motor failure mid-air
tester.start_checking_altitude(altitude_tolerance);
const int motor_instance = 1;
const unsigned num_motors = 6; // TODO: get from model
tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, motor_instance,
mavsdk::Failure::Result::Success);
const unsigned num_motors = 6; // TODO: get from model
tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off,
motor_instance, mavsdk::Failure::Result::Success);
std::this_thread::sleep_for(std::chrono::seconds(1));
tester.ensure_motor_stopped(motor_instance - 1, num_motors);
tester.execute_mission();
tester.stop_checking_altitude();
tester.ensure_motor_stopped(motor_instance - 1, num_motors); // just to be sure
tester.ensure_motor_stopped(motor_instance - 1, num_motors); // just to be sure
// RTL
tester.execute_rtl();
@@ -92,8 +92,7 @@ TEST_CASE("Control Allocation - Remove one motor", "[controlallocation]")
tester.check_home_within(5.f);
}
TEST_CASE("Control Allocation - Remove two motors", "[controlallocation]")
{
TEST_CASE("Control Allocation - Remove two motors", "[controlallocation]") {
const float flight_altitude = 10.0f;
const float altitude_tolerance = 4.0f;
const float hover_speed_tolerance = 1.0f;
@@ -102,9 +101,9 @@ TEST_CASE("Control Allocation - Remove two motors", "[controlallocation]")
tester.connect(connection_url);
tester.wait_until_ready();
tester.set_param_sys_failure_en(true); // Enable failure injection
tester.set_param_fd_act_en(true); // Enable motor failure detection
tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure
tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor
tester.set_param_fd_act_en(true); // Enable motor failure detection
tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure
tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor
AutopilotTester::MissionOptions mission_options;
mission_options.rtl_at_end = false;
@@ -115,8 +114,8 @@ TEST_CASE("Control Allocation - Remove two motors", "[controlallocation]")
tester.set_rtl_altitude(flight_altitude);
tester.check_tracks_mission(5.f);
tester.store_home();
std::this_thread::sleep_for(std::chrono::seconds(
1)); // This is necessary for the takeoff altitude to be applied properly
std::this_thread::sleep_for(
std::chrono::seconds(1)); // This is necessary for the takeoff altitude to be applied properly
tester.arm();
tester.takeoff();
@@ -129,12 +128,10 @@ TEST_CASE("Control Allocation - Remove two motors", "[controlallocation]")
const int second_motor_instance = 2;
tester.start_checking_altitude(altitude_tolerance);
tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off,
first_motor_instance,
mavsdk::Failure::Result::Success);
first_motor_instance, mavsdk::Failure::Result::Success);
std::this_thread::sleep_for(std::chrono::seconds(1));
tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off,
second_motor_instance,
mavsdk::Failure::Result::Success);
second_motor_instance, mavsdk::Failure::Result::Success);
std::this_thread::sleep_for(std::chrono::seconds(1));
tester.execute_mission();
@@ -147,8 +144,7 @@ TEST_CASE("Control Allocation - Remove two motors", "[controlallocation]")
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Control Allocation - Remove and restore every motor once", "[controlallocation]")
{
TEST_CASE("Control Allocation - Remove and restore every motor once", "[controlallocation]") {
const float flight_altitude = 10.0f;
const float altitude_tolerance = 4.0f;
const float hover_speed_tolerance = 1.0f;
@@ -157,9 +153,9 @@ TEST_CASE("Control Allocation - Remove and restore every motor once", "[controla
tester.connect(connection_url);
tester.wait_until_ready();
tester.set_param_sys_failure_en(true); // Enable failure injection
tester.set_param_fd_act_en(true); // Enable motor failure detection
tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure
tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor
tester.set_param_fd_act_en(true); // Enable motor failure detection
tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure
tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor
AutopilotTester::MissionOptions mission_options;
mission_options.rtl_at_end = false;
@@ -170,8 +166,8 @@ TEST_CASE("Control Allocation - Remove and restore every motor once", "[controla
tester.set_rtl_altitude(flight_altitude);
tester.check_tracks_mission(5.f);
tester.store_home();
std::this_thread::sleep_for(std::chrono::seconds(
1)); // This is necessary for the takeoff altitude to be applied properly
std::this_thread::sleep_for(
std::chrono::seconds(1)); // This is necessary for the takeoff altitude to be applied properly
tester.arm();
tester.takeoff();
@@ -199,8 +195,7 @@ TEST_CASE("Control Allocation - Remove and restore every motor once", "[controla
tester.check_home_within(5.f);
}
TEST_CASE("Control Allocation - Return home on motor failure", "[controlallocation]")
{
TEST_CASE("Control Allocation - Return home on motor failure", "[controlallocation]") {
const float flight_altitude = 10.0f;
const float hover_speed_tolerance = 1.0f;
@@ -210,14 +205,14 @@ TEST_CASE("Control Allocation - Return home on motor failure", "[controlallocati
// Configuration
tester.set_param_sys_failure_en(true); // Enable failure injection
tester.set_param_fd_act_en(true); // Enable motor failure detection
tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure
tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor
tester.set_param_com_act_fail_act(3); // RTL on motor failure
tester.set_param_fd_act_en(true); // Enable motor failure detection
tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure
tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor
tester.set_param_com_act_fail_act(3); // RTL on motor failure
tester.set_takeoff_altitude(flight_altitude);
tester.store_home();
std::this_thread::sleep_for(std::chrono::seconds(
1)); // This is necessary for the takeoff altitude to be applied properly
std::this_thread::sleep_for(
std::chrono::seconds(1)); // This is necessary for the takeoff altitude to be applied properly
// Takeoff
tester.arm();
@@ -226,12 +221,13 @@ TEST_CASE("Control Allocation - Return home on motor failure", "[controlallocati
tester.wait_until_altitude(flight_altitude, std::chrono::seconds(30));
tester.wait_until_speed_lower_than(hover_speed_tolerance, std::chrono::seconds(30));
// TODO: Minor improvement, fly forward for a little bit before triggering motor failure to distinguish "RTL" and "Land only"
// TODO: Minor improvement, fly forward for a little bit before triggering motor failure to distinguish "RTL"
// and "Land only"
// Motor failure mid-air
const int motor_instance = 1;
tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, motor_instance,
mavsdk::Failure::Result::Success);
tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off,
motor_instance, mavsdk::Failure::Result::Success);
std::this_thread::sleep_for(std::chrono::seconds(1));
// Wait for RTL to trigger automatically
@@ -240,8 +236,7 @@ TEST_CASE("Control Allocation - Return home on motor failure", "[controlallocati
tester.check_home_within(5.f);
}
TEST_CASE("Control Allocation - Terminate on motor failure", "[controlallocation]")
{
TEST_CASE("Control Allocation - Terminate on motor failure", "[controlallocation]") {
const float flight_altitude = 100.0f;
const float hover_speed_tolerance = 1.0f;
@@ -251,13 +246,13 @@ TEST_CASE("Control Allocation - Terminate on motor failure", "[controlallocation
// Configuration
tester.set_param_sys_failure_en(true); // Enable failure injection
tester.set_param_fd_act_en(true); // Enable motor failure detection
tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure
tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor
tester.set_param_com_act_fail_act(4); // Terminate on motor failure
tester.set_param_fd_act_en(true); // Enable motor failure detection
tester.set_param_mc_airmode(1); // Enable airmode for control allocation with motor failure
tester.set_param_ca_failure_mode(1); // Enable control allocation handling of failed motor
tester.set_param_com_act_fail_act(4); // Terminate on motor failure
tester.set_takeoff_altitude(flight_altitude);
std::this_thread::sleep_for(std::chrono::seconds(
1)); // This is necessary for the takeoff altitude to be applied properly
std::this_thread::sleep_for(
std::chrono::seconds(1)); // This is necessary for the takeoff altitude to be applied properly
// Takeoff
tester.arm();
@@ -267,8 +262,8 @@ TEST_CASE("Control Allocation - Terminate on motor failure", "[controlallocation
// Motor failure mid-air
const int motor_instance = 1;
tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off, motor_instance,
mavsdk::Failure::Result::Success);
tester.inject_failure(mavsdk::Failure::FailureUnit::SystemMotor, mavsdk::Failure::FailureType::Off,
motor_instance, mavsdk::Failure::Result::Success);
std::this_thread::sleep_for(std::chrono::seconds(1));
// Wait for disarm with a low enough timeout such that it's necessary for the
test/mavsdk_tests/test_multicopter_failsafe.cpp
+7 -15
View File
@@ -33,9 +33,7 @@
#include "autopilot_tester.h"
TEST_CASE("Land on GPS lost during mission (baro height mode)", "[multicopter]")
{
TEST_CASE("Land on GPS lost during mission (baro height mode)", "[multicopter]") {
AutopilotTester tester;
tester.connect(connection_url);
@@ -51,8 +49,7 @@ TEST_CASE("Land on GPS lost during mission (baro height mode)", "[multicopter]")
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Land on GPS lost during mission (GPS height mode)", "[multicopter]")
{
TEST_CASE("Land on GPS lost during mission (GPS height mode)", "[multicopter]") {
AutopilotTester tester;
tester.connect(connection_url);
@@ -68,8 +65,7 @@ TEST_CASE("Land on GPS lost during mission (GPS height mode)", "[multicopter]")
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Continue on mag lost during mission", "[multicopter]")
{
TEST_CASE("Continue on mag lost during mission", "[multicopter]") {
AutopilotTester tester;
tester.connect(connection_url);
tester.wait_until_ready();
@@ -102,8 +98,7 @@ TEST_CASE("Continue on mag stuck during mission", "[multicopter][vtol]")
}
#endif
TEST_CASE("Continue on baro lost during mission (baro height mode)", "[multicopter]")
{
TEST_CASE("Continue on baro lost during mission (baro height mode)", "[multicopter]") {
AutopilotTester tester;
tester.connect(connection_url);
tester.set_height_source(AutopilotTester::HeightSource::Baro);
@@ -118,8 +113,7 @@ TEST_CASE("Continue on baro lost during mission (baro height mode)", "[multicopt
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Continue on baro lost during mission (GPS height mode)", "[multicopter]")
{
TEST_CASE("Continue on baro lost during mission (GPS height mode)", "[multicopter]") {
AutopilotTester tester;
tester.connect(connection_url);
tester.set_height_source(AutopilotTester::HeightSource::Gps);
@@ -134,8 +128,7 @@ TEST_CASE("Continue on baro lost during mission (GPS height mode)", "[multicopte
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Continue on baro stuck during mission (baro height mode)", "[multicopter]")
{
TEST_CASE("Continue on baro stuck during mission (baro height mode)", "[multicopter]") {
AutopilotTester tester;
tester.connect(connection_url);
tester.set_height_source(AutopilotTester::HeightSource::Baro);
@@ -150,8 +143,7 @@ TEST_CASE("Continue on baro stuck during mission (baro height mode)", "[multicop
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Continue on baro stuck during mission (GPS height mode)", "[multicopter]")
{
TEST_CASE("Continue on baro stuck during mission (GPS height mode)", "[multicopter]") {
AutopilotTester tester;
tester.connect(connection_url);
tester.set_height_source(AutopilotTester::HeightSource::Gps);
test/mavsdk_tests/test_multicopter_failure_injection.cpp
+2 -4
View File
@@ -31,13 +31,12 @@
*
****************************************************************************/
#include <thread>
#include <chrono>
#include <thread>
#include "autopilot_tester_failure.h"
TEST_CASE("Failure Injection - Reject mid-air when it is disabled", "[multicopter]")
{
TEST_CASE("Failure Injection - Reject mid-air when it is disabled", "[multicopter]") {
AutopilotTesterFailure tester;
tester.connect(connection_url);
tester.wait_until_ready();
@@ -51,4 +50,3 @@ TEST_CASE("Failure Injection - Reject mid-air when it is disabled", "[multicopte
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180);
tester.wait_until_disarmed(until_disarmed_timeout);
}
test/mavsdk_tests/test_multicopter_follow_me.cpp
+4 -10
View File
@@ -35,10 +35,7 @@
#include "autopilot_tester_follow_me.h"
TEST_CASE("Follow target - Position streaming", "[multicopter]")
{
TEST_CASE("Follow target - Position streaming", "[multicopter]") {
const bool stream_velocity = false;
AutopilotTesterFollowMe tester;
tester.connect(connection_url);
@@ -58,8 +55,7 @@ TEST_CASE("Follow target - Position streaming", "[multicopter]")
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Follow target - Position and velocity streaming", "[multicopter]")
{
TEST_CASE("Follow target - Position and velocity streaming", "[multicopter]") {
const bool stream_velocity = true;
AutopilotTesterFollowMe tester;
tester.connect(connection_url);
@@ -79,8 +75,7 @@ TEST_CASE("Follow target - Position and velocity streaming", "[multicopter]")
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Follow target - Velocity streaming only", "[multicopter]")
{
TEST_CASE("Follow target - Velocity streaming only", "[multicopter]") {
// Streaming only velocity should not work, the drone should not move.
// There needs to be at least one position message for follow-me
// to be able to integrate velocity.
@@ -103,8 +98,7 @@ TEST_CASE("Follow target - Velocity streaming only", "[multicopter]")
tester.check_home_within(1.0f);
}
TEST_CASE("Follow target - RC Adjustment", "[multicopter]")
{
TEST_CASE("Follow target - RC Adjustment", "[multicopter]") {
AutopilotTesterFollowMe tester;
tester.connect(connection_url);
tester.wait_until_ready();
test/mavsdk_tests/test_multicopter_manual.cpp
+2 -5
View File
@@ -33,9 +33,7 @@
#include "autopilot_tester.h"
TEST_CASE("Fly forward in position control", "[multicopter][vtol]")
{
TEST_CASE("Fly forward in position control", "[multicopter][vtol]") {
AutopilotTester tester;
tester.connect(connection_url);
tester.wait_until_ready();
@@ -47,8 +45,7 @@ TEST_CASE("Fly forward in position control", "[multicopter][vtol]")
tester.check_home_not_within(5.f);
}
TEST_CASE("Fly forward in altitude control", "[multicopter][vtol]")
{
TEST_CASE("Fly forward in altitude control", "[multicopter][vtol]") {
AutopilotTester tester;
tester.connect(connection_url);
tester.wait_until_ready();
test/mavsdk_tests/test_multicopter_mission.cpp
+5 -9
View File
@@ -31,12 +31,11 @@
*
****************************************************************************/
#include "autopilot_tester.h"
#include <chrono>
#include "autopilot_tester.h"
TEST_CASE("Takeoff and Land", "[multicopter][vtol]")
{
TEST_CASE("Takeoff and Land", "[multicopter][vtol]") {
AutopilotTester tester;
tester.connect(connection_url);
tester.wait_until_ready();
@@ -48,8 +47,7 @@ TEST_CASE("Takeoff and Land", "[multicopter][vtol]")
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Fly square Multicopter Missions including RTL", "[multicopter]")
{
TEST_CASE("Fly square Multicopter Missions including RTL", "[multicopter]") {
AutopilotTester tester;
tester.connect(connection_url);
tester.wait_until_ready();
@@ -63,8 +61,7 @@ TEST_CASE("Fly square Multicopter Missions including RTL", "[multicopter]")
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Fly square Multicopter Missions with manual RTL", "[multicopter]")
{
TEST_CASE("Fly square Multicopter Missions with manual RTL", "[multicopter]") {
AutopilotTester tester;
tester.connect(connection_url);
tester.wait_until_ready();
@@ -81,8 +78,7 @@ TEST_CASE("Fly square Multicopter Missions with manual RTL", "[multicopter]")
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Fly straight Multicopter Mission", "[multicopter]")
{
TEST_CASE("Fly straight Multicopter Mission", "[multicopter]") {
AutopilotTester tester;
tester.connect(connection_url);
tester.wait_until_ready();
test/mavsdk_tests/test_multicopter_offboard.cpp
+8 -10
View File
@@ -31,14 +31,13 @@
*
****************************************************************************/
#include "autopilot_tester.h"
#include <chrono>
#include "autopilot_tester.h"
TEST_CASE("Offboard takeoff and land", "[multicopter][offboard]")
{
TEST_CASE("Offboard takeoff and land", "[multicopter][offboard]") {
AutopilotTester tester;
Offboard::PositionNedYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f};
Offboard::PositionNedYaw takeoff_position{0.0f, 0.0f, -2.0f, 0.0f};
tester.connect(connection_url);
tester.wait_until_ready();
tester.store_home();
@@ -51,13 +50,12 @@ TEST_CASE("Offboard takeoff and land", "[multicopter][offboard]")
tester.check_home_within(1.0f);
}
TEST_CASE("Offboard position control", "[multicopter][offboard]")
{
TEST_CASE("Offboard position control", "[multicopter][offboard]") {
AutopilotTester tester;
Offboard::PositionNedYaw takeoff_position {0.0f, 0.0f, -2.0f, 0.0f};
Offboard::PositionNedYaw setpoint_1 {0.0f, 5.0f, -2.0f, 180.0f};
Offboard::PositionNedYaw setpoint_2 {5.0f, 5.0f, -4.0f, 180.0f};
Offboard::PositionNedYaw setpoint_3 {5.0f, 0.0f, -4.0f, 90.0f};
Offboard::PositionNedYaw takeoff_position{0.0f, 0.0f, -2.0f, 0.0f};
Offboard::PositionNedYaw setpoint_1{0.0f, 5.0f, -2.0f, 180.0f};
Offboard::PositionNedYaw setpoint_2{5.0f, 5.0f, -4.0f, 180.0f};
Offboard::PositionNedYaw setpoint_3{5.0f, 0.0f, -4.0f, 90.0f};
tester.connect(connection_url);
tester.wait_until_ready();
tester.store_home();
test/mavsdk_tests/test_vtol_mission.cpp
+1 -3
View File
@@ -33,9 +33,7 @@
#include "autopilot_tester.h"
TEST_CASE("Fly VTOL mission", "[vtol]")
{
TEST_CASE("Fly VTOL mission", "[vtol]") {
AutopilotTester tester;
tester.connect(connection_url);
tester.wait_until_ready();