gnss: u-blox f9p: Add RTK integration to driver

Enable driver to consume RTK data-correction messages published
in order to enhance GNSS Navigation results.

Signed-off-by: Luis Ubieda <luisf@croxel.com>

Author: ubieda

drivers/gnss/Kconfig.u_blox_f9p

@@ -49,4 +49,14 @@ config GNSS_U_BLOX_F9P_SATELLITES_COUNT
 	default 24
 
 endif # GNSS_SATELLITES
+
+if GNSS_RTK
+
+config GNSS_U_BLOX_F9P_RTK
+	bool "Use RTK to improve fix accuracy"
+	default y
+	depends on GNSS_RTK_PROTOCOL_RTCM3
+
+endif # GNSS_RTK
+
 endif # GNSS_U_BLOX_F9P

drivers/gnss/gnss_u_blox_f9p.c

@@ -18,6 +18,7 @@
 #include <zephyr/modem/backend/uart.h>
 
 #include "gnss_ubx_common.h"
+#include <zephyr/gnss/rtk/rtk.h>
 
 #include <zephyr/logging/log.h>
 LOG_MODULE_REGISTER(ubx_f9p, CONFIG_GNSS_LOG_LEVEL);
@@ -81,6 +82,19 @@ UBX_FRAME_DEFINE(enable_nav,
 	UBX_FRAME_CFG_VAL_SET_U8_INITIALIZER(UBX_KEY_MSG_OUT_UBX_NAV_PVT_UART1, 1));
 UBX_FRAME_DEFINE(nav_fix_mode_auto,
 	UBX_FRAME_CFG_VAL_SET_U8_INITIALIZER(UBX_KEY_NAV_CFG_FIX_MODE, UBX_FIX_MODE_AUTO));
+UBX_FRAME_DEFINE(enable_prot_in_ubx,
+	UBX_FRAME_CFG_VAL_SET_U8_INITIALIZER(UBX_KEY_UART1_PROTO_IN_UBX, 1));
+UBX_FRAME_DEFINE(enable_prot_in_rtcm3,
+	UBX_FRAME_CFG_VAL_SET_U8_INITIALIZER(UBX_KEY_UART1_PROTO_IN_RTCM3X, 1));
+UBX_FRAME_DEFINE(enable_prot_out_ubx,
+	UBX_FRAME_CFG_VAL_SET_U8_INITIALIZER(UBX_KEY_UART1_PROTO_OUT_UBX, 1));
+UBX_FRAME_DEFINE(disable_prot_out_rtcm3,
+	UBX_FRAME_CFG_VAL_SET_U8_INITIALIZER(UBX_KEY_UART1_PROTO_OUT_RTCM3X, 0));
+UBX_FRAME_DEFINE(enable_ubx_rtcm_rsp,
+	UBX_FRAME_CFG_VAL_SET_U8_INITIALIZER(UBX_KEY_MSG_OUT_UBX_RXM_RTCM_UART1, 1));
+UBX_FRAME_DEFINE(set_rtk_fix_mode,
+	UBX_FRAME_CFG_VAL_SET_U8_INITIALIZER(UBX_KEY_NAV_HP_CFG_GNSS_MODE,
+					     UBX_NAV_HP_DGNSS_MODE_RTK_FIXED));
 #if CONFIG_GNSS_SATELLITES
 UBX_FRAME_DEFINE(enable_sat,
 	UBX_FRAME_CFG_VAL_SET_U8_INITIALIZER(UBX_KEY_MSG_OUT_UBX_NAV_SAT_UART1, 1));
@@ -91,6 +105,8 @@ UBX_FRAME_ARRAY_DEFINE(u_blox_f9p_init_seq,
 	&disable_nmea_gbs, &disable_nmea_gll, &disable_nmea_gns, &disable_nmea_grs,
 	&disable_nmea_gsa, &disable_nmea_gst, &disable_nmea_vlw, &disable_nmea_vtg,
 	&disable_nmea_zda, &enable_nav, &nav_fix_mode_auto,
+	&enable_prot_in_ubx, &enable_prot_in_rtcm3, &enable_prot_out_ubx,
+	&disable_prot_out_rtcm3, &enable_ubx_rtcm_rsp, &set_rtk_fix_mode,
 #if CONFIG_GNSS_SATELLITES
 	&enable_sat,
 #endif
@@ -187,6 +203,19 @@ static int ubx_f9p_msg_payload_send(const struct device *dev, uint8_t class, uin
 	return err;
 }
 
+#if CONFIG_GNSS_U_BLOX_F9P_RTK
+
+static void f9p_rtk_data_cb(const struct device *dev, const struct gnss_rtk_data *data)
+{
+	/** In this case, we forward the frame directly to the modem. It can either use
+	 * it or not depending on the RTCM3 message type and its alignment with what the
+	 * GNSS modem has observed.
+	 */
+	(void)ubx_f9p_msg_send(dev, (const void *)data->data, data->len, false);
+}
+
+#endif /* CONFIG_GNSS_U_BLOX_F9P_RTK */
+
 static inline int init_modem(const struct device *dev)
 {
 	int err;
@@ -516,6 +545,9 @@ static DEVICE_API(gnss, ublox_f9p_driver_api) = {
 												   \
 	static struct ubx_f9p_data ubx_f9p_data_##inst;						   \
 												   \
+	IF_ENABLED(CONFIG_GNSS_U_BLOX_F9P_RTK,							   \
+		   (GNSS_RTK_DATA_CALLBACK_DEFINE(DEVICE_DT_INST_GET(inst), f9p_rtk_data_cb)));	   \
+												   \
 	DEVICE_DT_INST_DEFINE(inst,								   \
 			      ublox_f9p_init,							   \
 			      NULL,								   \

drivers/gnss/gnss_ubx_common.c

@@ -27,19 +27,24 @@ void gnss_ubx_common_pvt_callback(struct modem_ubx *ubx, const struct ubx_frame
 	if ((nav_pvt->flags & UBX_NAV_PVT_FLAGS_GNSS_FIX_OK) &&
 	    !(nav_pvt->nav.flags3 & UBX_NAV_PVT_FLAGS3_INVALID_LLH)) {
 
-		switch (nav_pvt->fix_type) {
-		case UBX_NAV_FIX_TYPE_DR:
-		case UBX_NAV_FIX_TYPE_GNSS_DR_COMBINED:
+		if (nav_pvt->flags & UBX_NAV_PVT_FLAGS_GNSS_CARR_SOLN_FLOATING) {
+			fix_quality = GNSS_FIX_QUALITY_FLOAT_RTK;
+			fix_status = GNSS_FIX_STATUS_DGNSS_FIX;
+		} else if (nav_pvt->flags & UBX_NAV_PVT_FLAGS_GNSS_CARR_SOLN_FIXED) {
+			fix_quality = GNSS_FIX_QUALITY_RTK;
+			fix_status = GNSS_FIX_STATUS_DGNSS_FIX;
+		} else if (
+			(nav_pvt->fix_type == UBX_NAV_FIX_TYPE_GNSS_DR_COMBINED) ||
+			(nav_pvt->fix_type == UBX_NAV_FIX_TYPE_DR)) {
+
 			fix_quality = GNSS_FIX_QUALITY_ESTIMATED;
 			fix_status = GNSS_FIX_STATUS_ESTIMATED_FIX;
-			break;
-		case UBX_NAV_FIX_TYPE_2D:
-		case UBX_NAV_FIX_TYPE_3D:
+		} else if (
+			(nav_pvt->fix_type == UBX_NAV_FIX_TYPE_2D) ||
+			(nav_pvt->fix_type == UBX_NAV_FIX_TYPE_3D)) {
+
 			fix_quality = GNSS_FIX_QUALITY_GNSS_SPS;
 			fix_status = GNSS_FIX_STATUS_GNSS_FIX;
-			break;
-		default:
-			break;
 		}
 	}
 
@@ -127,6 +132,7 @@ void gnss_ubx_common_satellite_callback(struct modem_ubx *ubx, const struct ubx_
 			.azimuth = ubx_sat->sat[i].azimuth,
 			.system = gnss_system,
 			.is_tracked = (ubx_sat->sat[i].flags & UBX_NAV_SAT_FLAGS_SV_USED),
+			.is_corrected = (ubx_sat->sat[i].flags & UBX_NAV_SAT_FLAGS_RTCM_CORR_USED)
 		};
 
 		data->satellites.data[i] = sat;

include/zephyr/drivers/gnss.h

@@ -211,6 +211,8 @@ struct gnss_satellite {
 	enum gnss_system system;
 	/** True if satellite is being tracked */
 	uint8_t is_tracked : 1;
+	/** True if satellite tracking has RTK corrections */
+	uint8_t is_corrected : 1;
 };
 
 /** Template for GNSS satellites callback */

include/zephyr/modem/ubx/keys.h

@@ -24,6 +24,7 @@ enum ubx_keys_msg_out {
 	UBX_KEY_MSG_OUT_NMEA_ZDA_UART1 = 0x209100d9,
 	UBX_KEY_MSG_OUT_UBX_NAV_PVT_UART1 = 0x20910007,
 	UBX_KEY_MSG_OUT_UBX_NAV_SAT_UART1 = 0x20910016,
+	UBX_KEY_MSG_OUT_UBX_RXM_RTCM_UART1 = 0x20910269,
 };
 
 enum ubx_keys_rate {
@@ -36,4 +37,17 @@ enum ubx_keys_nav_cfg {
 	UBX_KEY_NAV_CFG_DYN_MODEL = 0x20110021,
 };
 
+enum ubx_keys_uart1_proto {
+	UBX_KEY_UART1_PROTO_IN_UBX = 0x10730001,
+	UBX_KEY_UART1_PROTO_IN_NMEA = 0x10730002,
+	UBX_KEY_UART1_PROTO_IN_RTCM3X = 0x10730004,
+	UBX_KEY_UART1_PROTO_OUT_UBX = 0x10740001,
+	UBX_KEY_UART1_PROTO_OUT_NMEA = 0x10740002,
+	UBX_KEY_UART1_PROTO_OUT_RTCM3X = 0x10740004,
+};
+
+enum ubx_keys_nav_hp_cfg {
+	UBX_KEY_NAV_HP_CFG_GNSS_MODE = 0x20140011,
+};
+
 #endif /* ZEPHYR_MODEM_UBX_KEYS_ */

include/zephyr/modem/ubx/protocol.h

@@ -74,12 +74,19 @@ enum ubx_nav_fix_type {
 	UBX_NAV_FIX_TYPE_TIME_ONLY = 5,
 };
 
+enum ubx_nav_hp_dgnss_mode {
+	UBX_NAV_HP_DGNSS_MODE_RTK_FLOAT = 2,
+	UBX_NAV_HP_DGNSS_MODE_RTK_FIXED = 3,
+};
+
 #define UBX_NAV_PVT_VALID_DATE				BIT(0)
 #define UBX_NAV_PVT_VALID_TIME				BIT(1)
 #define UBX_NAV_PVT_VALID_UTC_TOD			BIT(2)
 #define UBX_NAV_PVT_VALID_MAGN				BIT(3)
 
 #define UBX_NAV_PVT_FLAGS_GNSS_FIX_OK			BIT(0)
+#define UBX_NAV_PVT_FLAGS_GNSS_CARR_SOLN_FLOATING	BIT(6)
+#define UBX_NAV_PVT_FLAGS_GNSS_CARR_SOLN_FIXED		BIT(7)
 
 #define UBX_NAV_PVT_FLAGS3_INVALID_LLH			BIT(0)
 
@@ -143,6 +150,7 @@ enum ubx_gnss_id {
 };
 
 #define UBX_NAV_SAT_FLAGS_SV_USED			BIT(3)
+#define UBX_NAV_SAT_FLAGS_RTCM_CORR_USED		BIT(17)
 
 struct ubx_nav_sat {
 	uint32_t itow;