SITL: create GPS heading offset parameter

Williangalvani · Jan 31, 2024 · 975312d · 975312d
1 parent b119a2a
commit 975312d
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
@@ -345,7 +345,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
@@ -575,6 +575,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
@@ -685,6 +690,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",  45, 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
@@ -204,6 +204,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];