diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp
index 504165804b..c58dd28bf6 100644
--- a/test/mavsdk_tests/autopilot_tester.cpp
+++ b/test/mavsdk_tests/autopilot_tester.cpp
@@ -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()
diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h
index a36a70f7bf..bb4b826c0e 100644
--- a/test/mavsdk_tests/autopilot_tester.h
+++ b/test/mavsdk_tests/autopilot_tester.h
@@ -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)