From 3ca6e29ad17c087bedc874c0e5f79409200ba768 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 29 Feb 2024 11:54:39 +1100 Subject: [PATCH] autotest: reimplement wait_location with new object-based approach the existing wait_and_maintain method is problematic when it comes to waiting on different value types. --- Tools/autotest/arducopter.py | 16 +- Tools/autotest/arduplane.py | 49 +++--- Tools/autotest/rover.py | 2 +- Tools/autotest/vehicle_test_suite.py | 216 +++++++++++++++++++-------- 4 files changed, 194 insertions(+), 89 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 528f9851fe..ca554a4a68 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -7090,22 +7090,23 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): def check_avoidance_corners(self): self.takeoff(10, mode="LOITER") + here = self.mav.location() self.set_rc(2, 1400) west_loc = mavutil.location(-35.363007, 149.164911, - 0, + here.alt, 0) self.wait_location(west_loc, accuracy=6) north_loc = mavutil.location(-35.362908, 149.165051, - 0, + here.alt, 0) self.reach_heading_manual(0) self.wait_location(north_loc, accuracy=6, timeout=200) self.reach_heading_manual(90) east_loc = mavutil.location(-35.363013, 149.165194, - 0, + here.alt, 0) self.wait_location(east_loc, accuracy=6) self.reach_heading_manual(225) @@ -7556,18 +7557,19 @@ class AutoTestCopter(vehicle_test_suite.TestSuite): self.takeoff(10, mode="LOITER") self.set_rc(2, 1400) - west_loc = mavutil.location(-35.362919, 149.165055, 0, 0) + here = self.mav.location() + west_loc = mavutil.location(-35.362919, 149.165055, here.alt, 0) self.wait_location(west_loc, accuracy=1) self.reach_heading_manual(0) - north_loc = mavutil.location(-35.362881, 149.165103, 0, 0) + north_loc = mavutil.location(-35.362881, 149.165103, here.alt, 0) self.wait_location(north_loc, accuracy=1) self.set_rc(2, 1500) self.set_rc(1, 1600) - east_loc = mavutil.location(-35.362986, 149.165227, 0, 0) + east_loc = mavutil.location(-35.362986, 149.165227, here.alt, 0) self.wait_location(east_loc, accuracy=1) self.set_rc(1, 1500) self.set_rc(2, 1600) - south_loc = mavutil.location(-35.363025, 149.165182, 0, 0) + south_loc = mavutil.location(-35.363025, 149.165182, here.alt, 0) self.wait_location(south_loc, accuracy=1) self.set_rc(2, 1500) self.do_RTL() diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 3365d81147..2c4b78f25f 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -175,10 +175,11 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): def fly_RTL(self): """Fly to home.""" self.progress("Flying home in RTL") + target_loc = self.homeloc + target_loc.alt += 100 self.change_mode('RTL') - self.wait_location(self.homeloc, + self.wait_location(target_loc, accuracy=120, - target_altitude=self.homeloc.alt+100, height_accuracy=20, timeout=180) self.progress("RTL Complete") @@ -1877,8 +1878,8 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.wait_ready_to_arm() self.homeloc = self.mav.location() - guided_loc = mavutil.location(-35.39723762, 149.07284612, 99.0, 0) - rally_loc = mavutil.location(-35.3654952000, 149.1558698000, 100, 0) + guided_loc = mavutil.location(-35.39723762, 149.07284612, self.homeloc.alt+99.0, 0) + rally_loc = mavutil.location(-35.3654952000, 149.1558698000, self.homeloc.alt+100, 0) terrain_wait_path(self.homeloc, rally_loc, 10) @@ -1898,19 +1899,23 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.change_mode("GUIDED") self.do_reposition(guided_loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT) self.progress("Flying to guided location") - self.wait_location(guided_loc, - accuracy=200, - target_altitude=None, - timeout=600) + self.wait_location( + guided_loc, + accuracy=200, + timeout=600, + height_accuracy=10, + ) self.progress("Reached guided location") self.set_parameter("RALLY_LIMIT_KM", 50) self.change_mode("RTL") self.progress("Flying to rally point") - self.wait_location(rally_loc, - accuracy=200, - target_altitude=None, - timeout=600) + self.wait_location( + rally_loc, + accuracy=200, + timeout=600, + height_accuracy=10, + ) self.progress("Reached rally point with TERRAIN_FOLLOW") # Fly back to guided location @@ -1933,17 +1938,21 @@ class AutoTestPlane(vehicle_test_suite.TestSuite): self.check_rally_upload_download(rally_item) # Once back at guided location re-trigger RTL - self.wait_location(guided_loc, - accuracy=200, - target_altitude=None, - timeout=600) + self.wait_location( + guided_loc, + accuracy=200, + timeout=600, + height_accuracy=10, + ) self.change_mode("RTL") self.progress("Flying to rally point") - self.wait_location(rally_loc, - accuracy=200, - target_altitude=None, - timeout=600) + self.wait_location( + rally_loc, + accuracy=200, + timeout=600, + height_accuracy=10, + ) self.progress("Reached rally point with terrain alt frame") self.context_pop() diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 8fbf383e10..ec7c012559 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -1369,7 +1369,7 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm) # location copied in from rover-test-rally.txt: loc = mavutil.location(40.071553, -105.229401, - 0, + 1583, 0) self.wait_location(loc, accuracy=accuracy, minimum_duration=10, timeout=45) diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 2b45dbec87..e0423db0c9 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -342,6 +342,117 @@ class Telem(object): self.update_read() +class WaitAndMaintain(object): + def __init__(self, + test_suite, + minimum_duration=None, + progress_print_interval=1, + timeout=30, + ): + self.test_suite = test_suite + self.minimum_duration = minimum_duration + self.achieving_duration_start = None + self.timeout = timeout + self.last_progress_print = 0 + self.progress_print_interval = progress_print_interval + + def run(self): + self.announce_test_start() + + tstart = self.test_suite.get_sim_time_cached() + while True: + now = self.test_suite.get_sim_time_cached() + current_value = self.get_current_value() + if now - self.last_progress_print > self.progress_print_interval: + self.print_progress(now, current_value) + self.last_progress_print = now + + # check for timeout + if now - tstart > self.timeout: + raise self.timeoutexception() + + # handle the case where we are are achieving our value: + if self.validate_value(current_value): + if self.achieving_duration_start is None: + self.achieving_duration_start = now + if (self.minimum_duration is None or + now - self.achieving_duration_start > self.minimum_duration): + self.announce_success() + return True + continue + + # handle the case where we are not achieving our value: + self.achieving_duration_start = None + + def progress(self, text): + self.test_suite.progress(text) + + def announce_test_start(self): + self.progress(self.announce_start_text()) + + def announce_success(self): + self.progress(self.success_text()) + + def print_progress(self, now, value): + text = self.progress_text(value) + if self.achieving_duration_start is not None: + delta = now - self.achieving_duration_start + text += f" (maintain={delta:.1f}/{self.minimum_duration})" + self.progress(text) + + +class WaitAndMaintainLocation(WaitAndMaintain): + def __init__(self, test_suite, target, accuracy=5, height_accuracy=1, **kwargs): + super(WaitAndMaintainLocation, self).__init__(test_suite, **kwargs) + self.target = target + self.height_accuracy = height_accuracy + self.accuracy = accuracy + + def announce_start_text(self): + t = self.target + if self.height_accuracy is not None: + return ("Waiting for distance to Location (%.4f, %.4f, %.2f) (h_err<%f, v_err<%.2f " % + (t.lat, t.lng, t.alt*0.01, self.accuracy, self.height_accuracy)) + return ("Waiting for distance to Location (%.4f, %.4f) (h_err<%f" % + (t.lat, t.lng, self.accuracy)) + + def get_target_value(self): + return self.loc + + def get_current_value(self): + return self.test_suite.mav.location() + + def horizontal_error(self, value): + return self.test_suite.get_distance(value, self.target) + + def vertical_error(self, value): + return math.fabs(value.alt*0.01 - self.target.alt*0.01) + + def validate_value(self, value): + if self.horizontal_error(value) > self.accuracy: + return False + + if self.height_accuracy is None: + return True + + if self.vertical_error(value) > self.height_accuracy: + return False + + return True + + def success_text(self): + return "Reached location" + + def timeoutexception(self): + return AutoTestTimeoutException("Failed to attain location") + + def progress_text(self, current_value): + if self.height_accuracy is not None: + return (f"Want=({self.target.lat:.7f},{self.target.lng:.7f},{self.target.alt:.2f}) Got=({current_value.lat:.7f},{current_value.lng:.7f},{current_value.alt:.2f}) dist={self.horizontal_error(current_value):.2f} vdist={self.vertical_error(current_value):.2f}") # noqa + + return (f"Want=({self.target.lat},{self.target.lng}) distance={self.horizontal_error(current_value)}") + + class MSP_Generic(Telem): def __init__(self, destination_address): super(MSP_Generic, self).__init__(destination_address) @@ -7490,38 +7601,9 @@ class TestSuite(ABC): 0, # "land dir" 0) # flags - def wait_location(self, - loc, - accuracy=5.0, - timeout=30, - target_altitude=None, - height_accuracy=-1, - **kwargs): - """Wait for arrival at a location.""" - def get_distance_to_loc(): - return self.get_distance(self.mav.location(), loc) - - def validator(value2, empty=None): - if value2 <= accuracy: - if target_altitude is not None: - height_delta = math.fabs(self.mav.location().alt - target_altitude) - if height_accuracy != -1 and height_delta > height_accuracy: - return False - return True - else: - return False - debug_text = "Distance to Location (%.4f, %.4f) " % (loc.lat, loc.lng) - if target_altitude is not None: - debug_text += ",at altitude %.1f height_accuracy=%.1f, d" % (target_altitude, height_accuracy) - self.wait_and_maintain( - value_name=debug_text, - target=0, - current_value_getter=lambda: get_distance_to_loc(), - accuracy=accuracy, - validator=lambda value2, target2: validator(value2, None), - timeout=timeout, - **kwargs - ) + def wait_location(self, loc, **kwargs): + waiter = WaitAndMaintainLocation(self, loc, **kwargs) + waiter.run() def assert_current_waypoint(self, wpnum): seq = self.mav.waypoint_current() @@ -10774,7 +10856,7 @@ Also, ignores heartbeats not from our target system''' # Disable heading and yaw test on rover type if self.is_rover(): - test_alt = False + test_alt = True test_heading = False test_yaw_rate = False else: @@ -10835,6 +10917,16 @@ Also, ignores heartbeats not from our target system''' 0, # yawrate ) + def testpos(self, targetpos : mavutil.location, test_alt : bool, frame_name : str, frame): + send_target_position(targetpos.lat, targetpos.lng, to_alt_frame(targetpos.alt, frame_name), frame) + self.wait_location( + targetpos, + accuracy=wp_accuracy, + timeout=timeout, + height_accuracy=(2 if test_alt else None), + minimum_duration=2, + ) + for frame in MAV_FRAMES_TO_TEST: frame_name = mavutil.mavlink.enums["MAV_FRAME"][frame].name self.start_subtest("Testing Set Position in %s" % frame_name) @@ -10842,37 +10934,25 @@ Also, ignores heartbeats not from our target system''' targetpos.lat += 0.0001 if test_alt: targetpos.alt += 5 - send_target_position(targetpos.lat, targetpos.lng, to_alt_frame(targetpos.alt, frame_name), frame) - self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout, - target_altitude=(targetpos.alt if test_alt else None), - height_accuracy=2, minimum_duration=2) + testpos(self, targetpos, test_alt, frame_name, frame) self.start_subtest("Changing Longitude") targetpos.lng += 0.0001 if test_alt: targetpos.alt -= 5 - send_target_position(targetpos.lat, targetpos.lng, to_alt_frame(targetpos.alt, frame_name), frame) - self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout, - target_altitude=(targetpos.alt if test_alt else None), - height_accuracy=2, minimum_duration=2) + testpos(self, targetpos, test_alt, frame_name, frame) self.start_subtest("Revert Latitude") targetpos.lat -= 0.0001 if test_alt: targetpos.alt += 5 - send_target_position(targetpos.lat, targetpos.lng, to_alt_frame(targetpos.alt, frame_name), frame) - self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout, - target_altitude=(targetpos.alt if test_alt else None), - height_accuracy=2, minimum_duration=2) + testpos(self, targetpos, test_alt, frame_name, frame) self.start_subtest("Revert Longitude") targetpos.lng -= 0.0001 if test_alt: targetpos.alt -= 5 - send_target_position(targetpos.lat, targetpos.lng, to_alt_frame(targetpos.alt, frame_name), frame) - self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout, - target_altitude=(targetpos.alt if test_alt else None), - height_accuracy=2, minimum_duration=2) + testpos(self, targetpos, test_alt, frame_name, frame) if test_heading: self.start_subtest("Testing Yaw targetting in %s" % frame_name) @@ -10900,9 +10980,13 @@ Also, ignores heartbeats not from our target system''' math.radians(42), # yaw 0, # yawrate ) - self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout, - target_altitude=(targetpos.alt if test_alt else None), - height_accuracy=2, minimum_duration=2) + self.wait_location( + targetpos, + accuracy=wp_accuracy, + timeout=timeout, + height_accuracy=(2 if test_alt else None), + minimum_duration=2, + ) self.wait_heading(42, minimum_duration=5, timeout=timeout) self.start_subtest("Revert Latitude and Heading") @@ -10929,9 +11013,13 @@ Also, ignores heartbeats not from our target system''' math.radians(0), # yaw 0, # yawrate ) - self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout, - target_altitude=(targetpos.alt if test_alt else None), - height_accuracy=2, minimum_duration=2) + self.wait_location( + targetpos, + accuracy=wp_accuracy, + timeout=timeout, + height_accuracy=(2 if test_alt else None), + minimum_duration=2, + ) self.wait_heading(0, minimum_duration=5, timeout=timeout) if test_yaw_rate: @@ -10967,9 +11055,12 @@ Also, ignores heartbeats not from our target system''' self.wait_yaw_speed(target_rate, timeout=timeout, called_function=lambda plop, empty: send_yaw_rate( target_rate, None), minimum_duration=5) - self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout, - target_altitude=(targetpos.alt if test_alt else None), - height_accuracy=2) + self.wait_location( + targetpos, + accuracy=wp_accuracy, + timeout=timeout, + height_accuracy=(2 if test_alt else None), + ) self.start_subtest("Revert Latitude and invert Yaw rate") target_rate = -1.0 @@ -10979,9 +11070,12 @@ Also, ignores heartbeats not from our target system''' self.wait_yaw_speed(target_rate, timeout=timeout, called_function=lambda plop, empty: send_yaw_rate( target_rate, None), minimum_duration=5) - self.wait_location(targetpos, accuracy=wp_accuracy, timeout=timeout, - target_altitude=(targetpos.alt if test_alt else None), - height_accuracy=2) + self.wait_location( + targetpos, + accuracy=wp_accuracy, + timeout=timeout, + height_accuracy=(2 if test_alt else None), + ) self.start_subtest("Changing Yaw rate to zero") target_rate = 0.0 self.wait_yaw_speed(target_rate, timeout=timeout,