dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

autopilot_tester: Make `wait_until_ready` also wait until vehicle can 

arm

- Previously, due to the way MAVSDK's `health_all_ok` was implemented,
vehicle often didn't have a valid global position estimate (although
function returned true), and it wouldn't arm, and the SITL would fail
- Also sometimes as vehicle didn't have manual control, it entered weird
states where it wasn't able to arm as well
- This adds a check to make sure vehicle is able to arm, directly from
the Health struct
This commit is contained in:
Junwoo Hwang 2022-10-26 15:39:29 +02:00 committed by Julian Oes
parent dac9f0dac4
commit 3e35f948d8
2 changed files with 22 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
test/mavsdk_tests/autopilot_tester.cpp
View File

@ -80,27 +80,22 @@ void AutopilotTester::connect(const std::string uri)
void AutopilotTester::wait_until_ready()
{
std::cout << time_str() << "Waiting for system to be ready" << std::endl;
std::cout << time_str() << "Waiting for system to be ready (system health ok & able to arm)" << std::endl;
// Wiat until the system is healthy
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,
// our arming presumably triggers a failsafe in manual mode.
std::this_thread::sleep_for(std::chrono::seconds(1));
}
// Note: There is a known bug in MAVSDK (https://github.com/mavlink/MAVSDK/issues/1852),
// where `health_all_ok()` returning true doesn't actually mean vehicle is ready to accept
// global position estimate as valid (due to hysteresis). This needs to be fixed properly.
void AutopilotTester::wait_until_ready_local_position_only()
{
std::cout << time_str() << "Waiting for system to be ready" << std::endl;
// However, this is mitigated by the `is_armable` check below as a side effect, since
// when the vehicle considers global position to be valid, it will then allow arming
// Wait until we can arm
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_armable; }, std::chrono::seconds(20)));
}
void AutopilotTester::store_home()

12
test/mavsdk_tests/autopilot_tester.h
View File

@ -95,8 +95,12 @@ public:
~AutopilotTester();
void connect(const std::string uri);
/**
* @brief Wait until vehicle's system status is healthy & is able to arm
*/
void wait_until_ready();
void wait_until_ready_local_position_only();
void store_home();
void check_home_within(float acceptance_radius_m);
void check_home_not_within(float min_distance_m);
@ -204,6 +208,12 @@ private:
void report_speed_factor();
/**
* @brief Continue polling until condition returns true or we have a timeout
*
* @param fun Boolean returning function. When true, the polling terminates.
* @param duration Timeout for polling in `std::chrono::` time unit
*/
template<typename Rep, typename Period>
bool poll_condition_with_timeout(
std::function<bool()> fun, std::chrono::duration<Rep, Period> duration)