Skip to content

Commit a8e3904

Browse files
tridgetridge
committed
autotest: test tilt servo failure detection in SITL
Run 17 deterministic regression cases in ArduPilot Lua with native uint32_t timestamps, and exercise the real telemetry and motor-output bindings through DroneCAN in an armed, stationary QuadPlane. Check individual and simultaneous faults, telemetry loss and recovery, override expiry during callback gaps, and resumption without a shutdown pulse. Register both tests in the Plane CI suite.
1 parent c27ced8 commit a8e3904

1 file changed

Lines changed: 158 additions & 0 deletions

File tree

‎Tools/autotest/quadplane.py‎

Lines changed: 158 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,8 @@
88
import math
99
import operator
1010
import os
11+
import socket
12+
import struct
1113
import tempfile
1214

1315
import numpy
@@ -3848,6 +3850,160 @@ def process(self, mav, m):
38483850

38493851
self.do_RTL()
38503852

3853+
def ServoFailDetectRegression(self):
3854+
'''run servo failure regression cases in ArduPilot Lua'''
3855+
self.context_collect('STATUSTEXT')
3856+
self.install_script_module_context(self.script_applet_source_path("ServoFailDetect.lua"),
3857+
"ServoFailDetect.lua")
3858+
self.install_test_script_context("ServoFailDetect_test.lua")
3859+
self.set_parameters({
3860+
"SCR_ENABLE": 1,
3861+
"SCR_HEAP_SIZE": 600000,
3862+
# Each case runs many simulated applet updates in one callback.
3863+
# The CAN integration test uses the normal 10000 instruction limit.
3864+
"SCR_VM_I_COUNT": 1000000,
3865+
})
3866+
self.reboot_sitl()
3867+
self.wait_text("Passed 17 ServoFailDetect regression tests", check_context=True, timeout=60)
3868+
3869+
def ServoFailDetect(self):
3870+
'''test servo failure detection with CAN feedback and delayed Lua callbacks'''
3871+
import dronecan
3872+
3873+
self.context_set_speedup(3)
3874+
self.context_collect('STATUSTEXT')
3875+
self.install_script_module_context(self.script_applet_source_path("ServoFailDetect.lua"),
3876+
"ServoFailDetect.lua")
3877+
self.install_test_script_context("ServoFailDetect_sitl.lua")
3878+
# Keep the applet's supplied table unchanged. This is an armed-at-idle
3879+
# test; map Motor1/2 to its outputs 11/13 instead of the model's 5/6.
3880+
self.set_parameters({
3881+
"SCR_ENABLE": 1,
3882+
"SCR_HEAP_SIZE": 300000,
3883+
"SCR_VM_I_COUNT": 10000,
3884+
"SCR_USER1": 0,
3885+
"CAN_P1_DRIVER": 1,
3886+
"CAN_D1_PROTOCOL": 1,
3887+
"SIM_CAN_TYPE1": 1,
3888+
"Q_TILT_ENABLE": 1,
3889+
"Q_TILT_TYPE": 2,
3890+
"Q_TILT_MASK": 3,
3891+
"Q_M_PWM_MIN": 1000,
3892+
"Q_M_PWM_MAX": 2000,
3893+
"SERVO5_FUNCTION": 0,
3894+
"SERVO6_FUNCTION": 0,
3895+
"SERVO11_FUNCTION": 33,
3896+
"SERVO13_FUNCTION": 34,
3897+
"SERVO15_FUNCTION": 75,
3898+
"SERVO16_FUNCTION": 76,
3899+
"SERVO15_MIN": 1000,
3900+
"SERVO15_MAX": 2000,
3901+
"SERVO16_MIN": 1000,
3902+
"SERVO16_MAX": 2000,
3903+
"FLTMODE_CH": 0,
3904+
"LOG_DISARMED": 1,
3905+
})
3906+
self.reboot_sitl()
3907+
self.context_set_message_rate_hz('SERVO_OUTPUT_RAW', 50)
3908+
self.wait_text("SRVFAIL: Left Tilt servo 15 motor 11", check_context=True)
3909+
self.wait_text("SRVFAIL: Right Tilt servo 16 motor 13", check_context=True)
3910+
3911+
faulty = set()
3912+
missing = set()
3913+
enforce_motor_check = False
3914+
transfer_id = 0
3915+
# Use the same multicast framing as AP_HAL_SITL/CAN_Multicast.cpp.
3916+
# Sending in this process avoids a separately scheduled Python CAN
3917+
# node, and respects the test runner's interface and port isolation.
3918+
address = os.environ.get('SITL_MULTICAST_IF_ADDR') or '127.0.0.1'
3919+
port = int(os.environ.get('SITL_CAN_MCAST_PORT') or 57732)
3920+
with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as can_socket:
3921+
can_socket.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(address))
3922+
3923+
def feedback(mav, msg):
3924+
nonlocal transfer_id
3925+
if msg.get_type() == 'STATUSTEXT' and msg.text.startswith('Lua:'):
3926+
raise NotAchievedException(msg.text)
3927+
if msg.get_type() != 'SERVO_OUTPUT_RAW' or msg.port != 0:
3928+
return
3929+
if enforce_motor_check and msg.servo11_raw == 1000 and msg.servo13_raw == 1000:
3930+
raise NotAchievedException("Both tilt motors stopped")
3931+
for channel in (15, 16):
3932+
if channel in missing:
3933+
continue
3934+
pwm = getattr(msg, f'servo{channel}_raw')
3935+
position = (pwm - 1000) * 115 / 1000 - 57.5
3936+
if channel in faulty:
3937+
position += 120
3938+
status = dronecan.uavcan.equipment.actuator.Status(
3939+
actuator_id=channel, position=math.radians(position),
3940+
force=0, speed=0, power_rating_pct=127)
3941+
transfer = dronecan.transport.Transfer(
3942+
transfer_id=transfer_id, source_node_id=121, payload=status)
3943+
transfer_id = (transfer_id + 1) % 32
3944+
for frame in transfer.to_frames():
3945+
body = struct.pack('<HI', 0, frame.message_id | (1 << 31)) + bytes(frame.bytes)
3946+
packet = struct.pack('<HH', 0x2934, dronecan.dsdl.common.crc16_from_bytes(body)) + body
3947+
can_socket.sendto(packet, ('239.65.82.0', port))
3948+
3949+
def check_outputs(left_cut, right_cut, duration=5):
3950+
start = self.get_sim_time()
3951+
checked = 0
3952+
while self.get_sim_time_cached() - start < duration:
3953+
msg = self.assert_receive_message('SERVO_OUTPUT_RAW')
3954+
if msg.port != 0 or self.get_sim_time_cached() - start < duration - 1:
3955+
continue
3956+
for channel, cut in ((11, left_cut), (13, right_cut)):
3957+
pwm = getattr(msg, f'servo{channel}_raw')
3958+
if (cut and pwm != 1000) or (not cut and pwm <= 1000):
3959+
raise NotAchievedException(f"SERVO{channel}={pwm}, expected shutdown={cut}")
3960+
checked += 1
3961+
if checked < 10:
3962+
raise NotAchievedException("Insufficient motor output samples")
3963+
3964+
self.install_message_hook(feedback)
3965+
try:
3966+
self.change_mode('QSTABILIZE')
3967+
self.set_rc(3, 1000)
3968+
self.wait_ready_to_arm()
3969+
self.arm_vehicle()
3970+
check_outputs(False, False)
3971+
enforce_motor_check = True
3972+
3973+
for name, bad, lost, expected in [
3974+
('left position error', {15}, set(), (True, False)),
3975+
('both position errors', {15, 16}, set(), (False, False)),
3976+
('right after left recovery', {16}, set(), (False, True)),
3977+
('position recovery', set(), set(), (False, False)),
3978+
('left feedback loss', set(), {15}, (True, False)),
3979+
('both feedback loss', set(), {15, 16}, (False, False)),
3980+
('feedback recovery', set(), set(), (False, False)),
3981+
('left fault before scheduling gap', {15}, set(), (True, False)),
3982+
]:
3983+
self.start_subtest(name)
3984+
faulty, missing = bad, lost
3985+
check_outputs(*expected)
3986+
3987+
self.start_subtest('override expires while applet is paused')
3988+
self.set_parameter('SCR_USER1', 2)
3989+
check_outputs(False, False, duration=2)
3990+
self.start_subtest('resume after five-second gap')
3991+
faulty.clear()
3992+
self.set_parameter('SCR_USER1', 1)
3993+
check_outputs(False, False, duration=9)
3994+
self.wait_text('SFD test: resumed without shutdown pulse', check_context=True)
3995+
3996+
self.start_subtest('failure detection resumes after scheduling gap')
3997+
self.set_parameter('SCR_USER1', 0)
3998+
faulty.add(16)
3999+
check_outputs(False, True)
4000+
faulty.clear()
4001+
check_outputs(False, False)
4002+
enforce_motor_check = False
4003+
self.disarm_vehicle()
4004+
finally:
4005+
self.remove_message_hook(feedback)
4006+
38514007
def CircuitStatusScript(self):
38524008
'''test CircuitStatus lua driver against a CAN periph'''
38534009
self.context_collect('STATUSTEXT')
@@ -4508,6 +4664,8 @@ def tests1b(self):
45084664
self.FenceRelativeToAMSLMinAlt,
45094665
self.FenceRelativeToTerrainMaxAlt,
45104666
self.FenceRelativeToTerrainMinAlt,
4667+
self.ServoFailDetectRegression,
4668+
self.ServoFailDetect,
45114669
# anomalous: ~9 minutes on CI against ~20s on a desktop; to be investigated
45124670
self.CircuitStatusScript,
45134671
])

0 commit comments

Comments
 (0)