gnss: rtk: Add basic integration

ubieda · ubieda · commit 96f80d2bffe4 · 2025-07-08T16:39:52.000-04:00
Incorporate the basic RTK API as well as a basic client integration
(serial) as a way to receive and publish the RTK data.

Signed-off-by: Luis Ubieda &lt;luisf@croxel.com&gt;
diff --git a/cmake/linker_script/common/common-rom.cmake b/cmake/linker_script/common/common-rom.cmake
@@ -259,3 +259,7 @@ endif()
 if(CONFIG_GNSS_SATELLITES)
   zephyr_iterable_section(NAME gnss_satellites_callback KVMA RAM_REGION GROUP RODATA_REGION)
 endif()
+
+if(CONFIG_GNSS_RTK)
+  zephyr_iterable_section(NAME gnss_rtk_data_callback KVMA RAM_REGION GROUP RODATA_REGION)
+endif()
diff --git a/include/zephyr/gnss/rtk/decoder.h b/include/zephyr/gnss/rtk/decoder.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2025 Croxel Inc.
+ * Copyright (c) 2025 CogniPilot Foundation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef ZEPHYR_INCLUDE_GNSS_RTK_DECODER_H_
+#define ZEPHYR_INCLUDE_GNSS_RTK_DECODER_H_
+
+#include <stdint.h>
+#include <stddef.h>
+
+/**
+ * @brief Get an RTK frame from buffer
+ *
+ * Used by RTK clients to extract frames from a data-buffer.
+ *
+ * @param[in] buf Buffer holding encoded data.
+ * @param[in] buf_len Buffer length.
+ * @param[out] data Pointer to the decoded frame.
+ * @param[out] data_len Length of the decoded frame
+ *
+ * @return Zero if successful.
+ * @return -ENOENT if no frames have been decoded successfully.
+ * @return -EAGAIN if there's an incomplete frame starting at the data pointer.
+ * @return Other negative error code if decoding failed.
+ */
+int gnss_rtk_decoder_frame_get(uint8_t *buf, size_t buf_len,
+			       uint8_t **data, size_t *data_len);
+
+#endif /* ZEPHYR_INCLUDE_GNSS_RTK_DECODER_H_ */
diff --git a/include/zephyr/gnss/rtk/rtk.h b/include/zephyr/gnss/rtk/rtk.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2023 Trackunit Corporation
+ * Copyright (c) 2025 Croxel Inc.
+ * Copyright (c) 2025 CogniPilot Foundation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef ZEPHYR_INCLUDE_GNSS_RTK_RTK_H_
+#define ZEPHYR_INCLUDE_GNSS_RTK_RTK_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stddef.h>
+
+struct gnss_rtk_data {
+	const uint8_t *data;
+	size_t len;
+};
+
+typedef void (*gnss_rtk_data_callback_t)(const struct device *dev,
+					 const struct gnss_rtk_data *data);
+
+struct gnss_rtk_data_callback {
+	const struct device *dev;
+	gnss_rtk_data_callback_t callback;
+};
+
+#if CONFIG_GNSS_RTK
+#define GNSS_RTK_DATA_CALLBACK_DEFINE(_dev, _callback)						   \
+	static const STRUCT_SECTION_ITERABLE(gnss_rtk_data_callback,				   \
+					     _gnss_rtk_data_callback__##_callback) = {		   \
+		.dev = _dev,									   \
+		.callback = _callback,								   \
+	}
+#else
+#define GNSS_RTK_DATA_CALLBACK_DEFINE(_dev, _callback)
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* ZEPHYR_INCLUDE_GNSS_RTK_RTK_H_ */
diff --git a/include/zephyr/gnss/rtk/rtk_publish.h b/include/zephyr/gnss/rtk/rtk_publish.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright (c) 2025 Croxel Inc.
+ * Copyright (c) 2025 CogniPilot Foundation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef ZEPHYR_INCLUDE_GNSS_RTK_PUBLISH_H_
+#define ZEPHYR_INCLUDE_GNSS_RTK_PUBLISH_H_
+
+#include <stdint.h>
+#include <stddef.h>
+#include <zephyr/gnss/rtk/rtk.h>
+
+/* Internal function used by RTK clients to publish data-correction. */
+void gnss_rtk_publish_data(const struct gnss_rtk_data *data);
+
+#endif /* ZEPHYR_INCLUDE_GNSS_RTK_PUBLISH_H_ */
diff --git a/include/zephyr/linker/common-rom/common-rom-misc.ld b/include/zephyr/linker/common-rom/common-rom-misc.ld
@@ -68,3 +68,7 @@
 #if defined(CONFIG_GNSS_SATELLITES)
 	ITERABLE_SECTION_ROM(gnss_satellites_callback, Z_LINK_ITERABLE_SUBALIGN)
 #endif
+
+#if defined(CONFIG_GNSS_RTK)
+	ITERABLE_SECTION_ROM(gnss_rtk_data_callback, Z_LINK_ITERABLE_SUBALIGN)
+#endif
diff --git a/subsys/CMakeLists.txt b/subsys/CMakeLists.txt
@@ -16,6 +16,7 @@ add_subdirectory(canbus)
 add_subdirectory(debug)
 add_subdirectory(fb)
 add_subdirectory(fs)
+add_subdirectory(gnss)
 add_subdirectory(ipc)
 add_subdirectory(logging)
 add_subdirectory(mem_mgmt)
diff --git a/subsys/Kconfig b/subsys/Kconfig
@@ -20,6 +20,7 @@ source "subsys/dsp/Kconfig"
 source "subsys/emul/Kconfig"
 source "subsys/fb/Kconfig"
 source "subsys/fs/Kconfig"
+source "subsys/gnss/Kconfig"
 source "subsys/input/Kconfig"
 source "subsys/ipc/Kconfig"
 source "subsys/jwt/Kconfig"
diff --git a/subsys/gnss/CMakeLists.txt b/subsys/gnss/CMakeLists.txt
@@ -0,0 +1,5 @@
+# Copyright (c) 2025 Croxel Inc.
+# Copyright (c) 2025 CogniPilot Foundation
+# SPDX-License-Identifier: Apache-2.0
+
+add_subdirectory_ifdef(CONFIG_GNSS_RTK rtk)
diff --git a/subsys/gnss/Kconfig b/subsys/gnss/Kconfig
@@ -0,0 +1,5 @@
+# Copyright (c) 2025 Croxel Inc.
+# Copyright (c) 2025 CogniPilot Foundation
+# SPDX-License-Identifier: Apache-2.0
+
+rsource "rtk/Kconfig"
diff --git a/subsys/gnss/rtk/CMakeLists.txt b/subsys/gnss/rtk/CMakeLists.txt
@@ -0,0 +1,9 @@
+# Copyright (c) 2025 Croxel Inc.
+# Copyright (c) 2025 CogniPilot Foundation
+# SPDX-License-Identifier: Apache-2.0
+
+zephyr_library()
+zephyr_library_sources(rtk.c)
+
+add_subdirectory(protocol)
+add_subdirectory_ifdef(CONFIG_GNSS_RTK_SERIAL serial)
diff --git a/subsys/gnss/rtk/Kconfig b/subsys/gnss/rtk/Kconfig
@@ -0,0 +1,20 @@
+# Copyright (c) 2025 Croxel Inc.
+# Copyright (c) 2025 CogniPilot Foundation
+# SPDX-License-Identifier: Apache-2.0
+
+menuconfig GNSS_RTK
+	bool "GNSS RTK client"
+	select EXPERIMENTAL
+	help
+	  Enable GNSS RTK data-correction clients
+
+if GNSS_RTK
+
+rsource "protocol/Kconfig"
+rsource "serial/Kconfig"
+
+module = GNSS_RTK
+module-str = GNSS RTK
+source "subsys/logging/Kconfig.template.log_config"
+
+endif
diff --git a/subsys/gnss/rtk/decoder.h b/subsys/gnss/rtk/decoder.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) 2025 Croxel Inc.
+ * Copyright (c) 2025 CogniPilot Foundation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef ZEPHYR_SUBSYS_RTK_DECODER_H_
+#define ZEPHYR_SUBSYS_RTK_DECODER_H_
+
+#include <stdint.h>
+#include <stdlib.h>
+
+/**
+ * @brief Decode RTK frame
+ *
+ * @note The expected use is to call this call iteratively until all the
+ * frames present in the incoming buffer have been decoded.
+ *
+ * @param[in] buf Buffer holding encoded data.
+ * @param[in] buf_len Buffer length.
+ * @param[out] data Pointer to the decoded frame.
+ * @param[out] data_len Length of the decoded frame
+ *
+ * @return Zero if successful.
+ * @return -ENOENT if no frames have been decoded successfully.
+ * @return -EAGAIN if there's an incomplete frame starting at the data pointer.
+ * @return Other negative error code if decoding failed.
+ */
+int rtk_decoder_frame_get(const uint8_t *buf, size_t buf_len,
+			  uint8_t **data, size_t *data_len);
+
+#endif /* ZEPHYR_SUBSYS_RTK_DECODER_H_ */
diff --git a/subsys/gnss/rtk/protocol/CMakeLists.txt b/subsys/gnss/rtk/protocol/CMakeLists.txt
@@ -0,0 +1,5 @@
+# Copyright (c) 2025 Croxel Inc.
+# Copyright (c) 2025 CogniPilot Foundation
+# SPDX-License-Identifier: Apache-2.0
+
+zephyr_library_sources_ifdef(CONFIG_GNSS_RTK_PROTOCOL_RTCM3 rtcm3.c)
diff --git a/subsys/gnss/rtk/protocol/Kconfig b/subsys/gnss/rtk/protocol/Kconfig
@@ -0,0 +1,17 @@
+# Copyright (c) 2025 Croxel Inc.
+# Copyright (c) 2025 CogniPilot Foundation
+# SPDX-License-Identifier: Apache-2.0
+
+choice GNSS_RTK_PROTOCOL
+	prompt "GNSS RTK Protocol selection"
+	default GNSS_RTK_PROTOCOL_RTCM3
+	help
+	  Select the GNSS RTK Protocol to use in data-correction
+
+config GNSS_RTK_PROTOCOL_RTCM3
+	bool "RTCM3 Protocol"
+	select CRC
+	help
+	  Select RTCM3 protocol as GNSS RTK corrections
+
+endchoice
diff --git a/subsys/gnss/rtk/protocol/rtcm3.c b/subsys/gnss/rtk/protocol/rtcm3.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2025 Croxel Inc.
+ * Copyright (c) 2025 CogniPilot Foundation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <errno.h>
+#include <zephyr/sys/byteorder.h>
+#include <zephyr/sys/util.h>
+#include <zephyr/gnss/rtk/decoder.h>
+#include <zephyr/sys/crc.h>
+
+#define RTCM3_FRAME_SYNC_SZ		1
+#define RTCM3_FRAME_HDR_SZ		2
+#define RTCM3_FRAME_CHECKSUM_SZ		3
+#define RTCM3_FRAME_OVERHEAD		RTCM3_FRAME_SYNC_SZ + RTCM3_FRAME_HDR_SZ + \
+					RTCM3_FRAME_CHECKSUM_SZ
+
+#define RTCM3_SYNC_BYTE 0xD3
+
+#define RTCM3_FRAME_PAYLOAD_SZ(hdr)	(sys_be16_to_cpu(hdr) & BIT_MASK(10))
+#define RTCM3_FRAME_SZ(payload_len) ((payload_len) + RTCM3_FRAME_OVERHEAD)
+
+struct rtcm3_frame {
+	uint8_t sync_frame;
+	uint16_t hdr;
+	uint8_t payload[];
+} __packed;
+
+int gnss_rtk_decoder_frame_get(uint8_t *buf, size_t buf_len,
+			       uint8_t **data, size_t *data_len)
+{
+	for (int i = 0 ; (i + RTCM3_FRAME_OVERHEAD - 1) < buf_len ; i++) {
+		if (buf[i] != RTCM3_SYNC_BYTE) {
+			continue;
+		}
+
+		const struct rtcm3_frame *frame = (const struct rtcm3_frame *)&buf[i];
+		uint16_t payload_len = RTCM3_FRAME_PAYLOAD_SZ(frame->hdr);
+		uint16_t remaining_bytes = buf_len - i;
+
+		if ((payload_len == 0) ||
+			((RTCM3_FRAME_SZ(payload_len)) > remaining_bytes)) {
+			continue;
+		}
+
+		if (crc24q_rtcm3((const uint8_t *)frame,
+				 RTCM3_FRAME_SZ(payload_len)) == 0) {
+			*data = (uint8_t *)frame;
+			*data_len = RTCM3_FRAME_SZ(payload_len);
+
+			return 0;
+		}
+	}
+
+	return -ENOENT;
+}
diff --git a/subsys/gnss/rtk/rtk.c b/subsys/gnss/rtk/rtk.c
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2023 Trackunit Corporation
+ * Copyright (c) 2025 Croxel Inc.
+ * Copyright (c) 2025 CogniPilot Foundation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <zephyr/kernel.h>
+#include <zephyr/sys/iterable_sections.h>
+#include <zephyr/gnss/rtk/rtk.h>
+
+void gnss_rtk_publish_data(const struct gnss_rtk_data *data)
+{
+	static K_SEM_DEFINE(publish_lock, 1, 1);
+
+	(void)k_sem_take(&publish_lock, K_FOREVER);
+
+	STRUCT_SECTION_FOREACH(gnss_rtk_data_callback, callback) {
+		callback->callback(callback->dev, data);
+	}
+
+	k_sem_give(&publish_lock);
+}
diff --git a/subsys/gnss/rtk/serial/CMakeLists.txt b/subsys/gnss/rtk/serial/CMakeLists.txt
@@ -0,0 +1,5 @@
+# Copyright (c) 2025 Croxel Inc.
+# Copyright (c) 2025 CogniPilot Foundation
+# SPDX-License-Identifier: Apache-2.0
+
+zephyr_library_sources(serial.c)
diff --git a/subsys/gnss/rtk/serial/Kconfig b/subsys/gnss/rtk/serial/Kconfig
@@ -0,0 +1,10 @@
+# Copyright (c) 2025 Croxel Inc.
+# Copyright (c) 2025 CogniPilot Foundation
+# SPDX-License-Identifier: Apache-2.0
+
+config GNSS_RTK_SERIAL
+	bool "Serial GNSS RTK client"
+	select SERIAL
+	select UART_INTERRUPT_DRIVEN
+	help
+	  Use RTK Serial client to obtain data-correction.
diff --git a/subsys/gnss/rtk/serial/serial.c b/subsys/gnss/rtk/serial/serial.c
