mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
autotest: create own RC thread to handle setting of RC overrides
autotest: prevent method-on-undef problem in case of early test failure autotest: plumb MAVProxy rc input back in for switch test autotest: close off rc thread in case of timeout
This commit is contained in:
Peter Barker
Peter Barker
parent
7b61a4d8c2
commit
cd8a393ac2
@ -178,10 +178,12 @@ class AutoTestCopter(AutoTest):
|
||||
# Takeoff, climb to given altitude, and fly east for 10 seconds
|
||||
def takeoffAndMoveAway(self, dAlt=50, dDist=50):
|
||||
self.progress("Centering sticks")
|
||||
self.set_rc(1, 1500)
|
||||
self.set_rc(2, 1500)
|
||||
self.set_rc(3, 1000)
|
||||
self.set_rc(4, 1500)
|
||||
self.set_rc_from_map({
|
||||
1: 1500,
|
||||
2: 1500,
|
||||
3: 1000,
|
||||
4: 1500,
|
||||
})
|
||||
self.takeoff(alt_min=dAlt)
|
||||
self.change_mode("ALT_HOLD")
|
||||
|
||||
@ -265,11 +267,15 @@ class AutoTestCopter(AutoTest):
|
||||
self.watch_altitude_maintained(9, 11, timeout=5)
|
||||
# feed in full elevator and aileron input and make sure we
|
||||
# retain altitude:
|
||||
self.set_rc(1, 1000)
|
||||
self.set_rc(2, 1000)
|
||||
self.set_rc_from_map({
|
||||
1: 1000,
|
||||
2: 1000,
|
||||
})
|
||||
self.watch_altitude_maintained(9, 11, timeout=5)
|
||||
self.set_rc(1, 1500)
|
||||
self.set_rc(2, 1500)
|
||||
self.set_rc_from_map({
|
||||
1: 1500,
|
||||
2: 1500,
|
||||
})
|
||||
self.do_RTL()
|
||||
|
||||
def fly_to_origin(self, final_alt=10):
|
||||
@ -318,10 +324,12 @@ class AutoTestCopter(AutoTest):
|
||||
tstart = self.get_sim_time()
|
||||
|
||||
# ensure all sticks in the middle
|
||||
self.set_rc(1, 1500)
|
||||
self.set_rc(2, 1500)
|
||||
self.set_rc(3, 1500)
|
||||
self.set_rc(4, 1500)
|
||||
self.set_rc_from_map({
|
||||
1: 1500,
|
||||
2: 1500,
|
||||
3: 1500,
|
||||
4: 1500,
|
||||
})
|
||||
|
||||
# switch to loiter mode temporarily to stop us from rising
|
||||
self.change_mode('LOITER')
|
||||
@ -384,8 +392,7 @@ class AutoTestCopter(AutoTest):
|
||||
|
||||
# descend to 10m
|
||||
self.progress("Descend to 10m in Loiter")
|
||||
self.mavproxy.send('switch 5\n') # loiter mode
|
||||
self.wait_mode('LOITER')
|
||||
self.change_mode('LOITER')
|
||||
self.set_rc(3, 1300)
|
||||
time_left = timeout - (self.get_sim_time() - tstart)
|
||||
self.progress("timeleft = %u" % time_left)
|
||||
@ -462,15 +469,13 @@ class AutoTestCopter(AutoTest):
|
||||
|
||||
num_wp = self.load_mission("copter_loiter_to_alt.txt")
|
||||
|
||||
self.mavproxy.send('switch 5\n')
|
||||
self.wait_mode('LOITER')
|
||||
self.change_mode('LOITER')
|
||||
|
||||
self.wait_ready_to_arm()
|
||||
|
||||
self.arm_vehicle()
|
||||
|
||||
self.mavproxy.send('mode auto\n')
|
||||
self.wait_mode('AUTO')
|
||||
self.change_mode('AUTO')
|
||||
|
||||
self.set_rc(3, 1550)
|
||||
|
||||
@ -1273,8 +1278,6 @@ class AutoTestCopter(AutoTest):
|
||||
def fly_alt_max_fence_test(self):
|
||||
self.takeoff(10, mode="LOITER")
|
||||
"""Hold loiter position."""
|
||||
self.mavproxy.send('switch 5\n') # loiter mode
|
||||
self.wait_mode('LOITER')
|
||||
|
||||
# enable fence, disable avoidance
|
||||
self.set_parameter("FENCE_ENABLE", 1)
|
||||
@ -1294,8 +1297,10 @@ class AutoTestCopter(AutoTest):
|
||||
self.wait_distance(20)
|
||||
|
||||
# stop flying forward and start flying up:
|
||||
self.set_rc(2, 1500)
|
||||
self.set_rc(3, 1800)
|
||||
self.set_rc_from_map({
|
||||
2: 1500,
|
||||
3: 1800,
|
||||
})
|
||||
|
||||
# wait for fence to trigger
|
||||
self.wait_mode('RTL', timeout=120)
|
||||
@ -1451,8 +1456,7 @@ class AutoTestCopter(AutoTest):
|
||||
self.arm_vehicle()
|
||||
|
||||
# switch into AUTO mode and raise throttle
|
||||
self.mavproxy.send('switch 4\n') # auto mode
|
||||
self.wait_mode('AUTO')
|
||||
self.change_mode('AUTO')
|
||||
self.set_rc(3, 1500)
|
||||
|
||||
# wait until 100m from home
|
||||
@ -1512,15 +1516,13 @@ class AutoTestCopter(AutoTest):
|
||||
def fly_simple(self, side=50):
|
||||
self.takeoff(10, mode="LOITER")
|
||||
# hold position in loiter
|
||||
self.mavproxy.send('switch 5\n') # loiter mode
|
||||
self.wait_mode('LOITER')
|
||||
self.change_mode('LOITER')
|
||||
|
||||
# set SIMPLE mode for all flight modes
|
||||
self.set_parameter("SIMPLE", 63)
|
||||
|
||||
# switch to stabilize mode
|
||||
self.mavproxy.send('switch 6\n')
|
||||
self.wait_mode('STABILIZE')
|
||||
self.change_mode('STABILIZE')
|
||||
self.set_rc(3, 1545)
|
||||
|
||||
# fly south 50m
|
||||
@ -1621,8 +1623,7 @@ class AutoTestCopter(AutoTest):
|
||||
self.set_rc(2, 1500)
|
||||
|
||||
# set CIRCLE mode
|
||||
self.mavproxy.send('switch 1\n') # circle mode
|
||||
self.wait_mode('CIRCLE')
|
||||
self.change_mode('CIRCLE')
|
||||
|
||||
# wait
|
||||
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
@ -2180,9 +2181,7 @@ class AutoTestCopter(AutoTest):
|
||||
|
||||
self.customise_SITL_commandline(["--uartF=sim:vicon:"])
|
||||
self.progress("Waiting for location")
|
||||
self.mavproxy.send('switch 6\n') # stabilize mode
|
||||
self.wait_heartbeat()
|
||||
self.wait_mode('STABILIZE')
|
||||
self.change_mode('LOITER')
|
||||
self.wait_ready_to_arm()
|
||||
|
||||
old_pos = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
|
||||
@ -2235,7 +2234,7 @@ class AutoTestCopter(AutoTest):
|
||||
# recenter controls:
|
||||
self.set_rc(1, 1500)
|
||||
self.progress("# Enter RTL")
|
||||
self.mavproxy.send('switch 3\n')
|
||||
self.change_mode('RTL')
|
||||
self.set_rc(3, 1500)
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
@ -3126,11 +3125,6 @@ class AutoTestCopter(AutoTest):
|
||||
self.set_rc(fltmode_ch, pwm)
|
||||
self.wait_mode(name)
|
||||
|
||||
self.mavproxy.send('switch 6\n')
|
||||
self.wait_mode("BRAKE")
|
||||
self.mavproxy.send('switch 5\n')
|
||||
self.wait_mode("ACRO")
|
||||
|
||||
except Exception as e:
|
||||
self.progress("Exception caught: %s" % (
|
||||
self.get_exception_stacktrace(e)))
|
||||
@ -3464,16 +3458,12 @@ class AutoTestCopter(AutoTest):
|
||||
self.progress("Waiting for location")
|
||||
self.mav.location()
|
||||
self.zero_throttle()
|
||||
self.mavproxy.send('switch 6\n') # stabilize mode
|
||||
self.wait_heartbeat()
|
||||
self.wait_mode('STABILIZE')
|
||||
self.change_mode('STABILIZE')
|
||||
self.wait_ready_to_arm()
|
||||
|
||||
self.arm_vehicle()
|
||||
|
||||
self.mavproxy.send('switch 4\n') # auto mode
|
||||
self.wait_heartbeat()
|
||||
self.wait_mode('AUTO')
|
||||
self.change_mode('AUTO')
|
||||
|
||||
self.set_rc(3, 1500)
|
||||
self.wait_text("Gripper load releas", timeout=90)
|
||||
@ -3812,17 +3802,19 @@ class AutoTestCopter(AutoTest):
|
||||
self.set_parameter('MNT_RC_IN_PAN', 13)
|
||||
self.progress("Testing RC angular control")
|
||||
# default RC min=1100 max=1900
|
||||
self.set_rc(11, 1500)
|
||||
self.set_rc(12, 1500)
|
||||
self.set_rc(13, 1500)
|
||||
self.set_rc_from_map({
|
||||
11: 1500,
|
||||
12: 1500,
|
||||
13: 1500,
|
||||
})
|
||||
self.test_mount_pitch(0, 1)
|
||||
self.progress("Testing RC input down 1/4 of its range in the output, should be down 1/4 range in output")
|
||||
rc12_in = 1400.0
|
||||
rc12_min = 1100.0 # default
|
||||
rc12_max = 1900.0 # default
|
||||
rc12_in = 1400
|
||||
rc12_min = 1100 # default
|
||||
rc12_max = 1900 # default
|
||||
angmin_tilt = -45.0 # default
|
||||
angmax_tilt = 45.0 # default
|
||||
expected_pitch = ((rc12_in-rc12_min)/(rc12_max-rc12_min) * (angmax_tilt-angmin_tilt)) + angmin_tilt
|
||||
expected_pitch = (float(rc12_in-rc12_min)/float(rc12_max-rc12_min) * (angmax_tilt-angmin_tilt)) + angmin_tilt
|
||||
self.progress("expected mount pitch: %f" % expected_pitch)
|
||||
if expected_pitch != -11.25:
|
||||
raise NotAchievedException("Calculation wrong - defaults changed?!")
|
||||
@ -3830,9 +3822,11 @@ class AutoTestCopter(AutoTest):
|
||||
self.test_mount_pitch(-11.25, 0.01)
|
||||
self.set_rc(12, 1800)
|
||||
self.test_mount_pitch(33.75, 0.01)
|
||||
self.set_rc(11, 1500)
|
||||
self.set_rc(12, 1500)
|
||||
self.set_rc(13, 1500)
|
||||
self.set_rc_from_map({
|
||||
11: 1500,
|
||||
12: 1500,
|
||||
13: 1500,
|
||||
})
|
||||
|
||||
try:
|
||||
self.progress("Issue https://discuss.ardupilot.org/t/gimbal-limits-with-storm32-backend-mavlink-not-applied-correctly/51438")
|
||||
@ -4672,9 +4666,7 @@ class AutoTestCopter(AutoTest):
|
||||
self.progress("Waiting for location")
|
||||
self.mav.location()
|
||||
self.zero_throttle()
|
||||
self.mavproxy.send('switch 6\n') # stabilize mode
|
||||
self.wait_heartbeat()
|
||||
self.wait_mode('STABILIZE')
|
||||
self.change_mode('STABILIZE')
|
||||
self.wait_ready_to_arm()
|
||||
|
||||
# we should be doing precision loiter at this point
|
||||
@ -6296,8 +6288,7 @@ class AutoTestHeli(AutoTestCopter):
|
||||
# fly_avc_test - fly AVC mission
|
||||
def fly_avc_test(self):
|
||||
# Arm
|
||||
self.mavproxy.send('switch 6\n') # stabilize mode
|
||||
self.wait_mode('STABILIZE')
|
||||
self.change_mode('STABILIZE')
|
||||
self.wait_ready_to_arm()
|
||||
|
||||
self.arm_vehicle()
|
||||
@ -6318,8 +6309,7 @@ class AutoTestHeli(AutoTestCopter):
|
||||
self.delay_sim_time(20)
|
||||
|
||||
# switch into AUTO mode and raise throttle
|
||||
self.mavproxy.send('switch 4\n') # auto mode
|
||||
self.wait_mode('AUTO')
|
||||
self.change_mode('AUTO')
|
||||
self.set_rc(3, 1500)
|
||||
|
||||
# fly the mission
|
||||
|
||||
@ -118,8 +118,7 @@ class AutoTestPlane(AutoTest):
|
||||
|
||||
def fly_left_circuit(self):
|
||||
"""Fly a left circuit, 200m on a side."""
|
||||
self.mavproxy.send('switch 4\n')
|
||||
self.wait_mode('FBWA')
|
||||
self.change_mode('FBWA')
|
||||
self.set_rc(3, 2000)
|
||||
self.wait_level_flight()
|
||||
|
||||
@ -138,8 +137,7 @@ class AutoTestPlane(AutoTest):
|
||||
def fly_RTL(self):
|
||||
"""Fly to home."""
|
||||
self.progress("Flying home in RTL")
|
||||
self.mavproxy.send('switch 2\n')
|
||||
self.wait_mode('RTL')
|
||||
self.change_mode('RTL')
|
||||
self.wait_location(self.homeloc,
|
||||
accuracy=120,
|
||||
target_altitude=self.homeloc.alt+100,
|
||||
@ -264,8 +262,7 @@ class AutoTestPlane(AutoTest):
|
||||
self.change_altitude(self.homeloc.alt+300)
|
||||
|
||||
# fly the roll in manual
|
||||
self.mavproxy.send('switch 6\n')
|
||||
self.wait_mode('MANUAL')
|
||||
self.change_mode('MANUAL')
|
||||
|
||||
while count > 0:
|
||||
self.progress("Starting roll")
|
||||
@ -281,8 +278,7 @@ class AutoTestPlane(AutoTest):
|
||||
|
||||
# back to FBWA
|
||||
self.set_rc(1, 1500)
|
||||
self.mavproxy.send('switch 4\n')
|
||||
self.wait_mode('FBWA')
|
||||
self.change_mode('FBWA')
|
||||
self.set_rc(3, 1700)
|
||||
return self.wait_level_flight()
|
||||
|
||||
@ -292,8 +288,7 @@ class AutoTestPlane(AutoTest):
|
||||
self.set_rc(3, 2000)
|
||||
self.change_altitude(self.homeloc.alt+300)
|
||||
# fly the loop in manual
|
||||
self.mavproxy.send('switch 6\n')
|
||||
self.wait_mode('MANUAL')
|
||||
self.change_mode('MANUAL')
|
||||
|
||||
while count > 0:
|
||||
self.progress("Starting loop")
|
||||
@ -304,8 +299,7 @@ class AutoTestPlane(AutoTest):
|
||||
|
||||
# back to FBWA
|
||||
self.set_rc(2, 1500)
|
||||
self.mavproxy.send('switch 4\n')
|
||||
self.wait_mode('FBWA')
|
||||
self.change_mode('FBWA')
|
||||
self.set_rc(3, 1700)
|
||||
return self.wait_level_flight()
|
||||
|
||||
@ -536,8 +530,7 @@ class AutoTestPlane(AutoTest):
|
||||
"""Fly a mission from a file."""
|
||||
self.progress("Flying mission %s" % filename)
|
||||
num_wp = self.load_mission(filename)-1
|
||||
self.mavproxy.send('switch 1\n') # auto mode
|
||||
self.wait_mode('AUTO')
|
||||
self.change_mode('AUTO')
|
||||
self.wait_waypoint(1, num_wp, max_dist=60)
|
||||
self.wait_groundspeed(0, 0.5, timeout=mission_timeout)
|
||||
self.mavproxy.expect("Auto disarmed")
|
||||
@ -759,8 +752,7 @@ class AutoTestPlane(AutoTest):
|
||||
self.progress("Flying mission %s" % filename)
|
||||
self.load_mission(filename)
|
||||
self.mavproxy.send('wp set 1\n')
|
||||
self.mavproxy.send('switch 1\n') # auto mode
|
||||
self.wait_mode('AUTO')
|
||||
self.change_mode('AUTO')
|
||||
self.wait_ready_to_arm()
|
||||
self.arm_vehicle()
|
||||
last_mission_current_msg = 0
|
||||
@ -1505,11 +1497,8 @@ class AutoTestPlane(AutoTest):
|
||||
|
||||
|
||||
def test_setting_modes_via_auxswitches(self):
|
||||
self.set_parameter("FLTMODE5", 1)
|
||||
self.mavproxy.send('switch 1\n') # random mode
|
||||
self.wait_heartbeat()
|
||||
self.change_mode('MANUAL')
|
||||
self.mavproxy.send('switch 5\n') # acro mode
|
||||
self.set_parameter("FLTMODE1", 1) # circle
|
||||
self.set_rc(8, 950)
|
||||
self.wait_mode("CIRCLE")
|
||||
self.set_rc(9, 1000)
|
||||
self.set_rc(10, 1000)
|
||||
@ -1720,7 +1709,7 @@ class AutoTestPlane(AutoTest):
|
||||
self.progress("Initial throttle: %u" % initial_throttle)
|
||||
# pitch down, ensure throttle decreases:
|
||||
rc2_max = self.get_parameter("RC2_MAX")
|
||||
self.set_rc(2, rc2_max)
|
||||
self.set_rc(2, int(rc2_max))
|
||||
tstart = self.get_sim_time()
|
||||
while True:
|
||||
now = self.get_sim_time_cached()
|
||||
@ -1784,10 +1773,10 @@ class AutoTestPlane(AutoTest):
|
||||
if rc_chan==0:
|
||||
raise NotAchievedException("Did not find soaring enable channel option.")
|
||||
|
||||
self.send_set_rc(rc_chan, 1900)
|
||||
|
||||
# Use trim airspeed.
|
||||
self.send_set_rc(3, 1500)
|
||||
self.set_rc_from_map({
|
||||
rc_chan: 1900,
|
||||
3: 1500, # Use trim airspeed.
|
||||
});
|
||||
|
||||
# Wait to detect thermal
|
||||
self.progress("Waiting for thermal")
|
||||
@ -1846,14 +1835,14 @@ class AutoTestPlane(AutoTest):
|
||||
self.set_parameter("SIM_THML_SCENARI", 1)
|
||||
|
||||
# Disable soaring using RC channel.
|
||||
self.send_set_rc(rc_chan, 1100)
|
||||
self.set_rc(rc_chan, 1100)
|
||||
|
||||
# Wait to get back to waypoint before thermal.
|
||||
self.progress("Waiting to get back to position")
|
||||
self.wait_current_waypoint(3,timeout=1200)
|
||||
|
||||
# Enable soaring with mode changes suppressed)
|
||||
self.send_set_rc(rc_chan, 1500)
|
||||
self.set_rc(rc_chan, 1500)
|
||||
|
||||
# Make sure this causes throttle down.
|
||||
self.wait_servo_channel_value(3, 1200, timeout=2, comparator=operator.lt)
|
||||
|
||||
@ -16,6 +16,7 @@ import subprocess
|
||||
import sys
|
||||
import time
|
||||
import traceback
|
||||
import threading
|
||||
from distutils.dir_util import copy_tree
|
||||
|
||||
import rover
|
||||
@ -31,6 +32,7 @@ from pysim import util
|
||||
from pymavlink import mavutil
|
||||
from pymavlink.generator import mavtemplate
|
||||
|
||||
tester = None
|
||||
|
||||
def buildlogs_dirpath():
|
||||
return os.getenv("BUILDLOGS", util.reltopdir("../buildlogs"))
|
||||
@ -237,9 +239,14 @@ def test_prerequisites():
|
||||
|
||||
def alarm_handler(signum, frame):
|
||||
"""Handle test timeout."""
|
||||
global results, opts
|
||||
global results, opts, tester
|
||||
try:
|
||||
print("Alarm handler called")
|
||||
if tester is not None:
|
||||
if tester.rc_thread is not None:
|
||||
tester.rc_thread_should_quit = True
|
||||
tester.rc_thread.join()
|
||||
tester.rc_thread = None
|
||||
results.add('TIMEOUT',
|
||||
'<span class="failed-text">FAILED</span>',
|
||||
opts.timeout)
|
||||
@ -360,6 +367,7 @@ def run_specific_test(step, *args, **kwargs):
|
||||
(testname, test) = t
|
||||
|
||||
tester_class = tester_class_map[testname]
|
||||
global tester
|
||||
tester = tester_class(*args, **kwargs)
|
||||
|
||||
print("Got %s" % str(tester))
|
||||
@ -460,9 +468,13 @@ def run_step(step):
|
||||
|
||||
# handle "test.Copter" etc:
|
||||
if step in tester_class_map:
|
||||
t = tester_class_map[step](binary, **fly_opts)
|
||||
return (t.autotest(), t)
|
||||
# create an instance of the tester class:
|
||||
global tester
|
||||
tester = tester_class_map[step](binary, **fly_opts)
|
||||
# run the test and return its result and the tester itself
|
||||
return (tester.autotest(), tester)
|
||||
|
||||
# handle "test.Copter.CPUFailsafe" etc:
|
||||
specific_test_to_run = find_specific_test_to_run(step)
|
||||
if specific_test_to_run is not None:
|
||||
return run_specific_test(specific_test_to_run, binary, **fly_opts)
|
||||
@ -678,6 +690,14 @@ def run_tests(steps):
|
||||
|
||||
print("FAILED %u tests: %s" % (len(failed), failed))
|
||||
|
||||
global tester
|
||||
if tester is not None and tester.rc_thread is not None:
|
||||
if passed:
|
||||
print("BAD: RC Thread still alive after tests passed")
|
||||
tester.rc_thread_should_quit = True
|
||||
tester.rc_thread.join()
|
||||
tester.rc_thread = None
|
||||
|
||||
util.pexpect_close_all()
|
||||
|
||||
write_fullresults()
|
||||
|
||||
@ -18,6 +18,7 @@ import numpy
|
||||
import socket
|
||||
import struct
|
||||
import random
|
||||
import threading
|
||||
|
||||
from MAVProxy.modules.lib import mp_util
|
||||
|
||||
@ -29,6 +30,11 @@ from pymavlink.rotmat import Vector3
|
||||
from pysim import util, vehicleinfo
|
||||
from io import StringIO
|
||||
|
||||
try:
|
||||
import queue as Queue
|
||||
except ImportError:
|
||||
import Queue
|
||||
|
||||
MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE |
|
||||
mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE)
|
||||
@ -1164,6 +1170,17 @@ class AutoTest(ABC):
|
||||
self.timesync_number = 137
|
||||
self.last_progress_sent_as_statustext = None
|
||||
|
||||
self.rc_thread = None
|
||||
self.rc_thread_should_quit = False
|
||||
self.rc_queue = Queue.Queue()
|
||||
|
||||
def __del__(self):
|
||||
if self.rc_thread is not None:
|
||||
self.progress("Joining thread in __del__")
|
||||
self.rc_thread_should_quit = True
|
||||
self.rc_thread.join()
|
||||
self.rc_thread = None
|
||||
|
||||
def progress(self, text, send_statustext=True):
|
||||
"""Display autotest progress text."""
|
||||
global __autotest__
|
||||
@ -1236,7 +1253,7 @@ class AutoTest(ABC):
|
||||
|
||||
def mavproxy_options(self):
|
||||
"""Returns options to be passed to MAVProxy."""
|
||||
ret = ['--sitl=127.0.0.1:5501',
|
||||
ret = ['--sitl=127.0.0.1:5502',
|
||||
'--out=' + self.autotest_connection_string_from_mavproxy(),
|
||||
'--streamrate=%u' % self.sitl_streamrate(),
|
||||
'--cmd="set heartbeat %u"' % self.speedup]
|
||||
@ -2998,23 +3015,69 @@ class AutoTest(ABC):
|
||||
16: 1500,
|
||||
}
|
||||
|
||||
def set_rc_from_map(self, _map, timeout=2000):
|
||||
def set_rc_from_map(self, _map, timeout=20):
|
||||
map_copy = _map.copy()
|
||||
self.rc_queue.put(map_copy)
|
||||
|
||||
if self.rc_thread is None:
|
||||
self.rc_thread = threading.Thread(target=self.rc_thread_main, name='RC')
|
||||
if self.rc_thread is None:
|
||||
raise NotAchievedException("Could not create thread")
|
||||
self.rc_thread.start()
|
||||
|
||||
tstart = self.get_sim_time()
|
||||
while len(map_copy.keys()):
|
||||
if self.get_sim_time_cached() - tstart > timeout:
|
||||
raise SetRCTimeout("Failed to set RC channels")
|
||||
for chan in map_copy:
|
||||
value = map_copy[chan]
|
||||
self.send_set_rc(chan, value)
|
||||
m = self.mav.recv_match(type='RC_CHANNELS', blocking=True)
|
||||
self.progress("m: %s" % m)
|
||||
new = dict()
|
||||
while True:
|
||||
if tstart - self.get_sim_time_cached() > timeout:
|
||||
raise NotAchievedException("Failed to set RC values")
|
||||
m = self.mav.recv_match(type='RC_CHANNELS', blocking=True, timeout=1)
|
||||
if m is None:
|
||||
continue
|
||||
bad_channels = ""
|
||||
for chan in map_copy:
|
||||
chan_pwm = getattr(m, "chan" + str(chan) + "_raw")
|
||||
if chan_pwm != map_copy[chan]:
|
||||
new[chan] = map_copy[chan]
|
||||
map_copy = new
|
||||
bad_channels += " (ch=%u want=%u got=%u)" % (chan, map_copy[chan], chan_pwm)
|
||||
break
|
||||
if len(bad_channels) == 0:
|
||||
break
|
||||
self.progress("RC values bad:%s" % bad_channels)
|
||||
if not self.rc_thread.is_alive():
|
||||
self.rc_thread = None
|
||||
raise ValueError("RC thread is dead") # FIXME: type
|
||||
|
||||
def rc_thread_main(self):
|
||||
chan16 = [1000] * 16
|
||||
last_sent_ms = 0
|
||||
|
||||
sitl_output = mavutil.mavudp("127.0.0.1:5501", input=False)
|
||||
buf = None
|
||||
|
||||
while True:
|
||||
if self.rc_thread_should_quit:
|
||||
break
|
||||
|
||||
# the 0.05 here means we're updating the RC values into
|
||||
# the autopilot at 20Hz - that's our 50Hz wallclock, , not
|
||||
# the autopilot's simulated 20Hz, so if speedup is 10 the
|
||||
# autopilot will see ~2Hz.
|
||||
try:
|
||||
map_copy = self.rc_queue.get(timeout=0.02)
|
||||
|
||||
# 16 packed entries:
|
||||
values = []
|
||||
for i in range(1,17):
|
||||
if i in map_copy:
|
||||
chan16[i-1] = map_copy[i]
|
||||
|
||||
except Queue.Empty:
|
||||
pass
|
||||
|
||||
buf = struct.pack('<HHHHHHHHHHHHHHHH', *chan16)
|
||||
|
||||
if buf is None:
|
||||
continue
|
||||
|
||||
sitl_output.write(buf)
|
||||
|
||||
def set_rc_default(self):
|
||||
"""Setup all simulated RC control to 1500."""
|
||||
@ -3037,37 +3100,9 @@ class AutoTest(ABC):
|
||||
need_set[chan] = default_value
|
||||
self.set_rc_from_map(need_set)
|
||||
|
||||
def send_set_rc(self, chan, pwm):
|
||||
self.mavproxy.send('rc %u %u\n' % (chan, pwm))
|
||||
|
||||
def set_rc(self, chan, pwm, timeout=2000):
|
||||
def set_rc(self, chan, pwm, timeout=20):
|
||||
"""Setup a simulated RC control to a PWM value"""
|
||||
self.drain_mav()
|
||||
tstart = self.get_sim_time()
|
||||
wclock = time.time()
|
||||
while self.get_sim_time_cached() < tstart + timeout:
|
||||
self.send_set_rc(chan, pwm)
|
||||
m = self.mav.recv_match(type='RC_CHANNELS', blocking=True)
|
||||
chan_pwm = getattr(m, "chan" + str(chan) + "_raw")
|
||||
wclock_delta = time.time() - wclock
|
||||
sim_time_delta = self.get_sim_time_cached()-tstart
|
||||
if sim_time_delta == 0:
|
||||
time_ratio = None
|
||||
else:
|
||||
time_ratio = wclock_delta / sim_time_delta
|
||||
self.progress("set_rc (wc=%s st=%s r=%s): ch=%u want=%u got=%u" %
|
||||
(wclock_delta,
|
||||
sim_time_delta,
|
||||
time_ratio,
|
||||
chan,
|
||||
pwm,
|
||||
chan_pwm))
|
||||
if chan_pwm == pwm:
|
||||
delta = self.get_sim_time_cached() - tstart
|
||||
if delta > self.max_set_rc_timeout:
|
||||
self.max_set_rc_timeout = delta
|
||||
return True
|
||||
raise SetRCTimeout("Failed to send RC commands to channel %s" % str(chan))
|
||||
self.set_rc_from_map({chan: pwm}, timeout=timeout)
|
||||
|
||||
def location_offset_ne(self, location, north, east):
|
||||
'''move location in metres'''
|
||||
@ -3329,10 +3364,10 @@ class AutoTest(ABC):
|
||||
# Different scaling for RC input and servo output means the
|
||||
# servo output value isn't the rc input value:
|
||||
self.progress("Setting RC to 1200")
|
||||
self.send_set_rc(2, 1200)
|
||||
self.rc_queue.put({2: 1200})
|
||||
self.progress("Waiting for servo of 1260")
|
||||
self.cpufailsafe_wait_servo_channel_value(2, 1260)
|
||||
self.send_set_rc(2, 1700)
|
||||
self.rc_queue.put({2: 1700})
|
||||
self.cpufailsafe_wait_servo_channel_value(2, 1660)
|
||||
self.reset_SITL_commandline()
|
||||
|
||||
@ -5004,6 +5039,8 @@ Also, ignores heartbeats not from our target system'''
|
||||
self.sup_prog = None
|
||||
|
||||
def sitl_is_running(self):
|
||||
if self.sitl is None:
|
||||
return False
|
||||
return self.sitl.isalive()
|
||||
|
||||
def init(self):
|
||||
@ -6849,13 +6886,19 @@ Also, ignores heartbeats not from our target system'''
|
||||
self.progress("Test gripper with RC9_OPTION")
|
||||
self.progress("Releasing load")
|
||||
# non strict string matching because of catching text issue....
|
||||
self.wait_text("Gripper load releas", the_function=lambda: self.send_set_rc(9, 1000))
|
||||
self.context_collect('STATUSTEXT')
|
||||
self.set_rc(9, 1000)
|
||||
self.wait_text("Gripper load releas", check_context=True)
|
||||
self.progress("Grabbing load")
|
||||
self.wait_text("Gripper load grabb", the_function=lambda: self.send_set_rc(9, 2000))
|
||||
self.set_rc(9, 2000)
|
||||
self.wait_text("Gripper load grabb", check_context=True)
|
||||
self.context_clear_collection('STATUSTEXT')
|
||||
self.progress("Releasing load")
|
||||
self.wait_text("Gripper load releas", the_function=lambda: self.send_set_rc(9, 1000))
|
||||
self.set_rc(9, 1000)
|
||||
self.wait_text("Gripper load releas", check_context=True)
|
||||
self.progress("Grabbing load")
|
||||
self.wait_text("Gripper load grabb", the_function=lambda: self.send_set_rc(9, 2000))
|
||||
self.set_rc(9, 2000)
|
||||
self.wait_text("Gripper load grabb", check_context=True)
|
||||
self.progress("Test gripper with Mavlink cmd")
|
||||
self.progress("Releasing load")
|
||||
self.wait_text("Gripper load releas",
|
||||
@ -7620,6 +7663,12 @@ switch value'''
|
||||
if self.logs_dir:
|
||||
if glob.glob("core*"):
|
||||
self.check_logs("FRAMEWORK")
|
||||
|
||||
if self.rc_thread is not None:
|
||||
self.progress("Joining thread")
|
||||
self.rc_thread_should_quit = True
|
||||
self.rc_thread.join()
|
||||
self.rc_thread = None
|
||||
self.close()
|
||||
|
||||
if len(self.skip_list):
|
||||
|
||||
@ -351,9 +351,7 @@ class AutoTestRover(AutoTest):
|
||||
self.load_mission(filename)
|
||||
self.wait_ready_to_arm()
|
||||
self.arm_vehicle()
|
||||
self.mavproxy.send('switch 4\n') # auto mode
|
||||
self.set_rc(3, 1500)
|
||||
self.wait_mode('AUTO')
|
||||
self.change_mode('AUTO')
|
||||
self.wait_waypoint(1, 4, max_dist=5)
|
||||
self.mavproxy.expect("Mission Complete")
|
||||
self.disarm_vehicle()
|
||||
@ -575,6 +573,9 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
||||
self.progress("Pin mask changed after relay command")
|
||||
|
||||
def test_setting_modes_via_mavproxy_switch(self):
|
||||
self.customise_SITL_commandline([
|
||||
"--rc-in-port", "5502",
|
||||
])
|
||||
self.load_mission(self.arming_test_mission())
|
||||
self.wait_ready_to_arm()
|
||||
fnoo = [(1, 'MANUAL'),
|
||||
@ -635,12 +636,12 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
|
||||
self.context_push()
|
||||
ex = None
|
||||
try:
|
||||
self.set_parameter("MODE5", 1)
|
||||
self.mavproxy.send('switch 1\n') # random mode
|
||||
self.wait_heartbeat()
|
||||
self.change_mode('MANUAL')
|
||||
self.mavproxy.send('switch 5\n') # acro mode
|
||||
self.wait_mode("ACRO")
|
||||
# from mavproxy_rc.py
|
||||
mapping = [ 0, 1165, 1295, 1425, 1555, 1685, 1815 ]
|
||||
self.set_parameter("MODE1", 1) # acro
|
||||
self.set_rc(8, mapping[1])
|
||||
self.wait_mode('ACRO')
|
||||
|
||||
self.set_rc(9, 1000)
|
||||
self.set_rc(10, 1000)
|
||||
self.set_parameter("RC9_OPTION", 53) # steering
|
||||
|
||||
Loading…
Reference in New Issue
Block a user