Tools: autotest: add test suite for Tracker

Tools/autotest/antennatracker.py

@@ -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

Tools/autotest/autotest.py

@@ -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',

Tools/autotest/common.py

@@ -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