fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavsdk_tests: use autopilot timestamps to sleep

Browse Source 
This way we should be able to avoid some of the timeouts happening on
the PX4 side if MAVSDK doesn't send setpoints in time.
This commit is contained in:
Julian Oes 2021-02-15 16:17:39 +01:00 committed by Lorenz Meier
parent 6c26387e85
commit 2315618b85
2 changed files with 27 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
test/mavsdk_tests/autopilot_tester.cpp
View File

@ -45,7 +45,7 @@ void AutopilotTester::connect(const std::string uri)
std::cout << time_str() << "Waiting for system connect" << std::endl;
REQUIRE(poll_condition_with_timeout(
[this]() { return _mavsdk.is_connected(); }, adjust_to_lockstep_speed(std::chrono::seconds(25))));
[this]() { return _mavsdk.is_connected(); }, std::chrono::seconds(25)));
auto &system = _mavsdk.system();
@ -347,7 +347,7 @@ void AutopilotTester::fly_forward_in_posctl()
// Send something to make sure RC is available.
for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) {
CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
}
CHECK(_manual_control->start_position_control() == ManualControl::Result::Success);
@ -355,19 +355,19 @@ void AutopilotTester::fly_forward_in_posctl()
// Climb up for 20 seconds
for (unsigned i = 0; i < 20 * manual_control_rate_hz; ++i) {
CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success);
std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
}
// 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);
std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
}
// Descend until disarmed
for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) {
CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.0f, 0.f) == ManualControl::Result::Success);
std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
if (!_telemetry->in_air()) {
break;
@ -382,7 +382,7 @@ void AutopilotTester::fly_forward_in_altctl()
// Send something to make sure RC is available.
for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) {
CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
}
CHECK(_manual_control->start_altitude_control() == ManualControl::Result::Success);
@ -390,19 +390,19 @@ void AutopilotTester::fly_forward_in_altctl()
// Climb up for 20 seconds
for (unsigned i = 0; i < 20 * manual_control_rate_hz; ++i) {
CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success);
std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
}
// 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);
std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
}
// Descend until disarmed
for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) {
CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.0f, 0.f) == ManualControl::Result::Success);
std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz)));
sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz));
if (!_telemetry->in_air()) {
break;

38
test/mavsdk_tests/autopilot_tester.h
View File

@ -183,30 +183,28 @@ private:
}
template<typename Rep, typename Period>
std::chrono::microseconds adjust_to_lockstep_speed(std::chrono::duration<Rep, Period> duration)
void sleep_for(std::chrono::duration<Rep, Period> duration)
{
const std::chrono::microseconds duration_us(duration);
if (_info == nullptr) {
return duration_us;
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;
if (elapsed_time_us > duration_us.count()) {
return;
}
}
} else {
std::this_thread::sleep_for(duration);
}
auto speed_factor = _info->get_speed_factor();
if (speed_factor.first != Info::Result::Success) {
return duration_us;
}
// FIXME: Remove this again:
// Sanitize speed factor to avoid test failures.
if (speed_factor.second > 10.0f) {
speed_factor.second = 10.0f;
}
const auto new_duration_us = static_cast<std::chrono::microseconds>(duration_us.count() / static_cast<int>
(speed_factor.second));
return new_duration_us;
}
mavsdk::Mavsdk _mavsdk{};