From 87977981dfe05d14fafe05c32424f10b5a01315d 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 +++++
 libraries/SITL/SITL.cpp                       |  5 +---
 3 files changed, 36 insertions(+), 5 deletions(-)
 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 6d234c8ebfd669..397ed02499caeb 100644
--- a/Tools/autotest/ardusub.py
+++ b/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
@@ -514,6 +515,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')
@@ -577,6 +603,7 @@ def tests(self):
             self.MAV_CMD_MISSION_START,
             self.MAV_CMD_DO_CHANGE_SPEED,
             self.MAV_CMD_CONDITION_YAW,
+            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
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index a7a88f20f9db90..49f0aed3121247 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -683,20 +683,17 @@ const AP_Param::GroupInfo SIM::var_gps[] = {
     // @Description: Log number for GPS:update_file()
     AP_GROUPINFO("GPS_LOG_NUM",   48, SIM,  gps_log_num, 0),
 
-<<<<<<< HEAD
     // @Param: GPS2_JAM
     // @DisplayName: GPS jamming enable
     // @Description: Enable simulated GPS jamming
     // @User: Advanced
     // @Values: 0:Disabled, 1:Enabled
     AP_GROUPINFO("GPS2_JAM",      49, SIM,  gps_jam[1], 0),
-=======
     // @Param: GPS2_HDG_OFS
     // @DisplayName: GPS2 heading offset
     // @Description: GPS heading offset in degrees. how off the GPS heading is from the actual heading
     // @User: Advanced
-    AP_GROUPINFO("GPS2_HDG_OFS",  49, SIM,  gps_heading_offset[1], 0),
->>>>>>> c519c9d26f (SITL: create GPS heading offset parameter)
+    AP_GROUPINFO("GPS2_HDG_OFS",  50, SIM,  gps_heading_offset[1], 0),
 
     AP_GROUPEND
 };