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
@@ -122,7 +138,7 @@ static int ubx_f9p_msg_get(const struct device *dev, const struct ubx_frame *req
 	data->script.inst.retry_count = 2;
 	data->script.inst.match.filter.class = req->class;
 	data->script.inst.match.filter.id = req->id;
-	data->script.inst.request.buf = req;
+	data->script.inst.request.buf = (const uint8_t *)req;
 	data->script.inst.request.len = len;
 
 	err = modem_ubx_run_script(&data->ubx.inst, &data->script.inst);
@@ -153,7 +169,7 @@ static int ubx_f9p_msg_send(const struct device *dev, const struct ubx_frame *re
 	data->script.inst.retry_count = wait_for_ack ? 2 : 0;
 	data->script.inst.match.filter.class = wait_for_ack ? UBX_CLASS_ID_ACK : 0;
 	data->script.inst.match.filter.id = UBX_MSG_ID_ACK;
-	data->script.inst.request.buf = req;
+	data->script.inst.request.buf = (const uint8_t *)req;
 	data->script.inst.request.len = len;
 
 	err = modem_ubx_run_script(&data->ubx.inst, &data->script.inst);
@@ -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,10 @@ static DEVICE_API(gnss, ublox_f9p_driver_api) = {
 												   \
 	static struct ubx_f9p_data ubx_f9p_data_##inst;						   \
 												   \
+	COND_CODE_1(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_u_blox_m8.c

@@ -121,7 +121,7 @@ static int ubx_m8_msg_get(const struct device *dev, const struct ubx_frame *req,
 	data->script.inst.retry_count = 2;
 	data->script.inst.match.filter.class = req->class;
 	data->script.inst.match.filter.id = req->id;
-	data->script.inst.request.buf = req;
+	data->script.inst.request.buf = (const uint8_t *)req;
 	data->script.inst.request.len = len;
 
 	err = modem_ubx_run_script(&data->ubx.inst, &data->script.inst);
@@ -153,7 +153,7 @@ static int ubx_m8_msg_send(const struct device *dev, const struct ubx_frame *req
 	data->script.inst.retry_count = wait_for_ack ? 2 : 0;
 	data->script.inst.match.filter.class = wait_for_ack ? UBX_CLASS_ID_ACK : 0;
 	data->script.inst.match.filter.id = UBX_MSG_ID_ACK;
-	data->script.inst.request.buf = req;
+	data->script.inst.request.buf = (const uint8_t *)req;
 	data->script.inst.request.len = len;
 
 	err = modem_ubx_run_script(&data->ubx.inst, &data->script.inst);

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.h

@@ -53,7 +53,7 @@ struct modem_ubx_match {
 
 struct modem_ubx_script {
 	struct {
-		const struct ubx_frame *buf;
+		const uint8_t *buf;
 		uint16_t len;
 	} request;
 	struct {

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;

samples/drivers/gnss/rtk/src/main.c

@@ -22,9 +22,10 @@ static void gnss_data_cb(const struct device *dev, const struct gnss_data *data)
 	if (data->info.fix_status != GNSS_FIX_STATUS_NO_FIX) {
 		if (gnss_get_latest_timepulse(dev, &timepulse) == 0) {
 			timepulse_ns = k_ticks_to_ns_near64(timepulse);
-			printf("Got a fix @ %lld ns\n", timepulse_ns);
+			printf("Got a fix (type: %d) @ %lld ns\n",
+				data->info.fix_status, timepulse_ns);
 		} else {
-			printf("Got a fix!\n");
+			printf("Got a fix (type: %d)\n", data->info.fix_status);
 		}
 	}
 }
@@ -35,12 +36,14 @@ static void gnss_satellites_cb(const struct device *dev, const struct gnss_satel
 			       uint16_t size)
 {
 	unsigned int tracked_count = 0;
+	unsigned int corrected_count = 0;
 
 	for (unsigned int i = 0; i != size; ++i) {
 		tracked_count += satellites[i].is_tracked;
+		corrected_count += satellites[i].is_corrected;
 	}
-	printf("%u satellite%s reported (of which %u tracked)!\n",
-		size, size > 1 ? "s" : "", tracked_count);
+	printf("%u satellite%s reported (of which %u tracked, of which %u has RTK corrections)!\n",
+		size, size > 1 ? "s" : "", tracked_count, corrected_count);
 }
 #endif
 GNSS_SATELLITES_CALLBACK_DEFINE(GNSS_MODEM, gnss_satellites_cb);