aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Tools: autotest: add test for delay-until-absolute-hour-minute-second

Browse Source
This commit is contained in:
Peter Barker 2018-08-07 19:58:40 +10:00 committed by Peter Barker
parent e932f873cc
commit eba8dee371
1 changed files with 182 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

182
Tools/autotest/arducopter.py
View File

@ -1295,6 +1295,185 @@ class AutoTestCopter(AutoTest):
if ex is not None: if ex is not None:
raise ex raise ex
def get_system_clock_utc(self, time):
# this is a copy of ArduPilot's AP_RTC function!
# separate time into ms, sec, min, hour and days but all expressed
# in milliseconds
time_ms = time * 1000
ms = time_ms % 1000;
sec_ms = (time_ms % (60 * 1000)) - ms;
min_ms = (time_ms % (60 * 60 * 1000)) - sec_ms - ms;
hour_ms = (time_ms % (24 * 60 * 60 * 1000)) - min_ms - sec_ms - ms;
# convert times as milliseconds into appropriate units
sec = sec_ms / 1000;
min = min_ms / (60 * 1000);
hour = hour_ms / (60 * 60 * 1000);
return (hour, min, sec, 0)
def calc_delay(self, seconds):
# delay-for-seconds has to be long enough that we're at the
# waypoint before that time. Otherwise we'll try to wait a
# day....
(hours,
mins,
secs,
ms) = self.get_system_clock_utc(seconds)
self.progress("Now is %uh %um %us" % (hours, mins, secs))
secs += 17 # add seventeen seconds
if secs >= 60:
secs %= 60
mins += 1 # add sixty seconds
mins += 1
if mins >= 60:
mins %= 60
hours += 1
if hours >= 24:
hours %= 24
return (hours, mins, secs, 0)
def reset_delay_item_seventyseven(self):
seq = 3
while True:
self.progress("Requesting request for seq %u" % (seq,))
self.mav.mav.mission_write_partial_list_send(1, # target system
1, # target component
seq, # start index
seq)
req = self.mav.recv_match(type='MISSION_REQUEST',
blocking=True,
timeout=1)
if req is not None and req.seq == seq:
if req.get_srcSystem() == 255:
self.progress("Shutup MAVProxy")
continue
# notionally this might be in the message cache before
# we prompt for it... *shrug*
break
# we have received a request for the item. Supply it:
frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
command = mavutil.mavlink.MAV_CMD_NAV_DELAY
# retrieve mission item and check it:
tried_set = False
while True:
st = self.mav.recv_match(type='MISSION_ITEM',
blocking=True,
timeout=1)
if st is not None:
print("Item: %s" % str(st))
if (tried_set and
st.seq == 3 and
st.command == command and
st.param2 == hours and
st.param3 == mins and
st.param4 == secs):
return
self.progress("Mission mismatch")
self.mav.mav.mission_write_partial_list_send(1,
1,
3,
3)
m = self.mav.messages.get("MISSION_REQUEST", None)
if m is not None:
if m.seq == 3:
self.progress("Sending absolute-time mission item")
# we have to change out the delay time...
now = self.mav.recv_match(type='SYSTEM_TIME',
blocking=True)
if now.time_unix_usec == 0:
raise PreconditionFailedException()
(hours, mins, secs, ms) = self.calc_delay(
now.time_unix_usec/1000000)
self.progress("Delay until %uh %um %us" %
(hours, mins, secs))
self.mav.mav.mission_item_send(
1, # target system
1, # target component
seq, # seq
frame, # frame
command, # command
0, # current
1, # autocontinue
0, # p1 (relative seconds)
hours, # p2
mins, # p3
secs, # p4
0, # p5
0, # p6
0) # p7
tried_set = True
self.progress("Requesting item")
self.mav.mav.mission_request_send(1,
1,
3
)
def fly_nav_delay_abstime(self):
'''fly a simple mission that has a delay in it'''
num_wp = self.load_mission("copter_nav_delay.txt")
self.progress("Starting mission")
self.mavproxy.send('mode loiter\n') # stabilize mode
self.mav.wait_heartbeat()
self.wait_mode('LOITER')
self.progress("Waiting reading for arm")
self.wait_ready_to_arm()
self.reset_delay_item_seventyseven()
delay_for_seconds = 77
reset_at_m = self.mav.recv_match(type='SYSTEM_TIME', blocking=True)
reset_at = reset_at_m.time_unix_usec/1000000
self.context_push();
ex = None
try:
self.arm_vehicle()
self.mavproxy.send('mode auto\n') # stabilize mode
self.wait_mode('AUTO')
self.set_rc(3, 1600)
count_stop = -1
tstart = self.get_sim_time()
while self.armed(): # we RTL at end of mission
now = self.get_sim_time()
if now - tstart > 120:
raise AutoTestTimeoutException()
m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
self.progress("MISSION_CURRENT.seq=%u" % (m.seq,))
if m.seq == 3:
self.progress("At delay item")
if m.seq > 3:
if count_stop == -1:
count_stop_m = self.mav.recv_match(type='SYSTEM_TIME',
blocking=True)
count_stop = count_stop_m.time_unix_usec/1000000
print("count_stop=%f reste at %f", count_stop, reset_at)
calculated_delay = count_stop - reset_at
error = abs(calculated_delay - delay_for_seconds)
self.progress("Stopped for %u seconds (want >=%u seconds)" %
(calculated_delay, delay_for_seconds))
if error > 2:
self.progress("Too far outside expectations")
raise NotAchievedException()
except Exception as e:
self.progress("Exception caught")
ex = e
self.set_rc(3, 1000)
if ex is not None:
raise ex
def test_setting_modes_via_modeswitch(self): def test_setting_modes_via_modeswitch(self):
self.context_push(); self.context_push();
ex = None ex = None
@ -1456,6 +1635,9 @@ class AutoTestCopter(AutoTest):
self.set_rc_default() self.set_rc_default()
self.set_rc(3, 1000) self.set_rc(3, 1000)
self.run_test("Fly Nav Delay (AbsTime)",
self.fly_nav_delay_abstime)
self.run_test("Fly Nav Delay", self.fly_nav_delay) self.run_test("Fly Nav Delay", self.fly_nav_delay)
self.run_test("Test submode change", self.run_test("Test submode change",