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
@@ -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

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

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
 };