Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 22:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Tools: autotest: add a test for RTL speed

Browse Source
This commit is contained in:
Peter Barker 2019-01-31 16:01:33 +11:00 committed by Randy Mackay
parent aa7743e991
commit 44ff21479d
2 changed files with 53 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
Tools/autotest/arducopter.py
View File

@ -1294,9 +1294,6 @@ class AutoTestCopter(AutoTest):
self.do_RTL()
def fly_mission(self):
"""Fly a mission from a file."""
def fly_vision_position(self):
"""Disable GPS navigation, enable Vicon input."""
# scribble down a location we can set origin to:
@ -1384,6 +1381,49 @@ class AutoTestCopter(AutoTest):
if ex is not None:
raise ex
def monitor_groundspeed(self, want, tolerance=0.5, timeout=5):
tstart = self.get_sim_time()
while True:
if self.get_sim_time() - tstart > timeout:
break
m = self.mav.recv_match(type='VFR_HUD', blocking=True)
if m.groundspeed > want+tolerance:
raise NotAchievedException("Too fast (%f > %f)" %
(m.groundspeed, want))
if m.groundspeed < want-tolerance:
raise NotAchievedException("Too slow (%f < %f)" %
(m.groundspeed, want))
self.progress("GroundSpeed OK (got=%f) (want=%f)" %
(m.groundspeed, want))
def fly_rtl_speed(self):
"""Test RTL Speed parameters"""
rtl_speed_ms = 7
wpnav_speed_ms = 4
wpnav_accel_mss = 3
tolerance = 0.5
self.load_mission("copter_rtl_speed.txt")
self.set_parameter('WPNAV_ACCEL', wpnav_accel_mss * 100)
self.set_parameter('RTL_SPEED', rtl_speed_ms * 100)
self.set_parameter('WPNAV_SPEED', wpnav_speed_ms * 100)
self.change_mode('LOITER')
self.wait_ready_to_arm()
self.arm_vehicle()
self.change_mode('AUTO')
self.set_rc(3, 1600)
self.wait_altitude(19, 25, relative=True)
self.wait_groundspeed(wpnav_speed_ms-tolerance, wpnav_speed_ms+tolerance)
self.monitor_groundspeed(wpnav_speed_ms, timeout=20)
self.change_mode('RTL')
self.wait_groundspeed(rtl_speed_ms-tolerance, rtl_speed_ms+tolerance)
self.monitor_groundspeed(rtl_speed_ms, timeout=5)
self.change_mode('AUTO')
self.wait_groundspeed(0-tolerance, 0+tolerance)
self.wait_groundspeed(wpnav_speed_ms-tolerance, wpnav_speed_ms+tolerance)
self.monitor_groundspeed(wpnav_speed_ms, timeout=5)
self.change_mode('RTL')
self.mav.motors_disarmed_wait()
def fly_nav_delay(self):
"""Fly a simple mission that has a delay in it."""
@ -2675,6 +2715,10 @@ class AutoTestCopter(AutoTest):
"Fly Vision Position",
self.fly_vision_position),
("RTLSpeed",
"Fly RTL Speed",
self.fly_rtl_speed),
("Mount",
"Test Camera/Antenna Mount",
self.test_mount),

6
Tools/autotest/copter_rtl_speed.txt Normal file
View File

@ -0,0 +1,6 @@
QGC WPL 110
0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.080017 1
1 0 3 22 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 20.000000 1
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361374 149.164917 20.000000 1
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361397 149.163910 20.000000 1
4 0 0 20 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1