1 /* 2 * Copyright (c) 2025 Croxel Inc. 3 * Copyright (c) 2025 CogniPilot Foundation 4 * 5 * SPDX-License-Identifier: Apache-2.0 6 */ 7 8 #include <errno.h> 9 #include <zephyr/sys/byteorder.h> 10 #include <zephyr/sys/util.h> 11 #include <zephyr/gnss/rtk/decoder.h> 12 #include <zephyr/sys/crc.h> 13 14 #define RTCM3_FRAME_SYNC_SZ 1 15 #define RTCM3_FRAME_HDR_SZ 2 16 #define RTCM3_FRAME_CHECKSUM_SZ 3 17 #define RTCM3_FRAME_OVERHEAD (RTCM3_FRAME_SYNC_SZ + RTCM3_FRAME_HDR_SZ + \ 18 RTCM3_FRAME_CHECKSUM_SZ) 19 20 #define RTCM3_SYNC_BYTE 0xD3 21 22 #define RTCM3_FRAME_PAYLOAD_SZ(hdr) (sys_be16_to_cpu(hdr) & BIT_MASK(10)) 23 #define RTCM3_FRAME_SZ(payload_len) ((payload_len) + RTCM3_FRAME_OVERHEAD) 24 25 struct rtcm3_frame { 26 uint8_t sync_frame; 27 uint16_t hdr; 28 uint8_t payload[]; 29 } __packed; 30 gnss_rtk_decoder_frame_get(uint8_t * buf,size_t buf_len,uint8_t ** data,size_t * data_len)31int gnss_rtk_decoder_frame_get(uint8_t *buf, size_t buf_len, 32 uint8_t **data, size_t *data_len) 33 { 34 for (size_t i = 0 ; (i + RTCM3_FRAME_OVERHEAD - 1) < buf_len ; i++) { 35 if (buf[i] != RTCM3_SYNC_BYTE) { 36 continue; 37 } 38 39 struct rtcm3_frame *frame = (struct rtcm3_frame *)&buf[i]; 40 uint16_t payload_len = RTCM3_FRAME_PAYLOAD_SZ(frame->hdr); 41 uint16_t remaining_bytes = buf_len - i; 42 43 if (payload_len == 0 || 44 RTCM3_FRAME_SZ(payload_len) > remaining_bytes) { 45 continue; 46 } 47 48 if (crc24q_rtcm3((const uint8_t *)frame, 49 RTCM3_FRAME_SZ(payload_len)) == 0) { 50 *data = (uint8_t *)frame; 51 *data_len = RTCM3_FRAME_SZ(payload_len); 52 53 return 0; 54 } 55 } 56 57 return -ENOENT; 58 } 59
