SITL: create GPS heading offset parameter

ArduPilot · Nov 29, 2023 · 351665b · 351665b
1 parent 4ede307
commit 351665b
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 1 deletion.
diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp
@@ -280,7 +280,7 @@ void GPS::update()
 
         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;
 

diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
@@ -569,6 +569,11 @@ const AP_Param::GroupInfo SIM::var_gps[] = {
     // @Vector3Parameter: 1
     // @User: Advanced
     AP_GROUPINFO("GPS_VERR",      15, SIM,  gps_vel_err[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",  16, SIM,  gps_heading_offset[0], 0),
     // @Param: GPS2_DISABLE
     // @DisplayName: GPS 2 disable
     // @Description: Disables GPS 2
@@ -672,6 +677,12 @@ 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),
 
+    // @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),
+
     AP_GROUPEND
 };
 #endif  // HAL_SIM_GPS_ENABLED

diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h
@@ -202,6 +202,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];