SITL: create GPS heading offset parameter

Williangalvani · Nov 22, 2023 · b2d36b6 · b2d36b6
1 parent e25d4dc
commit b2d36b6
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
@@ -556,7 +556,7 @@ void GPS_NMEA::update_write(const GPS_Data *d)
                      dstring);
 
     if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_HDT) {
-        nmea_printf("$GPHDT,%.2f,T", d->yaw_deg);
+        nmea_printf("$GPHDT,%.2f,T", d->yaw_deg + _sitl->gps_hdg_offset[instance]);
     }
     else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_THS) {
         nmea_printf("$GPTHS,%.2f,%c,T", d->yaw_deg, d->have_lock ? 'A' : 'V');

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_hdg_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_hdg_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_hdg_offset[2]; // heading offset in degrees for HDT sentences
     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];