Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

autotest: sub: fix altitude-hold for being below target altitude

This commit is contained in:
Peter Barker 2020-04-05 19:19:43 +10:00 committed by Peter Barker
parent 78e432af0c
commit 5881692e4f
1 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
Tools/autotest/ardusub.py
View File

@ -102,8 +102,14 @@ class AutoTestSub(AutoTest):
self.mavproxy.send('mode ALT_HOLD\n')
self.wait_mode('ALT_HOLD')
self.set_rc(Joystick.Throttle, 1000)
msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5)
if msg is None:
raise NotAchievedException("Did not get GLOBAL_POSITION_INT")
pwm = 1000
if msg.relative_alt/1000.0 < -5.5:
# need to g`o up, not down!
pwm = 2000
self.set_rc(Joystick.Throttle, pwm)
self.wait_altitude(altitude_min=-6, altitude_max=-5)
self.set_rc(Joystick.Throttle, 1500)
@ -191,8 +197,7 @@ class AutoTestSub(AutoTest):
self.arm_vehicle()
self.mavproxy.send('mode auto\n')
self.wait_mode('AUTO')
self.change_mode('AUTO')
self.wait_waypoint(1, 5, max_dist=5)
@ -214,8 +219,7 @@ class AutoTestSub(AutoTest):
self.mavproxy.send('mode loiter\n')
self.wait_ready_to_arm()
self.arm_vehicle()
self.mavproxy.send('mode auto\n')
self.wait_mode('AUTO')
self.change_mode('AUTO')
self.mavproxy.expect("Gripper Grabbed")
self.mavproxy.expect("Gripper Released")
except Exception as e: