mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-22 07:44:03 -04:00
autotest: tidy Rover DriveMaxRCIN test
take advantage of created infrastructure
This commit is contained in:
parent
1839db7c1f
commit
a44820cfde
@ -342,38 +342,21 @@ class AutoTestRover(AutoTest):
|
|||||||
|
|
||||||
def DriveMaxRCIN(self, timeout=30):
|
def DriveMaxRCIN(self, timeout=30):
|
||||||
"""Drive rover at max RC inputs"""
|
"""Drive rover at max RC inputs"""
|
||||||
self.context_push()
|
self.progress("Testing max RC inputs")
|
||||||
ex = None
|
self.change_mode("MANUAL")
|
||||||
|
|
||||||
try:
|
self.wait_ready_to_arm()
|
||||||
self.progress("Testing max RC inputs")
|
self.arm_vehicle()
|
||||||
self.change_mode("MANUAL")
|
|
||||||
|
|
||||||
self.wait_ready_to_arm()
|
self.set_rc(3, 2000)
|
||||||
self.arm_vehicle()
|
self.set_rc(1, 1000)
|
||||||
|
|
||||||
self.set_rc(3, 2000)
|
tstart = self.get_sim_time()
|
||||||
self.set_rc(1, 1000)
|
while self.get_sim_time_cached() - tstart < timeout:
|
||||||
|
m = self.assert_receive_message('VFR_HUD')
|
||||||
tstart = self.get_sim_time()
|
self.progress("Current speed: %f" % m.groundspeed)
|
||||||
while self.get_sim_time_cached() - tstart < timeout:
|
|
||||||
m = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=1)
|
|
||||||
if m is not None:
|
|
||||||
self.progress("Current speed: %f" % m.groundspeed)
|
|
||||||
|
|
||||||
# reduce throttle
|
|
||||||
self.set_rc(3, 1500)
|
|
||||||
self.set_rc(1, 1500)
|
|
||||||
|
|
||||||
except Exception as e:
|
|
||||||
self.print_exception_caught(e)
|
|
||||||
ex = e
|
|
||||||
|
|
||||||
self.disarm_vehicle()
|
self.disarm_vehicle()
|
||||||
self.context_pop()
|
|
||||||
|
|
||||||
if ex:
|
|
||||||
raise ex
|
|
||||||
|
|
||||||
#################################################
|
#################################################
|
||||||
# AUTOTEST ALL
|
# AUTOTEST ALL
|
||||||
|
Loading…
Reference in New Issue
Block a user