autotest: add simple test for Rover set_attitude_target handling

Tools/autotest/rover.py

@@ -4,6 +4,7 @@
 from __future__ import print_function
 
 import copy
+import math
 import os
 import shutil
 import sys
@@ -18,6 +19,7 @@ from common import MsgRcvTimeoutException
 from common import NotAchievedException
 from common import PreconditionFailedException
 
+from pymavlink import mavextra
 from pymavlink import mavutil
 
 # get location of scripts
@@ -5347,6 +5349,39 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
         self.disarm_vehicle()
         self.context_pop()
 
+    def SET_ATTITUDE_TARGET(self, target_sysid=None, target_compid=1):
+        if target_sysid is None:
+            target_sysid = self.sysid_thismav()
+        self.change_mode('GUIDED')
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        tstart = self.get_sim_time()
+        while True:
+            now = self.get_sim_time_cached()
+            if now - tstart > 10:
+                raise AutoTestTimeoutException("Didn't get to speed")
+            self.mav.mav.set_attitude_target_send(
+                0, # time_boot_ms
+                target_sysid,
+                target_compid,
+                mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE |
+                mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE |
+                mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE,
+                mavextra.euler_to_quat([0,
+                                        math.radians(0),
+                                        math.radians(0)]), # att
+                0, # yaw rate (rad/s)
+                0, # pitch rate
+                0, # yaw rate
+                1) # thrust
+
+            msg = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=1)
+            if msg is None:
+                raise NotAchievedException("No VFR_HUD message")
+            if msg.groundspeed > 5:
+                break
+        self.disarm_vehicle()
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestRover, self).tests()
@@ -5441,6 +5476,10 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
              "Test enforcement of SYSID_MYGCS",
              self.test_sysid_enforce),
 
+            ("SET_ATTITUDE_TARGET",
+             "Test handling of SET_ATTITUDE_TARGET",
+             self.SET_ATTITUDE_TARGET),
+
             ("Button",
              "Test Buttons",
              self.test_button),