From 64182261060098b43f692e67758b410f7ad51e67 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 8 Mar 2019 10:06:46 +1100 Subject: [PATCH] Tools: autotest: increase test diagnostics Tools: autotest: improve wait_distance_home_gt output Tools: autotest: emit progress for parameter sets Tools: autotest: add progress for RC health checks Tools: autotest: remove pointless context --- Tools/autotest/apmrover2.py | 5 ++++- Tools/autotest/arduplane.py | 1 + Tools/autotest/common.py | 10 +++++++--- 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py index 59a2c4e758..e7c8bec2ba 100644 --- a/Tools/autotest/apmrover2.py +++ b/Tools/autotest/apmrover2.py @@ -500,8 +500,11 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) tstart = self.get_sim_time() while self.get_sim_time_cached() - tstart < timeout: # m = self.mav.recv_match(type='VFR_HUD', blocking=True) - if self.distance_to_home() > distance: + 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)) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 8170a15a90..b6e072011f 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -771,6 +771,7 @@ class AutoTestPlane(AutoTest): self.progress("Testing receiver health") if (m.onboard_control_sensors_health & receiver_bit): raise NotAchievedException("Sensor healthy when it shouldn't be") + self.progress("Making RC work again") self.set_parameter("SIM_RC_FAIL", 0) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 71e877dfcb..37465e0101 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -944,6 +944,7 @@ class AutoTest(ABC): def set_parameter(self, name, value, add_to_context=True, epsilon=0.00001): """Set parameters from vehicle.""" + self.progress("Setting %s to %f" % (name, value)) old_value = self.get_parameter(name, retry=2) for i in range(1, 10): self.mavproxy.send("param set %s %s\n" % (name, str(value))) @@ -1461,19 +1462,24 @@ class AutoTest(ABC): "%.4f,%.4f at altitude %.1f height_accuracy=%.1f" % (loc.lat, loc.lng, target_altitude, height_accuracy)) last_distance_message = 0 + closest = 10000000 + last = 0 while self.get_sim_time() < tstart + timeout: pos = self.mav.location() delta = self.get_distance(loc, pos) if self.get_sim_time_cached() - last_distance_message >= 1: self.progress("Distance %.2f meters alt %.1f" % (delta, pos.alt)) last_distance_message = self.get_sim_time_cached() + if closest > delta: + closest = delta + last = delta if delta <= accuracy: height_delta = math.fabs(pos.alt - target_altitude) if height_accuracy != -1 and height_delta > height_accuracy: continue self.progress("Reached location (%.2f meters)" % delta) return True - raise WaitLocationTimeout("Failed to attain location") + raise WaitLocationTimeout("Failed to attain location (want=<%f) (closest=%f)" % (accuracy, closest, last)) def wait_current_waypoint(self, wpnum, timeout=60): tstart = self.get_sim_time() @@ -1849,7 +1855,6 @@ class AutoTest(ABC): def test_arm_feature(self): """Common feature to test.""" - self.context_push() # TEST ARMING/DISARM self.set_parameter("ARMING_RUDDER", 2) # allow arm and disarm with rudder on first tests interlock_channel = 8 # Plane got flighmode_ch on channel 8 @@ -1980,7 +1985,6 @@ class AutoTest(ABC): raise NotAchievedException("Motor interlock was changed while disarmed") self.set_rc(interlock_channel, 1000) self.progress("ALL PASS") - self.context_pop() # TODO : add failure test : arming check, wrong mode; Test arming magic; Same for disarm def get_message_rate(self, victim_message, timeout):