From fa33719da6e23d43bfd2beab37421ce226f93f4f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 13 Feb 2020 09:47:09 +1100 Subject: [PATCH] autotest: improve distance-from-home debug --- Tools/autotest/apmrover2.py | 35 ++++++----------------------------- Tools/autotest/arducopter.py | 23 ++--------------------- Tools/autotest/common.py | 11 +++++++++++ 3 files changed, 19 insertions(+), 50 deletions(-) diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index f2378a80e6..06b96981f7 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -464,19 +464,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) self.progress("RTL Mission OK (%fm)" % home_distance) - def wait_distance_home_gt(self, distance, timeout=60): - home_distance = None - tstart = self.get_sim_time() - while self.get_sim_time_cached() - tstart < timeout: - # m = self.mav.recv_match(type='VFR_HUD', blocking=True) - distance_home = self.distance_to_home(use_cached_home=True) - self.progress("distance_home=%f want=%f" % (distance_home, distance)) - if distance_home > distance: - return - self.drain_mav() - raise NotAchievedException("Failed to get %fm from home (now=%f)" % - (distance, home_distance)) - def drive_fence_ac_avoidance(self): self.context_push() ex = None @@ -493,7 +480,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) self.set_rc(10, 1000) self.change_mode("ACRO") self.set_rc(3, 1550) - self.wait_distance_home_gt(25) + self.wait_distance_to_home(25, 100000, timeout=60) self.change_mode("RTL") self.mavproxy.expect("APM: Reached destination") # now enable avoidance and make sure we can't: @@ -3401,16 +3388,6 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) # MANUAL> usage: wp - def wait_distance_to_home(self, distance, accuracy=5, timeout=30): - tstart = self.get_sim_time() - while True: - if self.get_sim_time_cached() - tstart > timeout: - raise AutoTestTimeoutException("Failed to get home") - self.mav.recv_match(type='VFR_HUD', blocking=True) - self.progress("Dist from home: %.02f" % self.distance_to_home(use_cached_home=True)) - if abs(distance-self.distance_to_home(use_cached_home=True)) <= accuracy: - break - def wait_location_sending_target(self, loc, target_system=1, target_component=1, timeout=60, max_delta=2): tstart = self.get_sim_time() last_sent = 0 @@ -3522,7 +3499,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) raise NotAchievedException("Breach of unexpected type") if self.mode_is("RTL", cached=True) and seen_fence_breach: break - self.wait_distance_to_home(5, accuracy=2) + self.wait_distance_to_home(3, 7, timeout=30) def drive_somewhere_stop_at_boundary(self, loc, @@ -4315,7 +4292,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) self.wait_distance_to_location(loc, 0, 5) self.progress("Ensure we get home") - self.wait_distance_to_home(5, accuracy=2) + self.wait_distance_to_home(3, 7, timeout=30) self.disarm_vehicle() @@ -4369,7 +4346,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) target_loc = mavutil.location(40.073799, -105.229156) self.wait_location(target_loc, timeout=300) # mission has RTL as last item - self.wait_distance_to_home(5, accuracy=2, timeout=300) + self.wait_distance_to_home(3, 7, timeout=300) self.disarm_vehicle() except Exception as e: self.progress("Caught exception: %s" % @@ -4554,7 +4531,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) def do_RTL(self, timeout=60): self.change_mode("RTL") - self.wait_distance_to_home(5, accuracy=2, timeout=timeout) + self.wait_distance_to_home(3, 7, timeout=timeout) def test_poly_fence_avoidance(self, target_system=1, target_component=1): self.change_mode("LOITER") @@ -4675,7 +4652,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) # target_loc is copied from the mission file self.wait_location(target_loc, timeout=300) # mission has RTL as last item - self.wait_distance_to_home(5, accuracy=2, timeout=300) + self.wait_distance_to_home(3, 7, timeout=300) self.disarm_vehicle() except Exception as e: self.progress("Caught exception: %s" % diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index f72b13a3de..e84a3341d8 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -969,17 +969,6 @@ class AutoTestCopter(AutoTest): self.progress("Armed") return - def wait_distance_from_home(self, distance_min, distance_max, timeout=10, use_cached_home=True): - tstart = self.get_sim_time() - while True: - if self.get_sim_time() - tstart > timeout: - raise NotAchievedException("Did not achieve distance from home") - distance = self.distance_to_home(use_cached_home) - self.progress("Distance from home: now=%f %f= distance_min and distance <= distance_max: - return - # fly_fence_test - fly east until you hit the horizontal circular fence avoid_behave_slide = 0 def fly_fence_avoid_test_radius_check(self, timeout=180, avoid_behave=avoid_behave_slide): @@ -1005,7 +994,7 @@ class AutoTestCopter(AutoTest): self.wait_altitude(10, 100, relative=True) self.set_rc(3, 1500) self.set_rc(2, 1400) - self.wait_distance_from_home(12, 20) + self.wait_distance_to_home(12, 20) tstart = self.get_sim_time() push_time = 70 # push against barrier for 60 seconds failed_max = False @@ -1408,15 +1397,7 @@ class AutoTestCopter(AutoTest): self.wait_waypoint(0, num_wp-1, timeout=500) # wait for arrival back home - self.mav.recv_match(type='VFR_HUD', blocking=True) - while self.distance_to_home(use_cached_home=True) > 5: - if self.get_sim_time_cached() > (tstart + timeout): - raise AutoTestTimeoutException( - ("GPS Glitch testing failed" - "- exceeded timeout %u seconds" % timeout)) - - self.mav.recv_match(type='VFR_HUD', blocking=True) - self.progress("Dist from home: %.02f" % self.distance_to_home(use_cached_home=True)) + self.wait_distance_to_home(0, 10, timeout=timeout) # turn off simulator display of gps and actual position if self.use_map: diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index dd05081d29..fa5b3d8042 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -4130,6 +4130,17 @@ switch value''' pass return fred + def wait_distance_to_home(self, distance_min, distance_max, timeout=10, use_cached_home=True): + tstart = self.get_sim_time() + while True: + if self.get_sim_time() - tstart > timeout: + raise NotAchievedException("Did not achieve distance from home") + distance = self.distance_to_home(use_cached_home) + self.progress("Distance from home: now=%f %f= distance_min and distance <= distance_max: + return + def download_parameters(self, target_system, target_component): # try a simple fetch-all: self.mav.mav.param_request_list_send(target_system, target_component)