Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Tools: autotest: allow nav-delay-abstime to delay arbitrary time

Browse Source
This commit is contained in:
Peter Barker 2019-08-15 20:14:52 +10:00 committed by Peter Barker
parent 1b36d64794
commit 479b2c5e11
1 changed files with 28 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
Tools/autotest/arducopter.py
View File

@ -1876,28 +1876,31 @@ class AutoTestCopter(AutoTest):
hours = hour_ms / (60 * 60 * 1000) hours = hour_ms / (60 * 60 * 1000)
return (hours, mins, secs, 0) return (hours, mins, secs, 0)
def calc_delay(self, seconds): def calc_delay(self, seconds, delay_for_seconds):
# delay-for-seconds has to be long enough that we're at the # delay-for-seconds has to be long enough that we're at the
# waypoint before that time. Otherwise we'll try to wait a # waypoint before that time. Otherwise we'll try to wait a
# day.... # day....
if delay_for_seconds >= 3600:
raise ValueError("Won't handle large delays")
(hours, (hours,
mins, mins,
secs, secs,
ms) = self.get_system_clock_utc(seconds) ms) = self.get_system_clock_utc(seconds)
self.progress("Now is %uh %um %us" % (hours, mins, secs)) self.progress("Now is %uh %um %us" % (hours, mins, secs))
secs += 17 # add seventeen seconds secs += delay_for_seconds # add seventeen seconds
if secs >= 60: mins += int(secs/60)
secs %= 60 secs %= 60
mins += 1 # add sixty seconds
mins += 1 hours += int(mins / 60)
if mins >= 60: mins %= 60
mins %= 60
hours += 1 if hours > 24:
if hours >= 24: raise ValueError("Way too big a delay")
hours %= 24 self.progress("Delay until %uh %um %us" %
(hours, mins, secs))
return (hours, mins, secs, 0) return (hours, mins, secs, 0)
def reset_delay_item_seventyseven(self, seq): def reset_delay_item(self, seq, seconds_in_future):
while True: while True:
self.progress("Requesting request for seq %u" % (seq,)) self.progress("Requesting request for seq %u" % (seq,))
self.mav.mav.mission_write_partial_list_send(1, # target system self.mav.mav.mission_write_partial_list_send(1, # target system
@ -1974,11 +1977,7 @@ class AutoTestCopter(AutoTest):
raise PreconditionFailedException("Never got SYSTEM_TIME") raise PreconditionFailedException("Never got SYSTEM_TIME")
if now.time_unix_usec == 0: if now.time_unix_usec == 0:
raise PreconditionFailedException("system time is zero") raise PreconditionFailedException("system time is zero")
(hours, mins, secs, ms) = self.calc_delay( (hours, mins, secs, ms) = self.calc_delay(now.time_unix_usec/1000000, seconds_in_future)
now.time_unix_usec/1000000)
self.progress("Delay until %uh %um %us" %
(hours, mins, secs))
self.mav.mav.mission_item_send( self.mav.mav.mission_item_send(
1, # target system 1, # target system
@ -2003,6 +2002,13 @@ class AutoTestCopter(AutoTest):
def fly_nav_delay_abstime(self): def fly_nav_delay_abstime(self):
"""fly a simple mission that has a delay in it""" """fly a simple mission that has a delay in it"""
self.fly_nav_delay_abstime_x(87)
def fly_nav_delay_abstime_x(self, delay_for, expected_delay=None):
"""fly a simple mission that has a delay in it, expect a delay"""
if expected_delay is None:
expected_delay = delay_for
self.load_mission("copter_nav_delay.txt") self.load_mission("copter_nav_delay.txt")
@ -2011,8 +2017,8 @@ class AutoTestCopter(AutoTest):
self.wait_ready_to_arm() self.wait_ready_to_arm()
delay_item_seq = 3 delay_item_seq = 3
self.reset_delay_item_seventyseven(delay_item_seq) self.reset_delay_item(delay_item_seq, delay_for)
delay_for_seconds = 77 delay_for_seconds = delay_for
reset_at_m = self.mav.recv_match(type='SYSTEM_TIME', blocking=True) reset_at_m = self.mav.recv_match(type='SYSTEM_TIME', blocking=True)
reset_at = reset_at_m.time_unix_usec/1000000 reset_at = reset_at_m.time_unix_usec/1000000
@ -2023,7 +2029,7 @@ class AutoTestCopter(AutoTest):
tstart = self.get_sim_time() tstart = self.get_sim_time()
while self.armed(): # we RTL at end of mission while self.armed(): # we RTL at end of mission
now = self.get_sim_time_cached() now = self.get_sim_time_cached()
if now - tstart > 120: if now - tstart > 240:
raise AutoTestTimeoutException("Did not disarm as expected") raise AutoTestTimeoutException("Did not disarm as expected")
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True) m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
at_delay_item = "" at_delay_item = ""
@ -2036,7 +2042,7 @@ class AutoTestCopter(AutoTest):
blocking=True) blocking=True)
count_stop = count_stop_m.time_unix_usec/1000000 count_stop = count_stop_m.time_unix_usec/1000000
calculated_delay = count_stop - reset_at calculated_delay = count_stop - reset_at
error = abs(calculated_delay - delay_for_seconds) error = abs(calculated_delay - expected_delay)
self.progress("Stopped for %u seconds (want >=%u seconds)" % self.progress("Stopped for %u seconds (want >=%u seconds)" %
(calculated_delay, delay_for_seconds)) (calculated_delay, delay_for_seconds))
if error > 2: if error > 2:
@ -2050,8 +2056,8 @@ class AutoTestCopter(AutoTest):
self.wait_ready_to_arm() self.wait_ready_to_arm()
delay_item_seq = 2 delay_item_seq = 2
self.reset_delay_item_seventyseven(delay_item_seq)
delay_for_seconds = 77 delay_for_seconds = 77
self.reset_delay_item(delay_item_seq, delay_for_seconds)
reset_at = self.get_sim_time_cached() reset_at = self.get_sim_time_cached()
self.arm_vehicle() self.arm_vehicle()