mirror of https://github.com/ArduPilot/ardupilot
autotest: add test for THR_FAILSAFE==2 and throttle output
This commit is contained in:
parent
884450868e
commit
850227ffdb
|
@ -898,7 +898,7 @@ class AutoTestPlane(AutoTest):
|
||||||
raise NotAchievedException("Bad absalt (want=%f vs got=%f)" % (original_alt+30, x.alt_msl))
|
raise NotAchievedException("Bad absalt (want=%f vs got=%f)" % (original_alt+30, x.alt_msl))
|
||||||
self.fly_home_land_and_disarm()
|
self.fly_home_land_and_disarm()
|
||||||
|
|
||||||
def test_throttle_failsafe(self):
|
def ThrottleFailsafe(self):
|
||||||
self.change_mode('MANUAL')
|
self.change_mode('MANUAL')
|
||||||
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
|
m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
|
||||||
receiver_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_RC_RECEIVER
|
receiver_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_RC_RECEIVER
|
||||||
|
@ -1021,6 +1021,27 @@ class AutoTestPlane(AutoTest):
|
||||||
if ex is not None:
|
if ex is not None:
|
||||||
raise ex
|
raise ex
|
||||||
|
|
||||||
|
self.start_subtest("Not use RC throttle input when THR_FAILSAFE==2")
|
||||||
|
self.takeoff(100)
|
||||||
|
self.set_rc(3, 1800)
|
||||||
|
self.set_rc(1, 2000)
|
||||||
|
self.wait_attitude(desroll=45, timeout=1)
|
||||||
|
self.context_push()
|
||||||
|
self.set_parameters({
|
||||||
|
"THR_FAILSAFE": 2,
|
||||||
|
"SIM_RC_FAIL": 1, # no pulses
|
||||||
|
})
|
||||||
|
self.delay_sim_time(1)
|
||||||
|
self.wait_attitude(desroll=0, timeout=5)
|
||||||
|
self.assert_servo_channel_value(3, self.get_parameter("RC3_MIN"))
|
||||||
|
self.set_parameters({
|
||||||
|
"SIM_RC_FAIL": 0, # fix receiver
|
||||||
|
})
|
||||||
|
self.zero_throttle()
|
||||||
|
self.disarm_vehicle()
|
||||||
|
self.context_pop()
|
||||||
|
self.reboot_sitl()
|
||||||
|
|
||||||
def test_throttle_failsafe_fence(self):
|
def test_throttle_failsafe_fence(self):
|
||||||
fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
|
fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
|
||||||
|
|
||||||
|
@ -3504,7 +3525,7 @@ function'''
|
||||||
|
|
||||||
("ThrottleFailsafe",
|
("ThrottleFailsafe",
|
||||||
"Fly throttle failsafe",
|
"Fly throttle failsafe",
|
||||||
self.test_throttle_failsafe),
|
self.ThrottleFailsafe),
|
||||||
|
|
||||||
("NeedEKFToArm",
|
("NeedEKFToArm",
|
||||||
"Ensure we need EKF to be healthy to arm",
|
"Ensure we need EKF to be healthy to arm",
|
||||||
|
|
|
@ -5822,6 +5822,21 @@ class AutoTest(ABC):
|
||||||
if comparator(m_value, value):
|
if comparator(m_value, value):
|
||||||
return m_value
|
return m_value
|
||||||
|
|
||||||
|
def assert_servo_channel_value(self, channel, value, comparator=operator.eq):
|
||||||
|
"""assert channel value (default condition is equality)"""
|
||||||
|
channel_field = "servo%u_raw" % channel
|
||||||
|
opstring = ("%s" % comparator)[-3:-1]
|
||||||
|
m = self.assert_receive_message('SERVO_OUTPUT_RAW', timeout=1)
|
||||||
|
m_value = getattr(m, channel_field, None)
|
||||||
|
if m_value is None:
|
||||||
|
raise ValueError("message (%s) has no field %s" %
|
||||||
|
(str(m), channel_field))
|
||||||
|
self.progress("assert SERVO_OUTPUT_RAW.%s=%u %s %u" %
|
||||||
|
(channel_field, m_value, opstring, value))
|
||||||
|
if comparator(m_value, value):
|
||||||
|
return m_value
|
||||||
|
raise NotAchievedException("Wrong value")
|
||||||
|
|
||||||
def get_rc_channel_value(self, channel, timeout=2):
|
def get_rc_channel_value(self, channel, timeout=2):
|
||||||
"""wait for channel to hit value"""
|
"""wait for channel to hit value"""
|
||||||
channel_field = "chan%u_raw" % channel
|
channel_field = "chan%u_raw" % channel
|
||||||
|
@ -5854,6 +5869,16 @@ class AutoTest(ABC):
|
||||||
if value == m_value:
|
if value == m_value:
|
||||||
return
|
return
|
||||||
|
|
||||||
|
def assert_rc_channel_value(self, channel, value):
|
||||||
|
channel_field = "chan%u_raw" % channel
|
||||||
|
|
||||||
|
m_value = self.get_rc_channel_value(channel, timeout=1)
|
||||||
|
self.progress("RC_CHANNELS.%s=%u want=%u" %
|
||||||
|
(channel_field, m_value, value))
|
||||||
|
if value != m_value:
|
||||||
|
raise NotAchievedException("Expected %s to be %u got %u" %
|
||||||
|
(channel, value, m_value))
|
||||||
|
|
||||||
def wait_location(self,
|
def wait_location(self,
|
||||||
loc,
|
loc,
|
||||||
accuracy=5.0,
|
accuracy=5.0,
|
||||||
|
|
Loading…
Reference in New Issue