Tools: Modify MANUAL_CONTROL autotest for Rover to include yaw

Author: ohyaiamhere

Tools/autotest/rover.py

@@ -1131,53 +1131,56 @@ def MANUAL_CONTROL(self):
 
         self.progress("now override to stop")
         throttle_override_normalized = 0
+        yaw_override_normalized = 0
         expected_throttle = 0 # in VFR_HUD
+        want_speed = 5.0
+        want_yaw = 0.002
 
         tstart = self.get_sim_time_cached()
         while True:
             if self.get_sim_time_cached() - tstart > 10:
-                raise AutoTestTimeoutException("Did not reach speed")
+                raise AutoTestTimeoutException("Did not reach speed or yaw")
             self.progress("Sending normalized throttle of %d" % (throttle_override_normalized,))
             self.mav.mav.manual_control_send(
                 1, # target system
                 32767, # x (pitch)
                 32767, # y (roll)
                 throttle_override_normalized, # z (thrust)
-                32767, # r (yaw)
+                yaw_override_normalized, # r (yaw)
                 0) # button mask
 
             m = self.assert_receive_message('VFR_HUD')
-            want_speed = 2.0
-            self.progress("Speed=%f want=<%f  throttle=%u want=%u" %
-                          (m.groundspeed, want_speed, m.throttle, expected_throttle))
-            if m.groundspeed < want_speed and m.throttle == expected_throttle:
+            n = self.assert_receive_message('ATTITUDE')
+            self.progress("Speed=%f want=<%f  throttle=%u want=%u yaw=%f want_yaw=%f" %
+                          (m.groundspeed, want_speed, m.throttle, expected_throttle, n.yaw, want_yaw))
+            if m.groundspeed < want_speed and m.throttle == expected_throttle and n.yaw < want_yaw:
                 break
 
         self.progress("now override to stop - but set the switch on the RC transmitter to deny overrides; this should send the speed back up to 5 metres/second")  # noqa
         self.set_rc(12, 1000)
 
         throttle_override_normalized = 500
+        yaw_override_normalized = 500
         expected_throttle = 36 # in VFR_HUD, corresponding to normal_rc_throttle adjusted for channel min/max
 
         tstart = self.get_sim_time_cached()
         while True:
             if self.get_sim_time_cached() - tstart > 10:
-                raise AutoTestTimeoutException("Did not stop")
+                raise AutoTestTimeoutException("Override did not stop")
             self.progress("Sending normalized throttle of %u" % (throttle_override_normalized,))
             self.mav.mav.manual_control_send(
                 1, # target system
                 32767, # x (pitch)
                 32767, # y (roll)
                 throttle_override_normalized, # z (thrust)
-                32767, # r (yaw)
+                yaw_override_normalized, # r (yaw)
                 0) # button mask
 
             m = self.assert_receive_message('VFR_HUD')
-            want_speed = 5.0
-
-            self.progress("Speed=%f want=>%f  throttle=%u want=%u" %
-                          (m.groundspeed, want_speed, m.throttle, expected_throttle))
-            if m.groundspeed > want_speed and m.throttle == expected_throttle:
+            n = self.assert_receive_message('ATTITUDE')
+            self.progress("Speed=%f want=>%f  throttle=%u want=%u yaw=%f want_yaw=%f" %
+                          (m.groundspeed, want_speed, m.throttle, expected_throttle, n.yaw, want_yaw))
+            if m.groundspeed > want_speed and m.throttle == expected_throttle and n.yaw > want_yaw:
                 break
 
         # re-enable RC overrides