mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
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.
This commit is contained in:
Peter Barker
Peter Barker
parent
1509a8ea8d
commit
3ca6e29ad1
@ -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()
|
||||
|
||||
@ -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,
|
||||
self.wait_location(
|
||||
guided_loc,
|
||||
accuracy=200,
|
||||
target_altitude=None,
|
||||
timeout=600)
|
||||
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,
|
||||
self.wait_location(
|
||||
rally_loc,
|
||||
accuracy=200,
|
||||
target_altitude=None,
|
||||
timeout=600)
|
||||
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,
|
||||
self.wait_location(
|
||||
guided_loc,
|
||||
accuracy=200,
|
||||
target_altitude=None,
|
||||
timeout=600)
|
||||
timeout=600,
|
||||
height_accuracy=10,
|
||||
)
|
||||
|
||||
self.change_mode("RTL")
|
||||
self.progress("Flying to rally point")
|
||||
self.wait_location(rally_loc,
|
||||
self.wait_location(
|
||||
rally_loc,
|
||||
accuracy=200,
|
||||
target_altitude=None,
|
||||
timeout=600)
|
||||
timeout=600,
|
||||
height_accuracy=10,
|
||||
)
|
||||
self.progress("Reached rally point with terrain alt frame")
|
||||
|
||||
self.context_pop()
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user