diff --git a/MAINTAINERS.yml b/MAINTAINERS.yml index b0d5ab3c6a9cf..e8f710ec082d4 100644 --- a/MAINTAINERS.yml +++ b/MAINTAINERS.yml @@ -1569,21 +1569,26 @@ Release Notes: collaborators: - tomi-font - fabiobaltieri + - ubieda files: - doc/hardware/peripherals/gnss.rst - drivers/gnss/ - include/zephyr/data/navigation.h - include/zephyr/drivers/gnss.h - include/zephyr/drivers/gnss/ + - include/zephyr/gnss/ - dts/bindings/gnss/ - include/zephyr/dt-bindings/gnss/ - tests/drivers/build_all/gnss/ - tests/drivers/gnss/ + - tests/subsys/gnss/ - samples/drivers/gnss/ + - subsys/gnss/ labels: - "area: GNSS" tests: - drivers.gnss + - gnss "Drivers: Haptics": status: maintained diff --git a/cmake/linker_script/common/common-rom.cmake b/cmake/linker_script/common/common-rom.cmake index a6d3e09bf2644..dd15bb5d8762f 100644 --- 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/drivers/gnss/Kconfig.u_blox_f9p b/drivers/gnss/Kconfig.u_blox_f9p index d14c3e2166c06..81ecb66e339e2 100644 --- a/drivers/gnss/Kconfig.u_blox_f9p +++ b/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 diff --git a/drivers/gnss/gnss_u_blox_f9p.c b/drivers/gnss/gnss_u_blox_f9p.c index eb35f593187c5..e08a738b9894d 100644 --- a/drivers/gnss/gnss_u_blox_f9p.c +++ b/drivers/gnss/gnss_u_blox_f9p.c @@ -18,6 +18,7 @@ #include #include "gnss_ubx_common.h" +#include #include 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 @@ -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 void *)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,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, \ diff --git a/drivers/gnss/gnss_u_blox_m8.c b/drivers/gnss/gnss_u_blox_m8.c index 428a9bec3e198..62d3c065b7722 100644 --- a/drivers/gnss/gnss_u_blox_m8.c +++ b/drivers/gnss/gnss_u_blox_m8.c @@ -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 void *)req; data->script.inst.request.len = len; err = modem_ubx_run_script(&data->ubx.inst, &data->script.inst); diff --git a/drivers/gnss/gnss_ubx_common.c b/drivers/gnss/gnss_ubx_common.c index 5f3a90da146f1..15f6ddf1263ec 100644 --- a/drivers/gnss/gnss_ubx_common.c +++ b/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; diff --git a/include/zephyr/drivers/gnss.h b/include/zephyr/drivers/gnss.h index 3a0ba8ba1b65a..bb6fc835e2834 100644 --- a/include/zephyr/drivers/gnss.h +++ b/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 */ diff --git a/include/zephyr/gnss/rtk/decoder.h b/include/zephyr/gnss/rtk/decoder.h new file mode 100644 index 0000000000000..e447b29b53173 --- /dev/null +++ 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 +#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 -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 new file mode 100644 index 0000000000000..dc783edfca06f --- /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 0000000000000..1a0d776f6fb13 --- /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 1559d1463ebd6..3c1f8922fba48 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/include/zephyr/modem/ubx.h b/include/zephyr/modem/ubx.h index 3b6493ee80ba8..fd4b05cea7475 100644 --- a/include/zephyr/modem/ubx.h +++ b/include/zephyr/modem/ubx.h @@ -53,7 +53,7 @@ struct modem_ubx_match { struct modem_ubx_script { struct { - const struct ubx_frame *buf; + const void *buf; uint16_t len; } request; struct { diff --git a/include/zephyr/modem/ubx/keys.h b/include/zephyr/modem/ubx/keys.h index 1b8100582c7e3..536cdf7d1064c 100644 --- a/include/zephyr/modem/ubx/keys.h +++ b/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_ */ diff --git a/include/zephyr/modem/ubx/protocol.h b/include/zephyr/modem/ubx/protocol.h index 9e40a29caf617..747edad32b10d 100644 --- a/include/zephyr/modem/ubx/protocol.h +++ b/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; diff --git a/include/zephyr/sys/crc.h b/include/zephyr/sys/crc.h index 052bb0ba459a8..1337326f1ccc7 100644 --- a/include/zephyr/sys/crc.h +++ b/include/zephyr/sys/crc.h @@ -391,6 +391,17 @@ uint32_t crc24_pgp(const uint8_t *data, size_t len); */ uint32_t crc24_pgp_update(uint32_t crc, const uint8_t *data, size_t len); +/** + * @brief Calculate an RTCM3 CRC24Q frame checksum + * + * @param[in] data RTCM3 Frame + * @param[in] len Frame length in bytes. + * + * @return 0 if the data-frame contains a checksum and it matches. + * @return Result if data-frame does not contain checksum. + */ +uint32_t crc24q_rtcm3(const uint8_t *data, size_t len); + /** * @brief Compute a CRC checksum, in a generic way. * diff --git a/lib/crc/crc24_sw.c b/lib/crc/crc24_sw.c index 78c2c0517f83b..5a9f52aba3dfa 100644 --- a/lib/crc/crc24_sw.c +++ b/lib/crc/crc24_sw.c @@ -1,5 +1,8 @@ /* * Copyright (C) 2024 BayLibre. + * Copyright (c) 2020 Brandon Owen. + * Copyright (c) 2025 Croxel Inc. + * Copyright (c) 2025 CogniPilot Foundation. * * SPDX-License-Identifier: Apache-2.0 */ @@ -30,3 +33,62 @@ uint32_t crc24_pgp_update(uint32_t crc, const uint8_t *data, size_t len) return crc; } + +/** This code is originally extracted from https://github.com/go-gnss/rtcm, + * licensed as Apache-2.0. + */ + +static const uint32_t crc24q[] = { + 0x000000, 0x864cfb, 0x8ad50d, 0x0c99f6, 0x93e6e1, 0x15aa1a, 0x1933ec, 0x9f7f17, + 0xa18139, 0x27cdc2, 0x2b5434, 0xad18cf, 0x3267d8, 0xb42b23, 0xb8b2d5, 0x3efe2e, + 0xc54e89, 0x430272, 0x4f9b84, 0xc9d77f, 0x56a868, 0xd0e493, 0xdc7d65, 0x5a319e, + 0x64cfb0, 0xe2834b, 0xee1abd, 0x685646, 0xf72951, 0x7165aa, 0x7dfc5c, 0xfbb0a7, + 0x0cd1e9, 0x8a9d12, 0x8604e4, 0x00481f, 0x9f3708, 0x197bf3, 0x15e205, 0x93aefe, + 0xad50d0, 0x2b1c2b, 0x2785dd, 0xa1c926, 0x3eb631, 0xb8faca, 0xb4633c, 0x322fc7, + 0xc99f60, 0x4fd39b, 0x434a6d, 0xc50696, 0x5a7981, 0xdc357a, 0xd0ac8c, 0x56e077, + 0x681e59, 0xee52a2, 0xe2cb54, 0x6487af, 0xfbf8b8, 0x7db443, 0x712db5, 0xf7614e, + 0x19a3d2, 0x9fef29, 0x9376df, 0x153a24, 0x8a4533, 0x0c09c8, 0x00903e, 0x86dcc5, + 0xb822eb, 0x3e6e10, 0x32f7e6, 0xb4bb1d, 0x2bc40a, 0xad88f1, 0xa11107, 0x275dfc, + 0xdced5b, 0x5aa1a0, 0x563856, 0xd074ad, 0x4f0bba, 0xc94741, 0xc5deb7, 0x43924c, + 0x7d6c62, 0xfb2099, 0xf7b96f, 0x71f594, 0xee8a83, 0x68c678, 0x645f8e, 0xe21375, + 0x15723b, 0x933ec0, 0x9fa736, 0x19ebcd, 0x8694da, 0x00d821, 0x0c41d7, 0x8a0d2c, + 0xb4f302, 0x32bff9, 0x3e260f, 0xb86af4, 0x2715e3, 0xa15918, 0xadc0ee, 0x2b8c15, + 0xd03cb2, 0x567049, 0x5ae9bf, 0xdca544, 0x43da53, 0xc596a8, 0xc90f5e, 0x4f43a5, + 0x71bd8b, 0xf7f170, 0xfb6886, 0x7d247d, 0xe25b6a, 0x641791, 0x688e67, 0xeec29c, + 0x3347a4, 0xb50b5f, 0xb992a9, 0x3fde52, 0xa0a145, 0x26edbe, 0x2a7448, 0xac38b3, + 0x92c69d, 0x148a66, 0x181390, 0x9e5f6b, 0x01207c, 0x876c87, 0x8bf571, 0x0db98a, + 0xf6092d, 0x7045d6, 0x7cdc20, 0xfa90db, 0x65efcc, 0xe3a337, 0xef3ac1, 0x69763a, + 0x578814, 0xd1c4ef, 0xdd5d19, 0x5b11e2, 0xc46ef5, 0x42220e, 0x4ebbf8, 0xc8f703, + 0x3f964d, 0xb9dab6, 0xb54340, 0x330fbb, 0xac70ac, 0x2a3c57, 0x26a5a1, 0xa0e95a, + 0x9e1774, 0x185b8f, 0x14c279, 0x928e82, 0x0df195, 0x8bbd6e, 0x872498, 0x016863, + 0xfad8c4, 0x7c943f, 0x700dc9, 0xf64132, 0x693e25, 0xef72de, 0xe3eb28, 0x65a7d3, + 0x5b59fd, 0xdd1506, 0xd18cf0, 0x57c00b, 0xc8bf1c, 0x4ef3e7, 0x426a11, 0xc426ea, + 0x2ae476, 0xaca88d, 0xa0317b, 0x267d80, 0xb90297, 0x3f4e6c, 0x33d79a, 0xb59b61, + 0x8b654f, 0x0d29b4, 0x01b042, 0x87fcb9, 0x1883ae, 0x9ecf55, 0x9256a3, 0x141a58, + 0xefaaff, 0x69e604, 0x657ff2, 0xe33309, 0x7c4c1e, 0xfa00e5, 0xf69913, 0x70d5e8, + 0x4e2bc6, 0xc8673d, 0xc4fecb, 0x42b230, 0xddcd27, 0x5b81dc, 0x57182a, 0xd154d1, + 0x26359f, 0xa07964, 0xace092, 0x2aac69, 0xb5d37e, 0x339f85, 0x3f0673, 0xb94a88, + 0x87b4a6, 0x01f85d, 0x0d61ab, 0x8b2d50, 0x145247, 0x921ebc, 0x9e874a, 0x18cbb1, + 0xe37b16, 0x6537ed, 0x69ae1b, 0xefe2e0, 0x709df7, 0xf6d10c, 0xfa48fa, 0x7c0401, + 0x42fa2f, 0xc4b6d4, 0xc82f22, 0x4e63d9, 0xd11cce, 0x575035, 0x5bc9c3, 0xdd8538 +}; + +/** + * @brief Calculate RTCM3 frame checksum + * + * @param[in] data RTCM3 Frame + * @param[in] size Frame length in bytes. + * + * @retval 0 if the data-frame contains a checksum and it matches. + * @retval Result if data-frame does not contain checksum. + */ +uint32_t crc24q_rtcm3(const uint8_t *data, size_t len) +{ + uint32_t crc = 0; + + for (uint32_t i = 0; i < len; ++i) { + crc = ((crc << 8) & 0xFFFFFF) ^ crc24q[data[i] ^ (crc >> 16)]; + } + + return crc; +} diff --git a/samples/drivers/gnss/README.rst b/samples/drivers/gnss/README.rst index 8d4749cd410a4..e6292096e76b9 100644 --- a/samples/drivers/gnss/README.rst +++ b/samples/drivers/gnss/README.rst @@ -26,4 +26,37 @@ Sample Output gnss has fix! gnss: gnss_satellite: {prn: 1, snr: 30, elevation 71, azimuth 276, system: GLONASS, is_tracked: 1} gnss: gnss_satellite: {prn: 11, snr: 31, elevation 62, azimuth 221, system: GLONASS, is_tracked: 1} - gnss reported 2 satellites! + gnss reported 2 satellites (of which 2 tracked, of which 0 has RTK corrections)! + +Real-Time Kinematics (RTK) +************************** + +This sample may also be configured to enable Real-Time Kinematics (RTK) positioning for +enhanced accuracy, with the assistance of a local base station. + +RTK Requirements +**************** + +This sample requires the following setup to work with RTK: + +* A UBlox F9P GNSS module connected to your board to act as a rover +* A second UBlox F9P module connected to a PC to act as a base station + +Base Station Setup +****************** + +To enable RTK functionality: + +1. Connect the base station F9P module to your PC via USB. +2. Also, connect the rover's serial port (running the sample) to your PC via USB. +3. Note the serial port the rover is connected through (e.g., /dev/ttyUSB0) +4. Run the base station script: + +.. code-block:: console + + python3 base_station/base_station_f9p.py --port /dev/ttyUSB0 + +The script configures the F9P module as a base station and streams RTCM3 +correction data to the rover. The base station will perform a survey-in +process to determine its precise position before starting to transmit +corrections. diff --git a/samples/drivers/gnss/base_station/base_station_f9p.py b/samples/drivers/gnss/base_station/base_station_f9p.py new file mode 100644 index 0000000000000..41a478d11e672 --- /dev/null +++ b/samples/drivers/gnss/base_station/base_station_f9p.py @@ -0,0 +1,83 @@ +# Copyright (c) 2025, Croxel Inc +# Copyright (c) 2025, CogniPilot Foundation +# +# SPDX-License-Identifier: Apache-2.0 + +import argparse + +import pyrtcm +import pyubx2 +import serial + +UBX_F9P_VID = 0x1546 +UBX_F9P_PID = 0x01A9 + + +def parse_args(): + parser = argparse.ArgumentParser(allow_abbrev=False) + parser.add_argument( + '-p', '--port', required=True, help='Serial Port connected to the Rover (e.g: /dev/ttyUSB0)' + ) + + return parser.parse_args() + + +def getF9PBaseStationSerialPort(): + from serial.tools import list_ports + + for port in list_ports.comports(): + if port.vid == UBX_F9P_VID and port.pid == UBX_F9P_PID: + return port.device + + raise RuntimeError( + f'Could not find any Serial Port with VID: {hex(UBX_F9P_VID)}, PID: {hex(UBX_F9P_PID)}' + ) + + +def enableBaseStationMode(serialEndpoint: serial.Serial): + frame = pyubx2.UBXMessage.config_set( + layers=1, + transaction=0, + cfgData=[ + ("CFG_MSGOUT_RTCM_3X_TYPE1005_USB", 1), + ("CFG_MSGOUT_RTCM_3X_TYPE1074_USB", 1), + ("CFG_MSGOUT_RTCM_3X_TYPE1077_USB", 1), + ("CFG_MSGOUT_RTCM_3X_TYPE1084_USB", 1), + ("CFG_MSGOUT_RTCM_3X_TYPE1087_USB", 1), + ("CFG_MSGOUT_RTCM_3X_TYPE1094_USB", 1), + ("CFG_MSGOUT_RTCM_3X_TYPE1097_USB", 1), + ("CFG_MSGOUT_RTCM_3X_TYPE1124_USB", 1), + ("CFG_MSGOUT_RTCM_3X_TYPE1127_USB", 1), + ("CFG_MSGOUT_RTCM_3X_TYPE1230_USB", 1), + ], + ) + serialEndpoint.write(frame.serialize()) + + frame = pyubx2.UBXMessage.config_set( + layers=1, + transaction=0, + cfgData=[ + ("CFG_TMODE_MODE", 1), + ("CFG_TMODE_SVIN_ACC_LIMIT", 250), + ("CFG_TMODE_SVIN_MIN_DUR", 60), + ], + ) + serialEndpoint.write(frame.serialize()) + + +def main(): + args = parse_args() + + with ( + serial.Serial(getF9PBaseStationSerialPort(), 115200) as serialIn, + serial.Serial(args.port, 115200) as serialOut, + ): + enableBaseStationMode(serialIn) + + for raw, parsed in pyrtcm.RTCMReader(serialIn): + print(parsed) + serialOut.write(raw) + + +if __name__ == '__main__': + main() diff --git a/samples/drivers/gnss/base_station/requirements.txt b/samples/drivers/gnss/base_station/requirements.txt new file mode 100644 index 0000000000000..c7f295527af22 --- /dev/null +++ b/samples/drivers/gnss/base_station/requirements.txt @@ -0,0 +1,3 @@ +pyubx2>=1.2.53 +pyrtcm>=1.1.8 +pyserial>=3.5 diff --git a/samples/drivers/gnss/boards/vmu_rt1170_mimxrt1176_cm7.conf b/samples/drivers/gnss/boards/vmu_rt1170_mimxrt1176_cm7.conf new file mode 100644 index 0000000000000..7f004345f8279 --- /dev/null +++ b/samples/drivers/gnss/boards/vmu_rt1170_mimxrt1176_cm7.conf @@ -0,0 +1,7 @@ +# Copyright (c) 2025 Croxel Inc. +# Copyright (c) 2025 CogniPilot Foundation +# SPDX-License-Identifier: Apache-2.0 + +CONFIG_SHELL=n +CONFIG_SERIAL=y +CONFIG_UART_INTERRUPT_DRIVEN=y diff --git a/samples/drivers/gnss/boards/vmu_rt1170_mimxrt1176_cm7.overlay b/samples/drivers/gnss/boards/vmu_rt1170_mimxrt1176_cm7.overlay index 481314e7e1ff0..b67d20e73053a 100644 --- a/samples/drivers/gnss/boards/vmu_rt1170_mimxrt1176_cm7.overlay +++ b/samples/drivers/gnss/boards/vmu_rt1170_mimxrt1176_cm7.overlay @@ -8,6 +8,9 @@ aliases { gnss = &gnss; }; + chosen { + zephyr,rtk-serial = &lpuart1; + }; }; &lpuart3 { @@ -16,7 +19,6 @@ gnss: gnss { status = "okay"; - compatible = "u-blox,m8"; - initial-baudrate = <115200>; + compatible = "u-blox,f9p"; }; }; diff --git a/samples/drivers/gnss/overlay-rtk.conf b/samples/drivers/gnss/overlay-rtk.conf new file mode 100644 index 0000000000000..8901d13de771e --- /dev/null +++ b/samples/drivers/gnss/overlay-rtk.conf @@ -0,0 +1,6 @@ +# Copyright (c) 2025 Croxel Inc. +# Copyright (c) 2025 CogniPilot Foundation +# SPDX-License-Identifier: Apache-2.0 + +CONFIG_GNSS_RTK=y +CONFIG_GNSS_RTK_SERIAL=y diff --git a/samples/drivers/gnss/sample.yaml b/samples/drivers/gnss/sample.yaml index 0ddd7262a2186..6571989a9936e 100644 --- a/samples/drivers/gnss/sample.yaml +++ b/samples/drivers/gnss/sample.yaml @@ -11,3 +11,14 @@ tests: filter: dt_alias_exists("gnss") integration_platforms: - mimxrt1062_fmurt6 + + sample.drivers.gnss.rtk: + harness: rtk + tags: + - drivers + - gnss + - rtk + filter: dt_chosen_enabled("zephyr,rtk-serial") and dt_alias_exists("gnss") + extra_args: EXTRA_CONF_FILE=overlay-rtk.conf + integration_platforms: + - vmu_rt1170/mimxrt1176/cm7 diff --git a/samples/drivers/gnss/src/main.c b/samples/drivers/gnss/src/main.c index 703595ab0e11d..c3fd1cd8f0b12 100644 --- a/samples/drivers/gnss/src/main.c +++ b/samples/drivers/gnss/src/main.c @@ -11,8 +11,6 @@ #define GNSS_MODEM DEVICE_DT_GET(DT_ALIAS(gnss)) -LOG_MODULE_REGISTER(gnss_sample, CONFIG_GNSS_LOG_LEVEL); - static void gnss_data_cb(const struct device *dev, const struct gnss_data *data) { uint64_t timepulse_ns; @@ -21,9 +19,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); } } } @@ -34,12 +33,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); diff --git a/subsys/CMakeLists.txt b/subsys/CMakeLists.txt index a0a2bc828cf23..340c7d09e3d7e 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 a85bda6ece4be..db6990eb1d029 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 0000000000000..e0661e1dc50d6 --- /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 0000000000000..6ccd3b31e417b --- /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 0000000000000..b5f6627bc494c --- /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 0000000000000..8d8936f173409 --- /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/decoder.h b/subsys/gnss/rtk/decoder.h new file mode 100644 index 0000000000000..7a694a87abca6 --- /dev/null +++ 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 +#include + +/** + * @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 gnss_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 new file mode 100644 index 0000000000000..ff443acb354f2 --- /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 0000000000000..e10e36373e5be --- /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 0000000000000..27a37d4437b25 --- /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 (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 new file mode 100644 index 0000000000000..35fba6f13c6df --- /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 0000000000000..80b5191f8bf94 --- /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 0000000000000..44b8fae61c91b --- /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 0000000000000..ffd8ce002e99d --- /dev/null +++ b/subsys/gnss/rtk/serial/serial.c @@ -0,0 +1,99 @@ +/* + * 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; + int err; + + do { + uint8_t *frame; + size_t frame_len = 0; + + err = gnss_rtk_decoder_frame_get(work_buf + offset, len - offset, + &frame, &frame_len); + if (err != 0) { + continue; + } + + 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 (err == 0 && 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. + */ + 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); diff --git a/tests/subsys/gnss/rtk/rtcm3/CMakeLists.txt b/tests/subsys/gnss/rtk/rtcm3/CMakeLists.txt new file mode 100644 index 0000000000000..ed02feb357f5d --- /dev/null +++ b/tests/subsys/gnss/rtk/rtcm3/CMakeLists.txt @@ -0,0 +1,9 @@ +# Copyright (c) 2025 Croxel Inc. +# Copyright (c) 2025 CogniPilot Foundation +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20.0) +find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) +project(gnss_rtk_decoder_rtcm3_test) + +target_sources(app PRIVATE src/main.c) diff --git a/tests/subsys/gnss/rtk/rtcm3/prj.conf b/tests/subsys/gnss/rtk/rtcm3/prj.conf new file mode 100644 index 0000000000000..257a1f9d85d90 --- /dev/null +++ b/tests/subsys/gnss/rtk/rtcm3/prj.conf @@ -0,0 +1,3 @@ +CONFIG_GNSS_RTK=y +CONFIG_GNSS_RTK_PROTOCOL_RTCM3=y +CONFIG_ZTEST=y diff --git a/tests/subsys/gnss/rtk/rtcm3/src/main.c b/tests/subsys/gnss/rtk/rtcm3/src/main.c new file mode 100644 index 0000000000000..f604e46c13a68 --- /dev/null +++ b/tests/subsys/gnss/rtk/rtcm3/src/main.c @@ -0,0 +1,58 @@ +/* + * Copyright (c) 2025 Croxel Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include + +#include + +ZTEST_SUITE(rtk_decoder, NULL, NULL, NULL, NULL, NULL); + +ZTEST(rtk_decoder, test_frame_is_detected) +{ + uint8_t cmd_rtcm3[] = { + 0xD3, /* Sync byte */ + 0x00, 0x04, /* Length: 4 bytes */ + 0x4C, 0xE0, 0x00, 0x80, /* Payload */ + 0xED, 0xED, 0xD6 /* CRC */ + }; + uint8_t *data; + size_t data_len; + + zassert_equal(0, + gnss_rtk_decoder_frame_get(cmd_rtcm3, sizeof(cmd_rtcm3), &data, &data_len)); + zassert_equal_ptr(cmd_rtcm3, data); + zassert_equal(sizeof(cmd_rtcm3), data_len); +} + +ZTEST(rtk_decoder, test_frame_is_detected_after_invalid_data) +{ + uint8_t cmd_rtcm3[] = { + 0xFF, 0xFF, + 0xD3, /* Sync byte */ + 0x00, 0x04, /* Length: 4 bytes */ + 0x4C, 0xE0, 0x00, 0x80, /* Payload */ + 0xED, 0xED, 0xD6 /* CRC */ + }; + uint8_t *data; + size_t data_len; + + zassert_equal(0, + gnss_rtk_decoder_frame_get(cmd_rtcm3, sizeof(cmd_rtcm3), &data, &data_len)); + zassert_equal_ptr(&cmd_rtcm3[2], data); + zassert_equal(sizeof(cmd_rtcm3) - 2, data_len); +} + +ZTEST(rtk_decoder, test_frame_with_invalid_crc_is_invalid_data) +{ + uint8_t cmd_rtcm3[] = {0xD3, 0x00, 0x01, 0xFF, 0x00, 0x01, 0x02}; + uint8_t *data; + size_t data_len; + + zassert_equal(-ENOENT, + gnss_rtk_decoder_frame_get(cmd_rtcm3, sizeof(cmd_rtcm3), &data, &data_len)); +} diff --git a/tests/subsys/gnss/rtk/rtcm3/testcase.yaml b/tests/subsys/gnss/rtk/rtcm3/testcase.yaml new file mode 100644 index 0000000000000..bb7e5dcd08dde --- /dev/null +++ b/tests/subsys/gnss/rtk/rtcm3/testcase.yaml @@ -0,0 +1,9 @@ +# Copyright (c) 2025 Croxel Inc. +# Copyright (c) 2025 CogniPilot Foundation +# SPDX-License-Identifier: Apache-2.0 + +tests: + gnss.rtk.decoder.rtcm3: + tags: rtk + platform_allow: + - native_sim diff --git a/tests/unit/crc/main.c b/tests/unit/crc/main.c index f2c2b91fd5b59..dab84c0021103 100644 --- a/tests/unit/crc/main.c +++ b/tests/unit/crc/main.c @@ -90,6 +90,15 @@ ZTEST(crc, test_crc24_pgp) zassert_equal(crc24_pgp_update(0x00BA353A, test2 + 5, 4), 0x0021CF02); } +ZTEST(crc, test_crc24q_rtcm3) +{ + uint8_t test1[] = {0xD3, 0x00, 0x04, 0x4C, 0xE0, 0x00, 0x80}; + uint8_t test2[] = {0xD3, 0x00, 0x04, 0x4C, 0xE0, 0x00, 0x80, 0xED, 0xED, 0xD6}; + + zassert_equal(crc24q_rtcm3(test1, sizeof(test1)), 0xEDEDD6); + zassert_equal(crc24q_rtcm3(test2, sizeof(test2)), 0x000000); +} + ZTEST(crc, test_crc16) { uint8_t test[] = { '1', '2', '3', '4', '5', '6', '7', '8', '9' };