From e3368e193dcbe0a8850ac167612d2922cef6240b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 21 Aug 2018 10:55:59 +1000 Subject: [PATCH] Tools: autotest: add test for throttle failsafe --- Tools/autotest/arduplane.py | 108 ++++++++++++++++++++++++++++++++++++ Tools/autotest/common.py | 22 ++++++++ 2 files changed, 130 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 80b5d2fad6..66b66b8ec9 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -681,6 +681,110 @@ class AutoTestPlane(AutoTest): if x is None: raise NotAchievedException("No CAMERA_FEEDBACK message received") + def test_throttle_failsafe(self): + self.change_mode('MANUAL') + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + receiver_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_RC_RECEIVER + self.progress("Testing receiver enabled") + if (not (m.onboard_control_sensors_enabled & receiver_bit)): + raise PreconditionFailedException() + self.progress("Testing receiver present") + if (not (m.onboard_control_sensors_present & receiver_bit)): + raise PreconditionFailedException() + self.progress("Testing receiver health") + if (not (m.onboard_control_sensors_health & receiver_bit)): + raise PreconditionFailedException() + + self.progress("Ensure we know original throttle value") + self.wait_rc_channel_value(3, 1000) + + self.set_parameter("THR_FS_VALUE", 960) + self.progress("Failing receiver (throttle-to-950)") + self.set_parameter("SIM_RC_FAIL", 2) # throttle-to-950 + self.wait_mode('CIRCLE') # short failsafe + self.wait_mode('RTL') # long failsafe + self.progress("Ensure we've had our throttle squashed to 950") + self.wait_rc_channel_value(3, 950) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + self.progress("Testing receiver enabled") + if (not (m.onboard_control_sensors_enabled & receiver_bit)): + raise NotAchievedException("Receiver not enabled") + self.progress("Testing receiver present") + if (not (m.onboard_control_sensors_present & receiver_bit)): + raise NotAchievedException("Receiver not present") + self.progress("Testing receiver health") + if (m.onboard_control_sensors_health & receiver_bit): + raise NotAchievedException("Sensor healthy when it shouldn't be") + self.set_parameter("SIM_RC_FAIL", 0) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Testing receiver enabled") + if (not (m.onboard_control_sensors_enabled & receiver_bit)): + raise NotAchievedException("Receiver not enabled") + self.progress("Testing receiver present") + if (not (m.onboard_control_sensors_present & receiver_bit)): + raise NotAchievedException("Receiver not present") + self.progress("Testing receiver health") + if (not (m.onboard_control_sensors_health & receiver_bit)): + raise NotAchievedException("Receiver not healthy") + self.change_mode('MANUAL') + + self.progress("Failing receiver (no-pulses)") + self.set_parameter("SIM_RC_FAIL", 1) # no-pulses + self.wait_mode('CIRCLE') # short failsafe + self.wait_mode('RTL') # long failsafe + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + print("%s" % str(m)) + self.progress("Testing receiver enabled") + if (not (m.onboard_control_sensors_enabled & receiver_bit)): + raise NotAchievedException("Receiver not enabled") + self.progress("Testing receiver present") + if (not (m.onboard_control_sensors_present & receiver_bit)): + raise NotAchievedException("Receiver not present") + self.progress("Testing receiver health") + if (m.onboard_control_sensors_health & receiver_bit): + raise NotAchievedException("Sensor healthy when it shouldn't be") + self.set_parameter("SIM_RC_FAIL", 0) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + self.progress("Testing receiver enabled") + if (not (m.onboard_control_sensors_enabled & receiver_bit)): + raise NotAchievedException("Receiver not enabled") + self.progress("Testing receiver present") + if (not (m.onboard_control_sensors_present & receiver_bit)): + raise NotAchievedException("Receiver not present") + self.progress("Testing receiver health") + if (not (m.onboard_control_sensors_health & receiver_bit)): + raise NotAchievedException("Receiver not healthy") + self.change_mode('MANUAL') + def test_gripper_mission(self): self.context_push() ex = None @@ -776,6 +880,10 @@ class AutoTestPlane(AutoTest): ("TestRCRelay", "Test Relay RC Channel Option", self.test_rc_relay), + ("ThrottleFailsafe", + "Fly throttle failsafe", + self.test_throttle_failsafe), + ("TestFlaps", "Flaps", self.fly_flaps), ("ArmFeatures", "Arm features", self.test_arm_feature), diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 7b4de22f94..c7e5e0526d 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -1328,6 +1328,28 @@ class AutoTest(ABC): if comparator(m_value, value): return + def wait_rc_channel_value(self, channel, value, timeout=2): + """wait for channel to hit value""" + channel_field = "chan%u_raw" % channel + tstart = self.get_sim_time() + while True: + remaining = timeout - (self.get_sim_time_cached() - tstart) + if remaining <= 0: + raise NotAchievedException("Channel never achieved value") + m = self.mav.recv_match(type='RC_CHANNELS', + blocking=True, + timeout=remaining) + if m is None: + continue + m_value = getattr(m, channel_field) + self.progress("RC_CHANNELS.%s=%u want=%u" % + (channel_field, m_value, value)) + if m_value is None: + raise ValueError("message (%s) has no field %s" % + (str(m), channel_field)) + if m_value == value: + return + def wait_location(self, loc, accuracy=5,