From ecc6f15dc0d20efa22752bbd5ca7bd81c9d0f962 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 11 May 2019 18:18:21 +1000 Subject: [PATCH] AP_Compass: use EXPECT_DELAY() macro --- libraries/AP_Compass/CompassCalibrator.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp index 3a472a7aeb..1758ae2665 100644 --- a/libraries/AP_Compass/CompassCalibrator.cpp +++ b/libraries/AP_Compass/CompassCalibrator.cpp @@ -781,7 +781,7 @@ bool CompassCalibrator::calculate_orientation(void) } // this function is very slow - hal.scheduler->expect_delay_ms(1000); + EXPECT_DELAY(hal, 1000); float variance[ROTATION_MAX] {}; @@ -849,8 +849,6 @@ bool CompassCalibrator::calculate_orientation(void) gcs().send_text(MAV_SEVERITY_INFO, "Mag(%u) new orientation: %u was %u %.1f", _compass_idx, besti, _orientation, (double)_orientation_confidence); } - hal.scheduler->expect_delay_ms(0); - if (!pass) { set_status(COMPASS_CAL_BAD_ORIENTATION); return false;