fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 16:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

autopilot_tester: Make wait_until_ready also wait until vehicle can

Browse Source 
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-11-08 11:53:43 +13:00
committed by Julian Oes
parent dac9f0dac4
commit 3e35f948d8
2 changed files with 22 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/mavsdk_tests/autopilot_tester.cpp
+11 -16
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()