diff --git a/cmake/linker_script/common/common-rom.cmake b/cmake/linker_script/common/common-rom.cmake index 087e106c8e6..bd51589e64b 100644 --- a/cmake/linker_script/common/common-rom.cmake +++ b/cmake/linker_script/common/common-rom.cmake @@ -258,3 +258,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 new file mode 100644 index 00000000000..477d9b9e121 --- /dev/null +++ b/include/zephyr/gnss/rtk/decoder.h @@ -0,0 +1,31 @@ +/* + * 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 +#include + +/** + * @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 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 new file mode 100644 index 00000000000..dc783edfca0 --- /dev/null +++ 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 +#include + +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 new file mode 100644 index 00000000000..1a0d776f6fb --- /dev/null +++ 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 +#include +#include + +/* 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 index 1559d1463eb..3c1f8922fba 100644 --- 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 index a0a2bc828cf..340c7d09e3d 100644 --- 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 index a85bda6ece4..db6990eb1d0 100644 --- 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 new file mode 100644 index 00000000000..e0661e1dc50 --- /dev/null +++ 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 new file mode 100644 index 00000000000..6ccd3b31e41 --- /dev/null +++ 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 new file mode 100644 index 00000000000..b5f6627bc49 --- /dev/null +++ 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 new file mode 100644 index 00000000000..8d8936f1734 --- /dev/null +++ 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/protocol/CMakeLists.txt b/subsys/gnss/rtk/protocol/CMakeLists.txt new file mode 100644 index 00000000000..ff443acb354 --- /dev/null +++ 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 new file mode 100644 index 00000000000..e10e36373e5 --- /dev/null +++ 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 new file mode 100644 index 00000000000..452ad5e9ffe --- /dev/null +++ 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 +#include +#include +#include +#include + +#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 (size_t i = 0 ; (i + RTCM3_FRAME_OVERHEAD - 1) < buf_len ; i++) { + if (buf[i] != RTCM3_SYNC_BYTE) { + continue; + } + + struct rtcm3_frame *frame = (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 new file mode 100644 index 00000000000..35fba6f13c6 --- /dev/null +++ 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 +#include +#include + +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 new file mode 100644 index 00000000000..80b5191f8bf --- /dev/null +++ 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 new file mode 100644 index 00000000000..44b8fae61c9 --- /dev/null +++ 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 new file mode 100644 index 00000000000..c6ba5d51a90 --- /dev/null +++ b/subsys/gnss/rtk/serial/serial.c @@ -0,0 +1,101 @@ +/* + * Copyright (c) 2025 Croxel Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +LOG_MODULE_REGISTER(rtk_serial, CONFIG_GNSS_RTK_LOG_LEVEL); + +static const struct device *rtk_serial_dev = DEVICE_DT_GET(DT_CHOSEN(zephyr_rtk_serial)); +static struct ring_buf process_ringbuf; +static uint8_t process_buf[2048]; + +static void gnss_rtk_process_work_handler(struct k_work *work) +{ + static uint8_t work_buf[2048]; + uint32_t len = ring_buf_get(&process_ringbuf, work_buf, sizeof(work_buf)); + uint32_t offset = 0; + + ARG_UNUSED(work); + + do { + uint8_t *frame; + size_t frame_len; + int err; + + err = gnss_rtk_decoder_frame_get(work_buf + offset, len - offset, + &frame, &frame_len); + if (err != 0) { + return; + } + + LOG_HEXDUMP_DBG(frame, frame_len, "Frame received"); + + /* Publish results */ + struct gnss_rtk_data rtk_data = { + .data = frame, + .len = frame_len, + }; + + gnss_rtk_publish_data(&rtk_data); + offset += frame_len; + + } while (len > offset); +} + +static K_WORK_DELAYABLE_DEFINE(gnss_rtk_process_work, gnss_rtk_process_work_handler); + +static void rtk_uart_isr_callback(const struct device *dev, void *user_data) +{ + ARG_UNUSED(user_data); + + (void)uart_irq_update(dev); + + if (uart_irq_rx_ready(dev)) { + int ret; + + do { + char c; + + ret = uart_fifo_read(dev, &c, 1); + if (ret > 0) { + ret = ring_buf_put(&process_ringbuf, &c, 1); + } + } while (ret > 0); + + /** Since messages come through in a burst at a period + * (e.g: 1 Hz), wait until all messages are received before + * processing. + */ + (void)k_work_reschedule(&gnss_rtk_process_work, K_MSEC(10)); + } +} + +static int rtk_serial_client_init(void) +{ + int err; + + ring_buf_init(&process_ringbuf, ARRAY_SIZE(process_buf), process_buf); + + err = uart_irq_callback_user_data_set(rtk_serial_dev, rtk_uart_isr_callback, NULL); + if (err < 0) { + return err; + } + + uart_irq_rx_enable(rtk_serial_dev); + + return 0; +} + +SYS_INIT(rtk_serial_client_init, APPLICATION, CONFIG_APPLICATION_INIT_PRIORITY);