From 00f7d891f0bd13d9912b68514f0fd54d9b152b2e Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Wed, 22 Nov 2023 13:35:13 -0300
Subject: [PATCH] Tools: Autotest: create gps-for-yaw test for Sub

---
 Tools/autotest/ardusub.py                     | 29 ++++++++++++++++++-
 .../default_params/sub-gps-for-yaw-nmea.parm  |  7 +++++
 2 files changed, 35 insertions(+), 1 deletion(-)
 create mode 100755 Tools/autotest/default_params/sub-gps-for-yaw-nmea.parm

diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py
index bca38d192b90c9..a38b1b8c3b7bfa 100644
--- a/Tools/autotest/ardusub.py
+++ b/Tools/autotest/ardusub.py
@@ -11,12 +11,13 @@
 import os
 import sys
 
-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 vehicle_test_suite import PreconditionFailedException
+from math import degrees
 
 if sys.version_info[0] < 3:
     ConnectionResetError = AutoTestTimeoutException
@@ -696,6 +697,31 @@ 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):
+        '''Consume heading of NMEA GPS and propagate to ATTITUDE'''
+
+        # 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()
+        # make sure we are getting both GPS_RAW_INT and SIMSTATE
+        simstate_m = self.assert_receive_message("SIMSTATE")
+        real_yaw_deg = degrees(simstate_m.yaw)
+        expected_yaw_deg = mavextra.wrap_180(real_yaw_deg + 10) # offset in the param file, in degrees
+        # wait for GPS_RAW_INT to have a good fix
+        self.wait_gps_fix_type_gte(3, message_type="GPS_RAW_INT", verbose=True)
+
+        gps_m = self.assert_receive_message("ATTITUDE")
+        achieved_yaw_deg = mavextra.wrap_180(degrees(gps_m.yaw))
+
+        # ensure new reading propagated to ATTITUDE
+        if abs(achieved_yaw_deg - expected_yaw_deg) > 5:
+            raise NotAchievedException(
+                "Expected to get yaw consumed and at ATTITUDE (want %f got %f)" % (expected_yaw_deg, achieved_yaw_deg)
+            )
+        self.context_pop()
+        self.reboot_sitl()
+
     def _MAV_CMD_CONDITION_YAW(self, run_cmd):
         self.arm_vehicle()
         self.change_mode('GUIDED')
@@ -846,6 +872,7 @@ def tests(self):
             self.MAV_CMD_CONDITION_YAW,
             self.TerrainMission,
             self.SetGlobalOrigin,
+            self.GPSForYaw,
         ])
 
         return ret
diff --git a/Tools/autotest/default_params/sub-gps-for-yaw-nmea.parm b/Tools/autotest/default_params/sub-gps-for-yaw-nmea.parm
new file mode 100755
index 00000000000000..82e58326f917c3
--- /dev/null
+++ b/Tools/autotest/default_params/sub-gps-for-yaw-nmea.parm
@@ -0,0 +1,7 @@
+# SITL GPS-for-yaw using a single simulated NMEA GPS
+EK3_SRC1_YAW 3
+GPS_TYPE 5
+SIM_GPS_TYPE 5
+SIM_GPS_HDG 1
+GPS_AUTO_CONFIG 0
+SIM_GPS_HDG_OFS 10.0