autotest: correct Rover DriveMission test

4 would seem to have been just a magic number and could be missed on faster speedups
This commit is contained in:
Peter Barker 2022-08-19 12:03:43 +10:00 committed by Peter Barker
parent 81740893ee
commit cd207164a2
2 changed files with 3 additions and 3 deletions

View File

@ -6543,7 +6543,7 @@ class AutoTest(ABC):
(seq, wp_dist, m.alt, current_wp, wpnum_end)) (seq, wp_dist, m.alt, current_wp, wpnum_end))
last_wp_msg = self.get_sim_time_cached() last_wp_msg = self.get_sim_time_cached()
if seq == current_wp+1 or (seq > current_wp+1 and allow_skip): if seq == current_wp+1 or (seq > current_wp+1 and allow_skip):
self.progress("test: Starting new waypoint %u" % seq) self.progress("WW: Starting new waypoint %u" % seq)
tstart = self.get_sim_time() tstart = self.get_sim_time()
current_wp = seq current_wp = seq
# the wp_dist check is a hack until we can sort out # the wp_dist check is a hack until we can sort out

View File

@ -395,11 +395,11 @@ class AutoTestRover(AutoTest):
def drive_mission(self, filename, strict=True): def drive_mission(self, filename, strict=True):
"""Drive a mission from a file.""" """Drive a mission from a file."""
self.progress("Driving mission %s" % filename) self.progress("Driving mission %s" % filename)
self.load_mission(filename, strict=strict) wp_count = self.load_mission(filename, strict=strict)
self.wait_ready_to_arm() self.wait_ready_to_arm()
self.arm_vehicle() self.arm_vehicle()
self.change_mode('AUTO') self.change_mode('AUTO')
self.wait_waypoint(1, 4, max_dist=5) self.wait_waypoint(1, wp_count-1, max_dist=5)
self.wait_statustext("Mission Complete", timeout=600) self.wait_statustext("Mission Complete", timeout=600)
self.disarm_vehicle() self.disarm_vehicle()
self.progress("Mission OK") self.progress("Mission OK")