Tools: autotest: break out roundtrip_fencepoint_protocol method

Tools/autotest/apmrover2.py

@@ -1044,25 +1044,21 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
         self.wait_location(loc, accuracy=accuracy)
         self.disarm_vehicle()
 
-    def test_gcs_fence(self):
-        self.progress("Testing FENCE_POINT protocol")
-        self.set_parameter("FENCE_TOTAL", 1)
-        target_system = 1
-        target_component = 1
+    def roundtrip_fencepoint_protocol(self, offset, count, lat, lng, target_system=1, target_component=1):
 
-        lat = 1.2345
-        lng = 5.4321
+        self.progress("Sending fence point")
         self.mav.mav.fence_point_send(target_system,
                                       target_component,
-                                      0,
+                                      offset,
                                       1,
                                       lat,
                                       lng)
-        self.progress("Requesting fence return point")
+
+        self.progress("Requesting fence point")
         self.mav.mav.fence_fetch_point_send(target_system,
                                             target_component,
-                                            0)
-        m = self.mav.recv_match(type="FENCE_POINT", blocking=True, timeout=1)
+                                            offset)
+        m = self.mav.recv_match(type="FENCE_POINT", blocking=True, timeout=2)
         print("m: %s" % str(m))
         if m is None:
             raise NotAchievedException("Did not get fence return point back")
@@ -1070,26 +1066,20 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
             raise NotAchievedException("Did not get correct lat in fencepoint: got=%f want=%f" % (m.lat, lat))
         if abs(m.lng - lng) > 0.000001:
             raise NotAchievedException("Did not get correct lng in fencepoint: got=%f want=%f" % (m.lng, lng))
+        self.progress("Roundtrip OK")
+
+    def test_gcs_fence(self):
+        target_system = 1
+        target_component = 1
+
+        self.progress("Testing FENCE_POINT protocol")
+        self.set_parameter("FENCE_TOTAL", 1)
+
+        self.roundtrip_fencepoint_protocol(0, 1, 1.2345, 5.4321, target_system=target_component, target_component=target_component)
 
-        self.progress("Now testing a different value")
         lat = 2.345
         lng = 4.321
-        self.mav.mav.fence_point_send(target_system,
-                                      target_component,
-                                      0,
-                                      1,
-                                      lat,
-                                      lng)
-        self.progress("Requesting fence return point")
-        self.mav.mav.fence_fetch_point_send(target_system,
-                                            target_component,
-                                            0)
-        m = self.mav.recv_match(type="FENCE_POINT", blocking=True, timeout=1)
-        print("m: %s" % str(m))
-        if abs(m.lat - lat) > 0.000001:
-            raise NotAchievedException("Did not get correct lat in fencepoint: got=%f want=%f" % (m.lat, lat))
-        if abs(m.lng - lng) > 0.000001:
-            raise NotAchievedException("Did not get correct lng in fencepoint: got=%f want=%f" % (m.lng, lng))
+        self.roundtrip_fencepoint_protocol(0, 1, lat, lng, target_system=target_component, target_component=target_component)
 
     def test_offboard(self, timeout=90):
         self.load_mission("rover-guided-mission.txt")