Tools: autotest: add test suite for Tracker
This commit is contained in:
Peter Barker
Peter Barker
parent
fe6ca9afa3
commit
31a9ac7dbd
124
Tools/autotest/antennatracker.py
Normal file
124
Tools/autotest/antennatracker.py
Normal file
@ -0,0 +1,124 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from __future__ import print_function
|
||||
import os
|
||||
import pexpect
|
||||
from pymavlink import mavutil
|
||||
|
||||
from common import AutoTest
|
||||
from pysim import util
|
||||
from pysim import vehicleinfo
|
||||
import operator
|
||||
|
||||
# get location of scripts
|
||||
testdir = os.path.dirname(os.path.realpath(__file__))
|
||||
HOME = mavutil.location(-27.274439, 151.290064, 343, 8.7)
|
||||
|
||||
class AutoTestTracker(AutoTest):
|
||||
def __init__(self,
|
||||
binary,
|
||||
valgrind=False,
|
||||
gdb=False,
|
||||
speedup=10,
|
||||
frame=None,
|
||||
params=None,
|
||||
gdbserver=False,
|
||||
breakpoints=[],
|
||||
**kwargs):
|
||||
super(AutoTestTracker, self).__init__(**kwargs)
|
||||
self.binary = binary
|
||||
self.valgrind = valgrind
|
||||
self.gdb = gdb
|
||||
self.frame = frame
|
||||
self.params = params
|
||||
self.gdbserver = gdbserver
|
||||
self.breakpoints = breakpoints
|
||||
|
||||
self.home = "%f,%f,%u,%u" % (HOME.lat,
|
||||
HOME.lng,
|
||||
HOME.alt,
|
||||
HOME.heading)
|
||||
self.homeloc = None
|
||||
self.speedup = speedup
|
||||
|
||||
self.log_name = "AntennaTracker"
|
||||
self.logfile = None
|
||||
|
||||
self.sitl = None
|
||||
|
||||
def start_stream_systemtime(self):
|
||||
'''AntennaTracker doesn't stream this by default but we need it for get_sim_time'''
|
||||
try:
|
||||
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_SYSTEM_TIME, 10)
|
||||
except Exception:
|
||||
pass
|
||||
self.set_message_rate_hz(mavutil.mavlink.MAVLINK_MSG_ID_SYSTEM_TIME, 10)
|
||||
|
||||
def set_rc_default(self):
|
||||
'''tracker does not send RC_CHANNELS, so can't set_rc_default'''
|
||||
'''... however, dodgily hook in here to get system time:'''
|
||||
self.start_stream_systemtime()
|
||||
|
||||
def initialise_after_reboot_sitl(self):
|
||||
self.start_stream_systemtime()
|
||||
|
||||
def default_mode(self):
|
||||
return "AUTO"
|
||||
|
||||
def is_tracker(self):
|
||||
return True
|
||||
|
||||
def init(self):
|
||||
if self.frame is None:
|
||||
self.frame = "tracker"
|
||||
|
||||
self.mavproxy_logfile = self.open_mavproxy_logfile()
|
||||
|
||||
self.sitl = util.start_SITL(self.binary,
|
||||
wipe=True,
|
||||
model=self.frame,
|
||||
home=self.home,
|
||||
speedup=self.speedup,
|
||||
valgrind=self.valgrind,
|
||||
gdb=self.gdb,
|
||||
gdbserver=self.gdbserver,
|
||||
breakpoints=self.breakpoints,
|
||||
)
|
||||
self.mavproxy = util.start_MAVProxy_SITL(
|
||||
'AntennaTracker', options=self.mavproxy_options())
|
||||
self.mavproxy.expect('Telemetry log: (\S+)\r\n')
|
||||
self.logfile = self.mavproxy.match.group(1)
|
||||
self.progress("LOGFILE %s" % self.logfile)
|
||||
self.try_symlink_tlog()
|
||||
|
||||
self.mavproxy.expect('Received [0-9]+ parameters')
|
||||
|
||||
util.expect_setup_callback(self.mavproxy, self.expect_callback)
|
||||
|
||||
self.expect_list_clear()
|
||||
self.expect_list_extend([self.sitl, self.mavproxy])
|
||||
|
||||
self.progress("Started simulator")
|
||||
|
||||
self.get_mavlink_connection_going()
|
||||
|
||||
self.progress("Ready to start testing!")
|
||||
|
||||
def sysid_thismav(self):
|
||||
return 2
|
||||
|
||||
def reboot_sitl(self):
|
||||
"""Reboot SITL instance and wait it to reconnect."""
|
||||
self.mavproxy.send("reboot\n")
|
||||
self.mavproxy.expect("Initialising APM")
|
||||
# empty mav to avoid getting old timestamps:
|
||||
while self.mav.recv_match(blocking=False):
|
||||
pass
|
||||
self.initialise_after_reboot_sitl()
|
||||
|
||||
def tests(self):
|
||||
'''return list of all tests'''
|
||||
ret = super(AutoTestTracker, self).tests()
|
||||
ret.extend([
|
||||
])
|
||||
return ret
|
||||
@ -21,6 +21,7 @@ import apmrover2
|
||||
import arducopter
|
||||
import arduplane
|
||||
import ardusub
|
||||
import antennatracker
|
||||
import quadplane
|
||||
import balancebot
|
||||
|
||||
@ -281,7 +282,7 @@ def binary_path(step, debug=False):
|
||||
|
||||
def split_specific_test_step(step):
|
||||
print('step=%s' % str(step))
|
||||
m = re.match("((fly|drive|dive)[.][^.]+)[.](.*)", step)
|
||||
m = re.match("((fly|drive|dive|test)[.][^.]+)[.](.*)", step)
|
||||
if m is None:
|
||||
return None
|
||||
return ( (m.group(1), m.group(3)) )
|
||||
@ -307,6 +308,7 @@ def run_specific_test(step, *args, **kwargs):
|
||||
"drive.BalanceBot": balancebot.AutoTestBalanceBot,
|
||||
"fly.CopterAVC": arducopter.AutoTestHeli,
|
||||
"dive.ArduSub": ardusub.AutoTestSub,
|
||||
"test.AntennaTracker": antennatracker.AutoTestTracker,
|
||||
}
|
||||
tester_class = tester_class_map[testname]
|
||||
tester = tester_class(*args, **kwargs)
|
||||
@ -405,6 +407,10 @@ def run_step(step):
|
||||
tester = ardusub.AutoTestSub(binary, **fly_opts)
|
||||
return tester.autotest()
|
||||
|
||||
if step == 'test.AntennaTracker':
|
||||
tester = antennatracker.AutoTestTracker(binary, **fly_opts)
|
||||
return tester.autotest()
|
||||
|
||||
specific_test_to_run = find_specific_test_to_run(step)
|
||||
if specific_test_to_run is not None:
|
||||
return run_specific_test(specific_test_to_run, binary, **fly_opts)
|
||||
@ -755,6 +761,8 @@ if __name__ == "__main__":
|
||||
'fly.CopterAVC',
|
||||
|
||||
'build.AntennaTracker',
|
||||
'defaults.AntennaTracker',
|
||||
'test.AntennaTracker',
|
||||
|
||||
'build.ArduSub',
|
||||
'defaults.ArduSub',
|
||||
|
||||
@ -672,7 +672,7 @@ class AutoTest(ABC):
|
||||
def check_rc_defaults(self):
|
||||
"""Ensure all rc outputs are at defaults"""
|
||||
_defaults = self.rc_defaults()
|
||||
m = self.mav.recv_match(type='RC_CHANNELS', blocking=True)
|
||||
m = self.mav.recv_match(type='RC_CHANNELS', blocking=True, timeout=5)
|
||||
if m is None:
|
||||
raise NotAchievedException("No RC_CHANNELS messages?!")
|
||||
need_set = {}
|
||||
@ -961,6 +961,45 @@ class AutoTest(ABC):
|
||||
old_value,
|
||||
add_to_context=False)
|
||||
|
||||
def sysid_thismav(self):
|
||||
return 1
|
||||
|
||||
def run_cmd_int(self,
|
||||
command,
|
||||
p1,
|
||||
p2,
|
||||
p3,
|
||||
p4,
|
||||
x,
|
||||
y,
|
||||
z,
|
||||
want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED,
|
||||
timeout=10,
|
||||
target_sysid=None,
|
||||
target_compid=None,
|
||||
frame=mavutil.mavlink.MAV_FRAME_GLOBAL_INT):
|
||||
|
||||
if target_sysid is None:
|
||||
target_sysid = self.sysid_thismav()
|
||||
if target_compid is None:
|
||||
target_compid = 1
|
||||
|
||||
"""Send a MAVLink command int."""
|
||||
self.mav.mav.command_int_send(target_sysid,
|
||||
target_compid,
|
||||
frame,
|
||||
command,
|
||||
0, # current
|
||||
0, # autocontinue
|
||||
p1,
|
||||
p2,
|
||||
p3,
|
||||
p4,
|
||||
x,
|
||||
y,
|
||||
z)
|
||||
self.run_cmd_get_ack(command, want_result, timeout)
|
||||
|
||||
def run_cmd(self,
|
||||
command,
|
||||
p1,
|
||||
@ -971,10 +1010,16 @@ class AutoTest(ABC):
|
||||
p6,
|
||||
p7,
|
||||
want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED,
|
||||
target_sysid=None,
|
||||
target_compid=None,
|
||||
timeout=10):
|
||||
"""Send a MAVLink command long."""
|
||||
self.mav.mav.command_long_send(1,
|
||||
1,
|
||||
if target_sysid is None:
|
||||
target_sysid = self.sysid_thismav()
|
||||
if target_compid is None:
|
||||
target_compid = 1
|
||||
self.mav.mav.command_long_send(target_sysid,
|
||||
target_compid,
|
||||
command,
|
||||
1, # confirmation
|
||||
p1,
|
||||
@ -984,6 +1029,9 @@ class AutoTest(ABC):
|
||||
p5,
|
||||
p6,
|
||||
p7)
|
||||
self.run_cmd_get_ack(command, want_result, timeout)
|
||||
|
||||
def run_cmd_get_ack(self, command, want_result, timeout):
|
||||
tstart = self.get_sim_time_cached()
|
||||
while True:
|
||||
if self.get_sim_time_cached() - tstart > timeout:
|
||||
@ -1635,39 +1683,23 @@ class AutoTest(ABC):
|
||||
self.check_test_syntax(test_file=os.path.realpath(file))
|
||||
|
||||
|
||||
def expect_command_ack(self, command):
|
||||
m = self.mav.recv_match(type='COMMAND_ACK', blocking=True, timeout=10)
|
||||
if m is None:
|
||||
raise NotAchievedException()
|
||||
if m.command != command:
|
||||
raise ValueError()
|
||||
if m.result != mavutil.mavlink.MAV_RESULT_ACCEPTED:
|
||||
raise NotAchievedException()
|
||||
|
||||
def poll_home_position(self):
|
||||
old = self.mav.messages.get("HOME_POSITION", None)
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
if self.get_sim_time() - tstart > 30:
|
||||
raise NotAchievedException("Failed to poll home position")
|
||||
self.mav.mav.command_long_send(
|
||||
1,
|
||||
1,
|
||||
mavutil.mavlink.MAV_CMD_GET_HOME_POSITION,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0)
|
||||
m = self.mav.recv_match(type='COMMAND_ACK', blocking=True, timeout=10)
|
||||
if m is None:
|
||||
continue
|
||||
if m.command != mavutil.mavlink.MAV_CMD_GET_HOME_POSITION:
|
||||
continue
|
||||
if m.result != 0:
|
||||
try:
|
||||
self.run_cmd(
|
||||
mavutil.mavlink.MAV_CMD_GET_HOME_POSITION,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0)
|
||||
except ValueError as e:
|
||||
continue
|
||||
break
|
||||
m = self.mav.messages.get("HOME_POSITION", None)
|
||||
@ -1711,20 +1743,15 @@ class AutoTest(ABC):
|
||||
new_y = orig_home.longitude + 2000
|
||||
new_z = orig_home.altitude + 300000 # 300 metres
|
||||
print("new home: %s %s %s" % (str(new_x), str(new_y), str(new_z)))
|
||||
self.mav.mav.command_int_send(1,
|
||||
1,
|
||||
mavutil.mavlink.MAV_FRAME_GLOBAL_INT,
|
||||
mavutil.mavlink.MAV_CMD_DO_SET_HOME,
|
||||
0, # current
|
||||
0, # autocontinue
|
||||
0, # p1,
|
||||
0, # p2,
|
||||
0, # p3,
|
||||
0, # p4,
|
||||
new_x,
|
||||
new_y,
|
||||
new_z/1000.0) # mm => m
|
||||
self.expect_command_ack(mavutil.mavlink.MAV_CMD_DO_SET_HOME)
|
||||
self.run_cmd_int(mavutil.mavlink.MAV_CMD_DO_SET_HOME,
|
||||
0, # p1,
|
||||
0, # p2,
|
||||
0, # p3,
|
||||
0, # p4,
|
||||
new_x,
|
||||
new_y,
|
||||
new_z/1000.0, # mm => m
|
||||
)
|
||||
|
||||
home = self.poll_home_position()
|
||||
self.progress("int-set home: %s" % str(home))
|
||||
@ -2126,6 +2153,9 @@ class AutoTest(ABC):
|
||||
def is_heli(self):
|
||||
return False
|
||||
|
||||
def is_tracker(self):
|
||||
return False
|
||||
|
||||
def initial_mode(self):
|
||||
'''return mode vehicle should start in with no RC inputs set'''
|
||||
return None
|
||||
|
||||
Loading…
Reference in New Issue
Block a user