diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index 4b7264c4bb9..0f0fa6a920c 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -7337,6 +7337,7 @@ class AutoTestCopter(AutoTest):
         self.do_RTL()
 
     def GPSForYaw(self):
+        self.context_push()
         self.load_default_params_file("copter-gps-for-yaw.parm")
         self.reboot_sitl()
         ex = None
@@ -7344,12 +7345,18 @@ class AutoTestCopter(AutoTest):
             self.wait_gps_fix_type_gte(6, message_type="GPS2_RAW", verbose=True)
             m = self.assert_receive_message("GPS2_RAW")
             self.progress(self.dump_message_verbose(m))
-            if m.yaw == 0:
-                raise NotAchievedException("Expected to get GPS-from-yaw")
+            want = 27000
+            if abs(m.yaw - want) > 500:
+                raise NotAchievedException("Expected to get GPS-from-yaw (want %f got %f)" % (want, m.yaw))
             self.wait_ready_to_arm()
         except Exception as e:
             self.print_exception_caught(e)
             ex = e
+
+        self.context_pop()
+
+        self.reboot_sitl()
+
         if ex is not None:
             raise ex