diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices
index 4d51e5378e..592e4289d9 160000
--- a/src/drivers/gps/devices
+++ b/src/drivers/gps/devices
@@ -1 +1 @@
-Subproject commit 4d51e5378ec74e8948981cfb9e47ed84191e2fd2
+Subproject commit 592e4289d941eb0b7d84e6da9d72d731083633ee
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index c49add233c..23278b86e9 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -409,6 +409,10 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 
 void GPS::handleInjectDataTopic()
 {
+	if (!_helper->shouldInjectRTCM()) {
+		return;
+	}
+
 	bool updated = false;
 
 	// Limit maximum number of GPS injections to 6 since usually
@@ -644,6 +648,24 @@ GPS::run()
 		param_get(handle, &gps_ubx_dynmodel);
 	}
 
+	handle = param_find("GPS_UBX_MODE");
+
+	GPSDriverUBX::UBXMode ubx_mode{GPSDriverUBX::UBXMode::Normal};
+
+	if (handle != PARAM_INVALID) {
+		int32_t gps_ubx_mode = 0;
+		param_get(handle, &gps_ubx_mode);
+
+		if (gps_ubx_mode == 1) { // heading
+			if (_instance == Instance::Main) {
+				ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBase;
+
+			} else {
+				ubx_mode = GPSDriverUBX::UBXMode::MovingBase;
+			}
+		}
+	}
+
 	initializeCommunicationDump();
 
 	uint64_t last_rate_measurement = hrt_absolute_time();
@@ -696,7 +718,7 @@ GPS::run()
 			/* FALLTHROUGH */
 			case GPS_DRIVER_MODE_UBX:
 				_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
-							   gps_ubx_dynmodel);
+							   gps_ubx_dynmodel, heading_offset, ubx_mode);
 				break;
 
 			case GPS_DRIVER_MODE_MTK:
diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c
index ee105aff2b..ed5a7b3480 100644
--- a/src/drivers/gps/params.c
+++ b/src/drivers/gps/params.c
@@ -64,6 +64,27 @@ PARAM_DEFINE_INT32(GPS_DUMP_COMM, 0);
  */
 PARAM_DEFINE_INT32(GPS_UBX_DYNMODEL, 7);
 
+/**
+ * u-blox GPS Mode
+ *
+ * Select the u-blox configuration setup. Most setups will use the default, including RTK and
+ * dual GPS without heading.
+ *
+ * The Heading mode requires 2 F9P devices to be attached. The main GPS will act as rover and output
+ * heading information, whereas the secondary will act as moving base, sending RTCM on UART2 to
+ * the rover GPS.
+ * RTK is still possible with this setup.
+ *
+ * @min 0
+ * @max 1
+ * @value 0 Default
+ * @value 1 Heading
+ *
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_UBX_MODE, 0);
+
 
 /**
  * Heading/Yaw offset for dual antenna GPS