Tools: autotest: add quadplane output_motor_mask check

add comparator arg to wait_servo_channel_value and cleanup

Tools/autotest/common.py

@@ -11,6 +11,7 @@ import time
 import traceback
 import pexpect
 import fnmatch
+import operator
 
 from pymavlink import mavwp, mavutil
 from pysim import util, vehicleinfo
@@ -261,7 +262,7 @@ class AutoTest(ABC):
 
     def reboot_sitl(self):
         """Reboot SITL instance and wait it to reconnect."""
-        old_bootcount= self.get_parameter('STAT_BOOTCNT')
+        old_bootcount = self.get_parameter('STAT_BOOTCNT')
         self.mavproxy.send("reboot\n")
         tstart = time.time()
         while True:
@@ -1304,26 +1305,27 @@ class AutoTest(ABC):
                                           % (delta, distance))
         raise WaitDistanceTimeout("Failed to attain distance %u" % distance)
 
-    def wait_servo_channel_value(self, channel, value, timeout=2):
+    def wait_servo_channel_value(self, channel, value, timeout=2, comparator=operator.eq):
-        """wait for channel to hit value"""
+        """wait for channel value comparison (default condition is equality)"""
         channel_field = "servo%u_raw" % channel
+        opstring = ("%s" % comparator)[-3:-1]
         tstart = self.get_sim_time()
         while True:
             remaining = timeout - (self.get_sim_time_cached() - tstart)
             if remaining <= 0:
-                raise NotAchievedException("Channel never achieved value")
+                raise NotAchievedException("Channel value condition not met")
             m = self.mav.recv_match(type='SERVO_OUTPUT_RAW',
                                     blocking=True,
                                     timeout=remaining)
             if m is None:
                 continue
             m_value = getattr(m, channel_field, None)
-            self.progress("SERVO_OUTPUT_RAW.%s=%u want=%u" %
+            self.progress("want SERVO_OUTPUT_RAW.%s=%u %s %u" %
-                          (channel_field, m_value, value))
+                          (channel_field, m_value, opstring, value))
             if m_value is None:
                 raise ValueError("message (%s) has no field %s" %
                                  (str(m), channel_field))
-            if m_value == value:
+            if comparator(m_value, value):
                 return
 
     def wait_location(self,
@@ -1439,7 +1441,7 @@ class AutoTest(ABC):
 
     def wait_ready_to_arm(self, timeout=None, require_absolute=True):
         # wait for EKF checks to pass
-        self.progress("Waiting reading for arm")
+        self.progress("Waiting for ready to arm")
         return self.wait_ekf_happy(timeout=timeout,
                                    require_absolute=require_absolute)
 
@@ -1999,7 +2001,7 @@ class AutoTest(ABC):
         self.set_parameter("RC9_OPTION", 19)
         self.set_rc(9, 1500)
         self.reboot_sitl()
-        self.progress("Waiting reading for arm")
+        self.progress("Waiting for ready to arm")
         self.wait_ready_to_arm()
         self.progress("Test gripper with RC9_OPTION")
         self.progress("Releasing load")

Tools/autotest/quadplane.py

@@ -9,6 +9,7 @@ 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__))
@@ -103,6 +104,33 @@ class AutoTestQuadPlane(AutoTest):
     def set_autodisarm_delay(self, delay):
         self.set_parameter("LAND_DISARMDELAY", delay)
 
+    def test_motor_mask(self):
+        """Check operation of output_motor_mask"""
+        """copter tailsitters will add condition: or (int(self.get_parameter('Q_TAILSIT_MOTMX')) & 1)"""
+        if not(int(self.get_parameter('Q_TILT_MASK')) & 1):
+            self.progress("output_motor_mask not in use")
+            return
+        self.progress("Testing output_motor_mask")
+        self.wait_ready_to_arm()
+
+        """Default channel for Motor1 is 5"""
+        self.progress('Assert that SERVO5 is Motor1')
+        assert(33 == self.get_parameter('SERVO5_FUNCTION'))
+
+        modes = ('MANUAL', 'FBWA', 'QHOVER')
+        for mode in modes:
+            self.progress("Testing %s mode" % mode)
+            self.change_mode(mode)
+            self.arm_vehicle()
+            self.progress("Raising throttle")
+            self.set_rc(3, 1800)
+            self.progress("Waiting for Motor1 to start")
+            self.wait_servo_channel_value(5, 1100, comparator=operator.gt)
+
+            self.set_rc(3, 1000)
+            self.disarm_vehicle()
+            self.wait_ready_to_arm()
+
     def fly_mission(self, filename, fence, height_accuracy=-1):
         """Fly a mission from a file."""
         self.progress("Flying mission %s" % filename)
@@ -158,7 +186,7 @@ class AutoTestQuadPlane(AutoTest):
         self.mav.motors_disarmed_wait()
 
     def default_mode(self):
-        return "FBWA"
+        return "MANUAL"
 
     def disabled_tests(self):
         return {
@@ -175,6 +203,8 @@ class AutoTestQuadPlane(AutoTest):
         ret.extend([
             ("ArmFeatures", "Arm features", self.test_arm_feature),
 
+            ("TestMotorMask", "Test output_motor_mask", self.test_motor_mask),
+
             ("QAutoTune", "Fly QAUTOTUNE mode", self.fly_qautotune),
 
             ("Mission", "Dalby Mission",