Tools: Autotest: create gps-for-yaw test for Sub

Tools/autotest/ardusub.py

@@ -9,11 +9,12 @@
 import sys
 import time
 
-from pymavlink import mavutil
+from pymavlink import mavutil, mavextra
 
 import vehicle_test_suite
 from vehicle_test_suite import NotAchievedException
 from vehicle_test_suite import AutoTestTimeoutException
+from math import degrees
 
 if sys.version_info[0] < 3:
     ConnectionResetError = AutoTestTimeoutException
@@ -474,6 +475,40 @@ def MAV_CMD_DO_CHANGE_SPEED(self):
                 self.wait_groundspeed(speed-0.2, speed+0.2, minimum_duration=2, timeout=60)
         self.disarm_vehicle()
 
+    def GPSForYaw(self):
+        '''Moving baseline GPS yaw'''
+        # store previous heading
+        previous_heading = self.assert_receive_message("ATTITUDE").yaw
+
+        # load parameters with gps for yaw and 10 degrees offset
+        self.context_push()
+        self.load_default_params_file("sub-gps-for-yaw-nmea.parm")
+        self.reboot_sitl()
+        ex = None
+        try:
+            self.wait_gps_fix_type_gte(3, message_type="GPS_RAW_INT", verbose=True)
+            gps_m = self.assert_receive_message("GPS_RAW_INT")
+            attitude_m = self.assert_receive_message("ATTITUDE")
+            self.progress(self.dump_message_verbose(gps_m))
+            self.wait_heading(195, minimum_duration=2)
+            want = mavextra.wrap_360(degrees(previous_heading) + 10) * 100
+            attitude_yaw_deg = mavextra.wrap_360(degrees(attitude_m.yaw)) * 100
+            # ensure new reading propagated to GPS_RAW_INT
+            if abs(gps_m.yaw - want) > 500:
+                raise NotAchievedException("Expected to get yaw populated on GPS_RAW_INT (want %f got %f)" % (want, gps_m.yaw))
+            # ensure new reading propagated to ATTITUDE
+            if abs(attitude_yaw_deg - want) > 500:
+                raise NotAchievedException("Expected to get yaw consumed and at ATTITUDE (want %f got %f)" % (want, attitude_yaw_deg))
+            self.wait_ready_to_arm()
+        except Exception as e:
+            self.print_exception_caught(e)
+            ex = e
+
+        self.context_pop()
+        self.reboot_sitl()
+        if ex is not None:
+            raise ex
+
     def _MAV_CMD_CONDITION_YAW(self, run_cmd):
         self.arm_vehicle()
         self.change_mode('GUIDED')
@@ -535,6 +570,7 @@ def tests(self):
             self.MAV_CMD_MISSION_START,
             self.MAV_CMD_DO_CHANGE_SPEED,
             self.MAV_CMD_CONDITION_YAW,
+            self.GPSForYaw,
         ])
 
         return ret