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

Tools/autotest/ardusub.py

@@ -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: