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

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
@@ -715,6 +716,30 @@ 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.load_default_params_file("sub-gps-for-yaw-nmea.parm")
+        self.reboot_sitl()
+        # wait for the vehicle to be ready
+        self.wait_ready_to_arm()
+        # 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 + 30) # 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)
+
+        att_m = self.assert_receive_message("ATTITUDE")
+        achieved_yaw_deg = mavextra.wrap_180(degrees(att_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)
+            )
+
     def _MAV_CMD_CONDITION_YAW(self, run_cmd):
         self.arm_vehicle()
         self.change_mode('GUIDED')
@@ -978,6 +1003,7 @@ def tests(self):
             self.SetGlobalOrigin,
             self.BackupOrigin,
             self.FuseMag,
+            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
+GPS1_TYPE 5
+SIM_GPS_TYPE 5
+SIM_GPS_HDG 1
+GPS_AUTO_CONFIG 0
+SIM_GPS_HDG_OFS 30.0

libraries/SITL/SIM_GPS.cpp

@@ -359,7 +359,7 @@ void GPS::update()
     d.num_sats = _sitl->gps_numsats[idx];
     d.latitude = latitude;
     d.longitude = longitude;
-    d.yaw_deg = _sitl->state.yawDeg;
+    d.yaw_deg = _sitl->state.yawDeg + _sitl->gps_heading_offset[idx];;
     d.roll_deg = _sitl->state.rollDeg;
     d.pitch_deg = _sitl->state.pitchDeg;
 

libraries/SITL/SITL.cpp

@@ -850,6 +850,11 @@ const AP_Param::GroupInfo SIM::var_gps[] = {
     // @User: Advanced
     // @Values: 0:Disabled, 1:Enabled
     AP_GROUPINFO("GPS_JAM",       16, SIM,  gps_jam[0], 0),
+    // @Param: GPS_HDG_OFS
+    // @DisplayName: GPS heading offset
+    // @Description: GPS heading offset in degrees. how off the GPS heading is from the actual heading
+    // @User: Advanced
+    AP_GROUPINFO("GPS_HDG_OFS",  17, SIM,  gps_heading_offset[0], 0),
     // @Param: GPS2_DISABLE
     // @DisplayName: GPS 2 disable
     // @Description: Disables GPS 2
@@ -960,6 +965,12 @@ const AP_Param::GroupInfo SIM::var_gps[] = {
     // @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",  50, SIM,  gps_heading_offset[1], 0),
+
     AP_GROUPEND
 };
 #endif  // HAL_SIM_GPS_ENABLED

libraries/SITL/SITL.h

@@ -207,6 +207,7 @@ class SIM {
     AP_Vector3f gps_glitch[2];  // glitch offsets in lat, lon and altitude
     AP_Int8  gps_hertz[2];   // GPS update rate in Hz
     AP_Int8 gps_hdg_enabled[2]; // enable the output of a NMEA heading HDT sentence or UBLOX RELPOSNED
+    AP_Float gps_heading_offset[2]; // heading offset for GpsForYaw
     AP_Float gps_drift_alt[2]; // altitude drift error
     AP_Vector3f gps_pos_offset[2];  // XYZ position of the GPS antenna phase centre relative to the body frame origin (m)
     AP_Float gps_accuracy[2];