zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Tools: autotest: add test for throttle failsafe

mission-4.1.18
Peter Barker 7 years ago committed by Peter Barker
parent
17589ae3b9
commit
e3368e193d
2 changed files with 130 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 108
      Tools/autotest/arduplane.py
  2. 22
      Tools/autotest/common.py

108
Tools/autotest/arduplane.py
Unescape View File

@ -681,6 +681,110 @@ class AutoTestPlane(AutoTest): @@ -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): @@ -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),

22
Tools/autotest/common.py
Unescape View File

@ -1328,6 +1328,28 @@ class AutoTest(ABC): @@ -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,

Write Preview
Loading…
Cancel
Save