Browse Source
During testing and code inspection, there were various anti-patterns on this (and U-Blox driver) codebase(s), including obfuscation, and lack of data validation. This made it increasingly difficult to introduce further variants of u-blox GNSS modems. With this patch, both the UBX modem and the M8 driver have been refactored to ease the reliability and maintainability of these codebases. Here are some highlights: WRT UBX modem: - Helper macros to easily create UBX frames, (including checksum calculation), at compile time; thus, making it easier to extend UBX commands. - Logic validation by the inclusion of the modem_ubx testsuite, used to refactor the code through TDD. - Ability to receive unsolicited messages, in order to enable U-Blox drivers to rely on modem_ubx to transceive all commands, and avoid hopping between modem_ubx and modem_chat. WRT M8 driver: - Remove GNSS specific protocol header files. Instead, unify them under modem/ubx/protocol.h. Background: After a survey and looking at ubxlib SDK I conclude the UBX protocol is by definition a GNSS protocol (there are non-GNSS u-blox modems, but they're not interfaced through UBX protocol). - Establish pattern to create and send/receive commands using new foundations on modem ubx. - Remove dependency of Modem chat, and instead use UBX unsolicited messages to get Navigation and Satellites data. - Switch from the auto-baudrate detection pattern to a pattern of transitioning between an initial known baudrate to a desired baudrate, in order to improve initialization time. - Add dts property to configure default fix-rate. Signed-off-by: Luis Ubieda <luisf@croxel.com>pull/90887/head
Luis Ubieda
2 months ago
committed by
Benjamin Cabé
20 changed files with 1942 additions and 1842 deletions
@ -1,180 +0,0 @@
@@ -1,180 +0,0 @@
|
||||
/*
|
||||
* Copyright 2024 NXP |
||||
* |
||||
* SPDX-License-Identifier: Apache-2.0 |
||||
*/ |
||||
|
||||
#include "gnss_u_blox_protocol.h" |
||||
|
||||
const uint32_t ubx_baudrate[UBX_BAUDRATE_COUNT] = { |
||||
4800, |
||||
9600, |
||||
19200, |
||||
38400, |
||||
57600, |
||||
115200, |
||||
230400, |
||||
460800, |
||||
921600, |
||||
}; |
||||
|
||||
static inline int ubx_validate_payload_size_ack(uint8_t msg_id, uint16_t payload_size) |
||||
{ |
||||
switch (msg_id) { |
||||
case UBX_ACK_ACK: |
||||
return payload_size == UBX_CFG_ACK_PAYLOAD_SZ ? 0 : -1; |
||||
case UBX_ACK_NAK: |
||||
return payload_size == UBX_CFG_NAK_PAYLOAD_SZ ? 0 : -1; |
||||
default: |
||||
return -1; |
||||
} |
||||
} |
||||
|
||||
static inline int ubx_validate_payload_size_cfg(uint8_t msg_id, uint16_t payload_size) |
||||
{ |
||||
switch (msg_id) { |
||||
case UBX_CFG_RATE: |
||||
return payload_size == UBX_CFG_RATE_PAYLOAD_SZ ? 0 : -1; |
||||
case UBX_CFG_PRT: |
||||
return (payload_size == UBX_CFG_PRT_POLL_PAYLOAD_SZ || |
||||
payload_size == UBX_CFG_PRT_SET_PAYLOAD_SZ) ? 0 : -1; |
||||
case UBX_CFG_RST: |
||||
return payload_size == UBX_CFG_RST_PAYLOAD_SZ ? 0 : -1; |
||||
case UBX_CFG_NAV5: |
||||
return payload_size == UBX_CFG_NAV5_PAYLOAD_SZ ? 0 : -1; |
||||
case UBX_CFG_GNSS: |
||||
return ((payload_size - UBX_CFG_GNSS_PAYLOAD_INIT_SZ) % |
||||
UBX_CFG_GNSS_PAYLOAD_CFG_BLK_SZ == 0) ? 0 : -1; |
||||
case UBX_CFG_MSG: |
||||
return payload_size == UBX_CFG_MSG_PAYLOAD_SZ ? 0 : -1; |
||||
default: |
||||
return -1; |
||||
} |
||||
} |
||||
|
||||
static inline int ubx_validate_payload_size(uint8_t msg_cls, uint8_t msg_id, uint16_t payload_size) |
||||
{ |
||||
if (payload_size == 0) { |
||||
return 0; |
||||
} |
||||
|
||||
if (payload_size > UBX_PAYLOAD_SZ_MAX) { |
||||
return -1; |
||||
} |
||||
|
||||
switch (msg_cls) { |
||||
case UBX_CLASS_ACK: |
||||
return ubx_validate_payload_size_ack(msg_id, payload_size); |
||||
case UBX_CLASS_CFG: |
||||
return ubx_validate_payload_size_cfg(msg_id, payload_size); |
||||
default: |
||||
return -1; |
||||
} |
||||
} |
||||
|
||||
int ubx_create_and_validate_frame(uint8_t *ubx_frame, uint16_t ubx_frame_size, uint8_t msg_cls, |
||||
uint8_t msg_id, const void *payload, uint16_t payload_size) |
||||
{ |
||||
if (ubx_validate_payload_size(msg_cls, msg_id, payload_size)) { |
||||
return -1; |
||||
} |
||||
|
||||
return modem_ubx_create_frame(ubx_frame, ubx_frame_size, msg_cls, msg_id, payload, |
||||
payload_size); |
||||
} |
||||
|
||||
void ubx_cfg_ack_payload_default(struct ubx_cfg_ack_payload *payload) |
||||
{ |
||||
payload->message_class = UBX_CLASS_CFG; |
||||
payload->message_id = UBX_CFG_PRT; |
||||
} |
||||
|
||||
void ubx_cfg_rate_payload_default(struct ubx_cfg_rate_payload *payload) |
||||
{ |
||||
payload->meas_rate_ms = 1000; |
||||
payload->nav_rate = 1; |
||||
payload->time_ref = UBX_CFG_RATE_TIME_REF_UTC; |
||||
} |
||||
|
||||
void ubx_cfg_prt_poll_payload_default(struct ubx_cfg_prt_poll_payload *payload) |
||||
{ |
||||
payload->port_id = UBX_PORT_NUMBER_UART; |
||||
} |
||||
|
||||
void ubx_cfg_prt_set_payload_default(struct ubx_cfg_prt_set_payload *payload) |
||||
{ |
||||
payload->port_id = UBX_PORT_NUMBER_UART; |
||||
payload->reserved0 = UBX_CFG_PRT_RESERVED0; |
||||
payload->tx_ready_pin_conf = UBX_CFG_PRT_TX_READY_PIN_CONF_POL_HIGH; |
||||
payload->port_mode = UBX_CFG_PRT_PORT_MODE_CHAR_LEN_8 | UBX_CFG_PRT_PORT_MODE_PARITY_NONE | |
||||
UBX_CFG_PRT_PORT_MODE_STOP_BITS_1; |
||||
payload->baudrate = ubx_baudrate[3]; |
||||
payload->in_proto_mask = UBX_CFG_PRT_IN_PROTO_UBX | UBX_CFG_PRT_IN_PROTO_NMEA | |
||||
UBX_CFG_PRT_IN_PROTO_RTCM; |
||||
payload->out_proto_mask = UBX_CFG_PRT_OUT_PROTO_UBX | UBX_CFG_PRT_OUT_PROTO_NMEA | |
||||
UBX_CFG_PRT_OUT_PROTO_RTCM3; |
||||
payload->flags = UBX_CFG_PRT_FLAGS_DEFAULT; |
||||
payload->reserved1 = UBX_CFG_PRT_RESERVED1; |
||||
} |
||||
|
||||
void ubx_cfg_rst_payload_default(struct ubx_cfg_rst_payload *payload) |
||||
{ |
||||
payload->nav_bbr_mask = UBX_CFG_RST_NAV_BBR_MASK_HOT_START; |
||||
payload->reset_mode = UBX_CFG_RST_RESET_MODE_CONTROLLED_SOFT_RESET; |
||||
payload->reserved0 = UBX_CFG_RST_RESERVED0; |
||||
} |
||||
|
||||
void ubx_cfg_nav5_payload_default(struct ubx_cfg_nav5_payload *payload) |
||||
{ |
||||
payload->mask = UBX_CFG_NAV5_MASK_ALL; |
||||
payload->dyn_model = UBX_DYN_MODEL_PORTABLE; |
||||
|
||||
payload->fix_mode = UBX_FIX_AUTO_FIX; |
||||
|
||||
payload->fixed_alt = UBX_CFG_NAV5_FIXED_ALT_DEFAULT; |
||||
payload->fixed_alt_var = UBX_CFG_NAV5_FIXED_ALT_VAR_DEFAULT; |
||||
|
||||
payload->min_elev = UBX_CFG_NAV5_MIN_ELEV_DEFAULT; |
||||
payload->dr_limit = UBX_CFG_NAV5_DR_LIMIT_DEFAULT; |
||||
|
||||
payload->p_dop = UBX_CFG_NAV5_P_DOP_DEFAULT; |
||||
payload->t_dop = UBX_CFG_NAV5_T_DOP_DEFAULT; |
||||
payload->p_acc = UBX_CFG_NAV5_P_ACC_DEFAULT; |
||||
payload->t_acc = UBX_CFG_NAV5_T_ACC_DEFAULT; |
||||
|
||||
payload->static_hold_threshold = UBX_CFG_NAV5_STATIC_HOLD_THRESHOLD_DEFAULT; |
||||
payload->dgnss_timeout = UBX_CFG_NAV5_DGNSS_TIMEOUT_DEFAULT; |
||||
payload->cno_threshold_num_svs = UBX_CFG_NAV5_CNO_THRESHOLD_NUM_SVS_DEFAULT; |
||||
payload->cno_threshold = UBX_CFG_NAV5_CNO_THRESHOLD_DEFAULT; |
||||
|
||||
payload->reserved0 = UBX_CFG_NAV5_RESERVED0; |
||||
|
||||
payload->static_hold_dist_threshold = UBX_CFG_NAV5_STATIC_HOLD_DIST_THRESHOLD; |
||||
payload->utc_standard = UBX_CFG_NAV5_UTC_STANDARD_DEFAULT; |
||||
} |
||||
|
||||
static struct ubx_cfg_gnss_payload_config_block ubx_cfg_gnss_payload_config_block_default = { |
||||
.gnss_id = UBX_GNSS_ID_GPS, |
||||
.num_res_trk_ch = 0x00, |
||||
.max_num_trk_ch = 0x00, |
||||
.reserved0 = UBX_CFG_GNSS_RESERVED0, |
||||
.flags = UBX_CFG_GNSS_FLAG_ENABLE | UBX_CFG_GNSS_FLAG_SGN_CNF_GPS_L1C_A, |
||||
}; |
||||
|
||||
void ubx_cfg_gnss_payload_default(struct ubx_cfg_gnss_payload *payload) |
||||
{ |
||||
payload->msg_ver = UBX_CFG_GNSS_MSG_VER; |
||||
payload->num_trk_ch_hw = UBX_CFG_GNSS_NUM_TRK_CH_HW_DEFAULT; |
||||
payload->num_trk_ch_use = UBX_CFG_GNSS_NUM_TRK_CH_USE_DEFAULT; |
||||
|
||||
for (int i = 0; i < payload->num_config_blocks; ++i) { |
||||
payload->config_blocks[i] = ubx_cfg_gnss_payload_config_block_default; |
||||
} |
||||
} |
||||
|
||||
void ubx_cfg_msg_payload_default(struct ubx_cfg_msg_payload *payload) |
||||
{ |
||||
payload->message_class = UBX_CLASS_NMEA; |
||||
payload->message_id = UBX_NMEA_GGA; |
||||
payload->rate = UBX_CFG_MSG_RATE_DEFAULT; |
||||
} |
||||
@ -1,251 +0,0 @@
@@ -1,251 +0,0 @@
|
||||
/*
|
||||
* Copyright 2024 NXP |
||||
* |
||||
* SPDX-License-Identifier: Apache-2.0 |
||||
*/ |
||||
|
||||
#include <zephyr/kernel.h> |
||||
#include <zephyr/types.h> |
||||
#include <zephyr/modem/ubx.h> |
||||
#include "gnss_u_blox_protocol_defines.h" |
||||
|
||||
#ifndef ZEPHYR_U_BLOX_PROTOCOL_ |
||||
#define ZEPHYR_U_BLOX_PROTOCOL_ |
||||
|
||||
#define UBX_BAUDRATE_COUNT 9 |
||||
|
||||
/* When a configuration frame is sent, the device requires some delay to reflect the changes. */ |
||||
/* TODO: check what is the precise waiting time for each message. */ |
||||
#define UBX_CFG_RST_WAIT_MS 6000 |
||||
#define UBX_CFG_GNSS_WAIT_MS 6000 |
||||
#define UBX_CFG_NAV5_WAIT_MS 6000 |
||||
|
||||
extern const uint32_t ubx_baudrate[UBX_BAUDRATE_COUNT]; |
||||
|
||||
#define UBX_FRM_GET_PAYLOAD_SZ 0 |
||||
#define UBX_CFG_ACK_PAYLOAD_SZ 2 |
||||
#define UBX_CFG_NAK_PAYLOAD_SZ 2 |
||||
#define UBX_CFG_RATE_PAYLOAD_SZ 6 |
||||
#define UBX_CFG_PRT_POLL_PAYLOAD_SZ 1 |
||||
#define UBX_CFG_PRT_POLL_FRM_SZ (UBX_FRM_SZ_WO_PAYLOAD + UBX_CFG_PRT_POLL_PAYLOAD_SZ) |
||||
#define UBX_CFG_PRT_SET_PAYLOAD_SZ 20 |
||||
#define UBX_CFG_PRT_SET_FRM_SZ (UBX_FRM_SZ_WO_PAYLOAD + UBX_CFG_PRT_SET_PAYLOAD_SZ) |
||||
#define UBX_CFG_RST_PAYLOAD_SZ 4 |
||||
#define UBX_CFG_RST_FRM_SZ (UBX_FRM_SZ_WO_PAYLOAD + UBX_CFG_RST_PAYLOAD_SZ) |
||||
#define UBX_CFG_NAV5_PAYLOAD_SZ 36 |
||||
#define UBX_CFG_NAV5_FRM_SZ (UBX_FRM_SZ_WO_PAYLOAD + UBX_CFG_NAV5_PAYLOAD_SZ) |
||||
#define UBX_CFG_MSG_PAYLOAD_SZ 3 |
||||
#define UBX_CFG_MSG_FRM_SZ (UBX_FRM_SZ_WO_PAYLOAD + UBX_CFG_MSG_PAYLOAD_SZ) |
||||
#define UBX_CFG_GNSS_PAYLOAD_INIT_SZ 4 |
||||
#define UBX_CFG_GNSS_PAYLOAD_CFG_BLK_SZ 8 |
||||
#define UBX_CFG_GNSS_PAYLOAD_SZ(n) \ |
||||
(UBX_CFG_GNSS_PAYLOAD_INIT_SZ + UBX_CFG_GNSS_PAYLOAD_CFG_BLK_SZ * n) |
||||
#define UBX_CFG_GNSS_FRM_SZ(n) (UBX_FRM_SZ_WO_PAYLOAD + UBX_CFG_GNSS_PAYLOAD_SZ(n)) |
||||
|
||||
|
||||
int ubx_create_and_validate_frame(uint8_t *ubx_frame, uint16_t ubx_frame_size, uint8_t msg_cls, |
||||
uint8_t msg_id, const void *payload, uint16_t payload_size); |
||||
|
||||
struct ubx_cfg_ack_payload { |
||||
uint8_t message_class; |
||||
uint8_t message_id; |
||||
}; |
||||
|
||||
void ubx_cfg_ack_payload_default(struct ubx_cfg_ack_payload *payload); |
||||
|
||||
#define UBX_CFG_RATE_TIME_REF_UTC 0 /* Align measurements to UTC time. */ |
||||
#define UBX_CFG_RATE_TIME_REF_GPS 1 /* Align measurements to GPS time. */ |
||||
#define UBX_CFG_RATE_TIME_REF_GLO 2 /* Align measurements to GLONASS time. */ |
||||
#define UBX_CFG_RATE_TIME_REF_BDS 3 /* Align measurements to BeiDou time. */ |
||||
#define UBX_CFG_RATE_TIME_REF_GAL 4 /* Align measurements to Galileo time. */ |
||||
|
||||
struct ubx_cfg_rate_payload { |
||||
uint16_t meas_rate_ms; |
||||
uint16_t nav_rate; |
||||
uint16_t time_ref; |
||||
}; |
||||
|
||||
void ubx_cfg_rate_payload_default(struct ubx_cfg_rate_payload *payload); |
||||
|
||||
struct ubx_cfg_prt_poll_payload { |
||||
uint8_t port_id; |
||||
}; |
||||
|
||||
void ubx_cfg_prt_poll_payload_default(struct ubx_cfg_prt_poll_payload *payload); |
||||
|
||||
#define UBX_CFG_PRT_IN_PROTO_UBX BIT(0) |
||||
#define UBX_CFG_PRT_IN_PROTO_NMEA BIT(1) |
||||
#define UBX_CFG_PRT_IN_PROTO_RTCM BIT(2) |
||||
#define UBX_CFG_PRT_IN_PROTO_RTCM3 BIT(5) |
||||
#define UBX_CFG_PRT_OUT_PROTO_UBX BIT(0) |
||||
#define UBX_CFG_PRT_OUT_PROTO_NMEA BIT(1) |
||||
#define UBX_CFG_PRT_OUT_PROTO_RTCM3 BIT(5) |
||||
|
||||
#define UBX_CFG_PRT_PORT_MODE_CHAR_LEN_5 0U |
||||
#define UBX_CFG_PRT_PORT_MODE_CHAR_LEN_6 BIT(6) |
||||
#define UBX_CFG_PRT_PORT_MODE_CHAR_LEN_7 BIT(7) |
||||
#define UBX_CFG_PRT_PORT_MODE_CHAR_LEN_8 (BIT(6) | BIT(7)) |
||||
|
||||
#define UBX_CFG_PRT_PORT_MODE_PARITY_EVEN 0U |
||||
#define UBX_CFG_PRT_PORT_MODE_PARITY_ODD BIT(9) |
||||
#define UBX_CFG_PRT_PORT_MODE_PARITY_NONE BIT(11) |
||||
|
||||
#define UBX_CFG_PRT_PORT_MODE_STOP_BITS_1 0U |
||||
#define UBX_CFG_PRT_PORT_MODE_STOP_BITS_1_HALF BIT(12) |
||||
#define UBX_CFG_PRT_PORT_MODE_STOP_BITS_2 BIT(13) |
||||
#define UBX_CFG_PRT_PORT_MODE_STOP_BITS_HALF (BIT(12) | BIT(13)) |
||||
|
||||
#define UBX_CFG_PRT_RESERVED0 0x00 |
||||
#define UBX_CFG_PRT_TX_READY_PIN_CONF_DEFAULT 0x0000 |
||||
#define UBX_CFG_PRT_TX_READY_PIN_CONF_EN BIT(0) |
||||
#define UBX_CFG_PRT_TX_READY_PIN_CONF_POL_LOW BIT(1) |
||||
#define UBX_CFG_PRT_TX_READY_PIN_CONF_POL_HIGH 0U |
||||
#define UBX_CFG_PRT_RESERVED1 0x00 |
||||
#define UBX_CFG_PRT_FLAGS_DEFAULT 0x0000 |
||||
#define UBX_CFG_PRT_FLAGS_EXTENDED_TX_TIMEOUT BIT(0) |
||||
|
||||
struct ubx_cfg_prt_set_payload { |
||||
uint8_t port_id; |
||||
uint8_t reserved0; |
||||
uint16_t tx_ready_pin_conf; |
||||
uint32_t port_mode; |
||||
uint32_t baudrate; |
||||
uint16_t in_proto_mask; |
||||
uint16_t out_proto_mask; |
||||
uint16_t flags; |
||||
uint8_t reserved1; |
||||
}; |
||||
|
||||
void ubx_cfg_prt_set_payload_default(struct ubx_cfg_prt_set_payload *payload); |
||||
|
||||
#define UBX_CFG_RST_NAV_BBR_MASK_HOT_START 0x0000 |
||||
#define UBX_CFG_RST_NAV_BBR_MASK_WARM_START 0x0001 |
||||
#define UBX_CFG_RST_NAV_BBR_MASK_COLD_START 0xFFFF |
||||
|
||||
#define UBX_CFG_RST_RESET_MODE_HARD_RESET 0x00 |
||||
#define UBX_CFG_RST_RESET_MODE_CONTROLLED_SOFT_RESET 0x01 |
||||
#define UBX_CFG_RST_RESET_MODE_CONTROLLED_SOFT_RESET_GNSS_ONLY 0x02 |
||||
#define UBX_CFG_RST_RESET_MODE_HARD_RESET_AFTER_SHUTDOWN 0x04 |
||||
#define UBX_CFG_RST_RESET_MODE_CONTROLLED_GNSS_STOP 0x08 |
||||
#define UBX_CFG_RST_RESET_MODE_CONTROLLED_GNSS_START 0x09 |
||||
|
||||
#define UBX_CFG_RST_RESERVED0 0x00 |
||||
|
||||
struct ubx_cfg_rst_payload { |
||||
uint16_t nav_bbr_mask; |
||||
uint8_t reset_mode; |
||||
uint8_t reserved0; |
||||
}; |
||||
|
||||
void ubx_cfg_rst_payload_default(struct ubx_cfg_rst_payload *payload); |
||||
|
||||
#define UBX_CFG_NAV5_MASK_ALL 0x05FF |
||||
#define UBX_CFG_NAV5_FIX_MODE_DEFAULT UBX_FIX_AUTO_FIX |
||||
#define UBX_CFG_NAV5_FIXED_ALT_DEFAULT 0 |
||||
#define UBX_CFG_NAV5_FIXED_ALT_VAR_DEFAULT 1U |
||||
#define UBX_CFG_NAV5_MIN_ELEV_DEFAULT 5 |
||||
#define UBX_CFG_NAV5_DR_LIMIT_DEFAULT 3U |
||||
#define UBX_CFG_NAV5_P_DOP_DEFAULT 100U |
||||
#define UBX_CFG_NAV5_T_DOP_DEFAULT 100U |
||||
#define UBX_CFG_NAV5_P_ACC_DEFAULT 100U |
||||
#define UBX_CFG_NAV5_T_ACC_DEFAULT 350U |
||||
#define UBX_CFG_NAV5_STATIC_HOLD_THRESHOLD_DEFAULT 0U |
||||
#define UBX_CFG_NAV5_DGNSS_TIMEOUT_DEFAULT 60U |
||||
#define UBX_CFG_NAV5_CNO_THRESHOLD_NUM_SVS_DEFAULT 0U |
||||
#define UBX_CFG_NAV5_CNO_THRESHOLD_DEFAULT 0U |
||||
#define UBX_CFG_NAV5_RESERVED0 0U |
||||
#define UBX_CFG_NAV5_STATIC_HOLD_DIST_THRESHOLD 0U |
||||
#define UBX_CFG_NAV5_UTC_STANDARD_DEFAULT UBX_UTC_AUTOUTC |
||||
|
||||
struct ubx_cfg_nav5_payload { |
||||
uint16_t mask; |
||||
uint8_t dyn_model; |
||||
|
||||
uint8_t fix_mode; |
||||
|
||||
int32_t fixed_alt; |
||||
uint32_t fixed_alt_var; |
||||
|
||||
int8_t min_elev; |
||||
uint8_t dr_limit; |
||||
|
||||
uint16_t p_dop; |
||||
uint16_t t_dop; |
||||
uint16_t p_acc; |
||||
uint16_t t_acc; |
||||
|
||||
uint8_t static_hold_threshold; |
||||
uint8_t dgnss_timeout; |
||||
uint8_t cno_threshold_num_svs; |
||||
uint8_t cno_threshold; |
||||
|
||||
uint16_t reserved0; |
||||
|
||||
uint16_t static_hold_dist_threshold; |
||||
uint8_t utc_standard; |
||||
}; |
||||
|
||||
void ubx_cfg_nav5_payload_default(struct ubx_cfg_nav5_payload *payload); |
||||
|
||||
#define UBX_CFG_GNSS_MSG_VER 0x00 |
||||
#define UBX_CFG_GNSS_NUM_TRK_CH_HW_DEFAULT 0x31 |
||||
#define UBX_CFG_GNSS_NUM_TRK_CH_USE_DEFAULT 0x31 |
||||
|
||||
#define UBX_CFG_GNSS_RESERVED0 0x00 |
||||
#define UBX_CFG_GNSS_FLAG_ENABLE BIT(0) |
||||
#define UBX_CFG_GNSS_FLAG_DISABLE 0U |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT 16 |
||||
/* When gnss_id is 0 (GPS) */ |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_GPS_L1C_A (0x01 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_GPS_L2C (0x10 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_GPS_L5 (0x20 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
/* When gnss_id is 1 (SBAS) */ |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_SBAS_L1C_A (0x01 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
/* When gnss_id is 2 (Galileo) */ |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_GALILEO_E1 (0x01 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_GALILEO_E5A (0x10 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_GALILEO_E5B (0x20 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
/* When gnss_id is 3 (BeiDou) */ |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_BEIDOU_B1I (0x01 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_BEIDOU_B2I (0x10 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_BEIDOU_B2A (0x80 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
/* When gnss_id is 4 (IMES) */ |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_IMES_L1 (0x01 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
/* When gnss_id is 5 (QZSS) */ |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_QZSS_L1C_A (0x01 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_QZSS_L1S (0x04 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_QZSS_L2C (0x10 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_QZSS_L5 (0x20 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
/* When gnss_id is 6 (GLONASS) */ |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_GLONASS_L1 (0x01 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
#define UBX_CFG_GNSS_FLAG_SGN_CNF_GLONASS_L2 (0x10 << UBX_CFG_GNSS_FLAG_SGN_CNF_SHIFT) |
||||
|
||||
struct ubx_cfg_gnss_payload_config_block { |
||||
uint8_t gnss_id; |
||||
uint8_t num_res_trk_ch; |
||||
uint8_t max_num_trk_ch; |
||||
uint8_t reserved0; |
||||
uint32_t flags; |
||||
}; |
||||
|
||||
struct ubx_cfg_gnss_payload { |
||||
uint8_t msg_ver; |
||||
uint8_t num_trk_ch_hw; |
||||
uint8_t num_trk_ch_use; |
||||
uint8_t num_config_blocks; |
||||
struct ubx_cfg_gnss_payload_config_block config_blocks[]; |
||||
}; |
||||
|
||||
void ubx_cfg_gnss_payload_default(struct ubx_cfg_gnss_payload *payload); |
||||
|
||||
#define UBX_CFG_MSG_RATE_DEFAULT 1 |
||||
|
||||
struct ubx_cfg_msg_payload { |
||||
uint8_t message_class; |
||||
uint8_t message_id; |
||||
uint8_t rate; |
||||
}; |
||||
|
||||
void ubx_cfg_msg_payload_default(struct ubx_cfg_msg_payload *payload); |
||||
|
||||
#endif /* ZEPHYR_U_BLOX_PROTOCOL_ */ |
||||
@ -1,258 +0,0 @@
@@ -1,258 +0,0 @@
|
||||
/*
|
||||
* Copyright 2024 NXP |
||||
* |
||||
* SPDX-License-Identifier: Apache-2.0 |
||||
*/ |
||||
|
||||
/* Referred some enum definitions from file "include/zephyr/drivers/gnss/ublox_neo_m8_defines.h"
|
||||
* from the pull request #46447 (link - https://github.com/zephyrproject-rtos/zephyr/pull/46447).
|
||||
*/ |
||||
|
||||
#ifndef ZEPHYR_U_BLOX_PROTOCOL_DEFINES_ |
||||
#define ZEPHYR_U_BLOX_PROTOCOL_DEFINES_ |
||||
|
||||
enum ubx_gnss_id { |
||||
UBX_GNSS_ID_GPS = 0, |
||||
UBX_GNSS_ID_SBAS = 1, |
||||
UBX_GNSS_ID_GALILEO = 2, |
||||
UBX_GNSS_ID_BEIDOU = 3, |
||||
UBX_GNSS_ID_IMES = 4, |
||||
UBX_GNSS_ID_QZSS = 5, |
||||
UBX_GNSS_ID_GLONASS = 6, |
||||
}; |
||||
|
||||
enum ubx_port_number { |
||||
UBX_PORT_NUMBER_DDC = 0, |
||||
UBX_PORT_NUMBER_UART, |
||||
UBX_PORT_NUMBER_USB, |
||||
UBX_PORT_NUMBER_SPI, |
||||
}; |
||||
|
||||
enum ubx_dynamic_model { |
||||
UBX_DYN_MODEL_PORTABLE = 0, |
||||
UBX_DYN_MODEL_STATIONARY = 2, |
||||
UBX_DYN_MODEL_PEDESTRIAN = 3, |
||||
UBX_DYN_MODEL_AUTOMOTIVE = 4, |
||||
UBX_DYN_MODEL_SEA = 5, |
||||
UBX_DYN_MODEL_AIRBORNE1G = 6, |
||||
UBX_DYN_MODEL_AIRBORNE2G = 7, |
||||
UBX_DYN_MODEL_AIRBORNE4G = 8, |
||||
UBX_DYN_MODEL_WRIST = 9, |
||||
UBX_DYN_MODEL_BIKE = 10, |
||||
}; |
||||
|
||||
enum ubx_fix_mode { |
||||
UBX_FIX_P_2D = 1, |
||||
UBX_FIX_P_3D, |
||||
UBX_FIX_AUTO_FIX, |
||||
}; |
||||
|
||||
enum ubx_utc_standard { |
||||
UBX_UTC_AUTOUTC = 0, |
||||
UBX_UTC_GPS = 3, |
||||
UBX_UTC_GALILEO = 5, |
||||
UBX_UTC_GLONASS, |
||||
UBX_UTC_BEIDOU, |
||||
UBX_UTC_NAVIC, |
||||
}; |
||||
|
||||
enum ubx_msg_class { |
||||
UBX_CLASS_NAV = 0x01, |
||||
UBX_CLASS_RXM = 0x02, |
||||
UBX_CLASS_INF = 0x04, |
||||
UBX_CLASS_ACK = 0x05, |
||||
UBX_CLASS_CFG = 0x06, |
||||
UBX_CLASS_UPD = 0x09, |
||||
UBX_CLASS_MON = 0x0A, |
||||
UBX_CLASS_AID = 0x0B, |
||||
UBX_CLASS_TIM = 0x0D, |
||||
UBX_CLASS_ESF = 0x10, |
||||
UBX_CLASS_MGA = 0x13, |
||||
UBX_CLASS_LOG = 0x21, |
||||
UBX_CLASS_SEC = 0x27, |
||||
UBX_CLASS_HNR = 0x28, |
||||
UBX_CLASS_NMEA = 0xF0, |
||||
}; |
||||
|
||||
enum ubx_ack_message { |
||||
UBX_ACK_ACK = 0x01, |
||||
UBX_ACK_NAK = 0x00, |
||||
}; |
||||
|
||||
enum ubx_config_message { |
||||
UBX_CFG_ANT = 0x13, |
||||
UBX_CFG_BATCH = 0x93, |
||||
UBX_CFG_CFG = 0x09, |
||||
UBX_CFG_DAT = 0x06, |
||||
UBX_CFG_DGNSS = 0x70, |
||||
UBX_CFG_DOSC = 0x61, |
||||
UBX_CFG_ESFALG = 0x56, |
||||
UBX_CFG_ESFAE = 0x4C, |
||||
UBX_CFG_ESFGE = 0x4D, |
||||
UBX_CFG_ESFWTE = 0x82, |
||||
UBX_CFG_ESRCE = 0x60, |
||||
UBX_CFG_GEOFENCE = 0x69, |
||||
UBX_CFG_GNSS = 0x3E, |
||||
UBX_CFG_HNR = 0x5C, |
||||
UBX_CFG_INF = 0x02, |
||||
UBX_CFG_ITFM = 0x39, |
||||
UBX_CFG_LOGFILTER = 0x47, |
||||
UBX_CFG_MSG = 0x01, |
||||
UBX_CFG_NAV5 = 0x24, |
||||
UBX_CFG_NAVX5 = 0x23, |
||||
UBX_CFG_NMEA = 0x17, |
||||
UBX_CFG_ODO = 0x1E, |
||||
UBX_CFG_PM2 = 0x3B, |
||||
UBX_CFG_PMS = 0x86, |
||||
UBX_CFG_PRT = 0x00, |
||||
UBX_CFG_PWR = 0x57, |
||||
UBX_CFG_RATE = 0x08, |
||||
UBX_CFG_RINV = 0x34, |
||||
UBX_CFG_RST = 0x04, |
||||
UBX_CFG_RXM = 0x11, |
||||
UBX_CFG_SBAS = 0x16, |
||||
UBX_CFG_SENIF = 0x88, |
||||
UBX_CFG_SLAS = 0x8D, |
||||
UBX_CFG_SMGR = 0x62, |
||||
UBX_CFG_SPT = 0x64, |
||||
UBX_CFG_TMODE2 = 0x3D, |
||||
UBX_CFG_TMODE3 = 0x71, |
||||
UBX_CFG_TP5 = 0x31, |
||||
UBX_CFG_TXSLOT = 0x53, |
||||
UBX_CFG_USB = 0x1B, |
||||
}; |
||||
|
||||
enum ubx_information_message { |
||||
UBX_INF_DEBUG = 0x04, |
||||
UBX_INF_ERROR = 0x00, |
||||
UBX_INF_NOTICE = 0x02, |
||||
UBX_INF_TEST = 0x03, |
||||
UBX_INF_WARNING = 0x01, |
||||
}; |
||||
|
||||
enum ubx_logging_message { |
||||
UBX_LOG_BATCH = 0x11, |
||||
UBX_LOG_CREATE = 0x07, |
||||
UBX_LOG_ERASE = 0x03, |
||||
UBX_LOG_FINDTIME = 0x0E, |
||||
UBX_LOG_INFO = 0x08, |
||||
UBX_LOG_RETRIEVEBATCH = 0x10, |
||||
UBX_LOG_RETRIEVEPOSEXTRA = 0x0f, |
||||
UBX_LOG_RETRIEVEPOS = 0x0b, |
||||
UBX_LOG_RETRIEVESTRING = 0x0d, |
||||
UBX_LOG_RETRIEVE = 0x09, |
||||
UBX_LOG_STRING = 0x04, |
||||
}; |
||||
|
||||
enum ubx_multiple_gnss_assistance_message { |
||||
UBX_MGA_ACK = 0x60, |
||||
UBX_MGA_ANO = 0x20, |
||||
UBX_MGA_BDS = 0x03, |
||||
UBX_MGA_DBD = 0x80, |
||||
UBX_MGA_FLASH = 0x21, |
||||
UBX_MGA_GAL = 0x02, |
||||
UBX_MGA_GLO = 0x06, |
||||
UBX_MGA_GPS = 0x00, |
||||
UBX_MGA_INI = 0x40, |
||||
UBX_MGA_QZSS = 0x05, |
||||
}; |
||||
|
||||
enum ubx_monitoring_message { |
||||
UBX_MON_BATCH = 0x32, |
||||
UBX_MON_GNSS = 0x28, |
||||
UBX_MON_HW2 = 0x0B, |
||||
UBX_MON_HW = 0x09, |
||||
UBX_MON_IO = 0x02, |
||||
UBX_MON_MSGPP = 0x06, |
||||
UBX_MON_PATCH = 0x27, |
||||
UBX_MON_RXBUF = 0x07, |
||||
UBX_MON_RXR = 0x21, |
||||
UBX_MON_SMGR = 0x2E, |
||||
UBX_MON_SPT = 0x2F, |
||||
UBX_MON_TXBUF = 0x08, |
||||
UBX_MON_VER = 0x04, |
||||
}; |
||||
|
||||
enum ubx_nagivation_results_message { |
||||
UBX_NAV_AOPSTATUS = 0x60, |
||||
UBX_NAV_ATT = 0x05, |
||||
UBX_NAV_CLOCK = 0x22, |
||||
UBX_NAV_COV = 0x36, |
||||
UBX_NAV_DGPS = 0x31, |
||||
UBX_NAV_DOP = 0x04, |
||||
UBX_NAV_EELL = 0x3d, |
||||
UBX_NAV_EOE = 0x61, |
||||
UBX_NAV_GEOFENCE = 0x39, |
||||
UBX_NAV_HPPOSECEF = 0x13, |
||||
UBX_NAV_HPPOSLLH = 0x14, |
||||
UBX_NAV_NMI = 0x28, |
||||
UBX_NAV_ODO = 0x09, |
||||
UBX_NAV_ORB = 0x34, |
||||
UBX_NAV_POSECEF = 0x01, |
||||
UBX_NAV_POSLLH = 0x02, |
||||
UBX_NAV_PVT = 0x07, |
||||
UBX_NAV_RELPOSNED = 0x3C, |
||||
UBX_NAV_RESETODO = 0x10, |
||||
UBX_NAV_SAT = 0x35, |
||||
UBX_NAV_SBAS = 0x32, |
||||
UBX_NAV_SLAS = 0x42, |
||||
UBX_NAV_SOL = 0x06, |
||||
UBX_NAV_STATUS = 0x03, |
||||
UBX_NAV_SVINFO = 0x30, |
||||
UBX_NAV_SVIN = 0x3B, |
||||
UBX_NAV_TIMEBDS = 0x24, |
||||
UBX_NAV_TIMEGAL = 0x25, |
||||
UBX_NAV_TIMEGLO = 0x23, |
||||
UBX_NAV_TIMEGPS = 0x20, |
||||
UBX_NAV_TIMELS = 0x26, |
||||
UBX_NAV_TIMEUTC = 0x21, |
||||
UBX_NAV_VELECEF = 0x11, |
||||
UBX_NAV_VELNED = 0x12, |
||||
}; |
||||
|
||||
enum ubx_receiver_manager_message { |
||||
UBX_RXM_IMES = 0x61, |
||||
UBX_RXM_MEASX = 0x14, |
||||
UBX_RXM_PMREQ = 0x41, |
||||
UBX_RXM_RAWX = 0x15, |
||||
UBX_RXM_RLM = 0x59, |
||||
UBX_RXM_RTCM = 0x32, |
||||
UBX_RXM_SFRBX = 0x13, |
||||
}; |
||||
|
||||
enum ubx_timing_message { |
||||
UBX_TIM_DOSC = 0x11, |
||||
UBX_TIM_FCHG = 0x16, |
||||
UBX_TIM_HOC = 0x17, |
||||
UBX_TIM_SMEAS = 0x13, |
||||
UBX_TIM_SVIN = 0x04, |
||||
UBX_TIM_TM2 = 0x03, |
||||
UBX_TIM_TOS = 0x12, |
||||
UBX_TIM_TP = 0x01, |
||||
UBX_TIM_VCOCAL = 0x15, |
||||
UBX_TIM_VRFY = 0x06, |
||||
}; |
||||
|
||||
enum ubx_nmea_message_id { |
||||
UBX_NMEA_DTM = 0x0A, |
||||
UBX_NMEA_GBQ = 0x44, |
||||
UBX_NMEA_GBS = 0x09, |
||||
UBX_NMEA_GGA = 0x00, |
||||
UBX_NMEA_GLL = 0x01, |
||||
UBX_NMEA_GLQ = 0x43, |
||||
UBX_NMEA_GNQ = 0x42, |
||||
UBX_NMEA_GNS = 0x0D, |
||||
UBX_NMEA_GPQ = 0x40, |
||||
UBX_NMEA_GRS = 0x06, |
||||
UBX_NMEA_GSA = 0x02, |
||||
UBX_NMEA_GST = 0x07, |
||||
UBX_NMEA_GSV = 0x03, |
||||
UBX_NMEA_RMC = 0x04, |
||||
UBX_NMEA_THS = 0x0E, |
||||
UBX_NMEA_TXT = 0x41, |
||||
UBX_NMEA_VLW = 0x0F, |
||||
UBX_NMEA_VTG = 0x05, |
||||
UBX_NMEA_ZDA = 0x08, |
||||
}; |
||||
|
||||
#endif /* ZEPHYR_U_BLOX_PROTOCOL_DEFINES_ */ |
||||
@ -0,0 +1,147 @@
@@ -0,0 +1,147 @@
|
||||
/*
|
||||
* Copyright (c) 2025 Croxel Inc. |
||||
* Copyright (c) 2025 CogniPilot Foundation |
||||
* |
||||
* SPDX-License-Identifier: Apache-2.0 |
||||
*/ |
||||
|
||||
#include <zephyr/drivers/gnss.h> |
||||
#include <zephyr/drivers/gnss/gnss_publish.h> |
||||
|
||||
#include "gnss_ubx_common.h" |
||||
|
||||
void gnss_ubx_common_pvt_callback(struct modem_ubx *ubx, const struct ubx_frame *frame, |
||||
size_t len, void *user_data) |
||||
{ |
||||
if (len < UBX_FRAME_SZ(sizeof(struct ubx_nav_pvt))) { |
||||
return; |
||||
} |
||||
|
||||
struct gnss_ubx_common_data *data = user_data; |
||||
const struct device *dev = data->gnss; |
||||
const struct ubx_nav_pvt *nav_pvt = (const struct ubx_nav_pvt *)frame->payload_and_checksum; |
||||
|
||||
enum gnss_fix_quality fix_quality = GNSS_FIX_QUALITY_INVALID; |
||||
enum gnss_fix_status fix_status = GNSS_FIX_STATUS_NO_FIX; |
||||
|
||||
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: |
||||
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: |
||||
fix_quality = GNSS_FIX_QUALITY_GNSS_SPS; |
||||
fix_status = GNSS_FIX_STATUS_GNSS_FIX; |
||||
break; |
||||
default: |
||||
break; |
||||
} |
||||
} |
||||
|
||||
struct gnss_data gnss_data = { |
||||
.info = { |
||||
.satellites_cnt = nav_pvt->nav.num_sv, |
||||
.hdop = nav_pvt->nav.pdop * 10, |
||||
.geoid_separation = (nav_pvt->nav.height - |
||||
nav_pvt->nav.hmsl), |
||||
.fix_status = fix_status, |
||||
.fix_quality = fix_quality, |
||||
}, |
||||
.nav_data = { |
||||
.latitude = (int64_t)nav_pvt->nav.latitude * 100, |
||||
.longitude = (int64_t)nav_pvt->nav.longitude * 100, |
||||
.bearing = (((nav_pvt->nav.head_motion < 0) ? |
||||
(nav_pvt->nav.head_motion + (360 * 100000)) : |
||||
(nav_pvt->nav.head_motion)) / 100), |
||||
.speed = nav_pvt->nav.ground_speed, |
||||
.altitude = nav_pvt->nav.hmsl, |
||||
}, |
||||
.utc = { |
||||
.hour = nav_pvt->time.hour, |
||||
.minute = nav_pvt->time.minute, |
||||
.millisecond = (nav_pvt->time.second * 1000) + |
||||
(nav_pvt->time.nano / 1000000), |
||||
.month_day = nav_pvt->time.day, |
||||
.month = nav_pvt->time.month, |
||||
.century_year = (nav_pvt->time.year % 100), |
||||
}, |
||||
}; |
||||
|
||||
gnss_publish_data(dev, &gnss_data); |
||||
} |
||||
|
||||
#if CONFIG_GNSS_SATELLITES |
||||
void gnss_ubx_common_satellite_callback(struct modem_ubx *ubx, const struct ubx_frame *frame, |
||||
size_t len, void *user_data) |
||||
{ |
||||
if (len < UBX_FRAME_SZ(sizeof(struct ubx_nav_sat))) { |
||||
return; |
||||
} |
||||
|
||||
const struct ubx_nav_sat *ubx_sat = (const struct ubx_nav_sat *)frame->payload_and_checksum; |
||||
int num_satellites = (len - UBX_FRAME_SZ_WITHOUT_PAYLOAD - sizeof(struct ubx_nav_sat)) / |
||||
sizeof(struct ubx_nav_sat_info); |
||||
struct gnss_ubx_common_data *data = user_data; |
||||
const struct device *dev = data->gnss; |
||||
|
||||
num_satellites = MIN(num_satellites, data->satellites.size); |
||||
|
||||
for (size_t i = 0 ; i < num_satellites ; i++) { |
||||
enum gnss_system gnss_system = 0; |
||||
|
||||
switch (ubx_sat->sat[i].gnss_id) { |
||||
case UBX_GNSS_ID_GPS: |
||||
gnss_system = GNSS_SYSTEM_GPS; |
||||
break; |
||||
case UBX_GNSS_ID_SBAS: |
||||
gnss_system = GNSS_SYSTEM_SBAS; |
||||
break; |
||||
case UBX_GNSS_ID_GALILEO: |
||||
gnss_system = GNSS_SYSTEM_GALILEO; |
||||
break; |
||||
case UBX_GNSS_ID_BEIDOU: |
||||
gnss_system = GNSS_SYSTEM_BEIDOU; |
||||
break; |
||||
case UBX_GNSS_ID_QZSS: |
||||
gnss_system = GNSS_SYSTEM_QZSS; |
||||
break; |
||||
case UBX_GNSS_ID_GLONASS: |
||||
gnss_system = GNSS_SYSTEM_GLONASS; |
||||
break; |
||||
default: |
||||
break; |
||||
} |
||||
|
||||
struct gnss_satellite sat = { |
||||
/** TODO: Determine how to determine PRN from UBX sat info.
|
||||
* For now passing SV_ID. |
||||
*/ |
||||
.prn = ubx_sat->sat[i].sv_id, |
||||
.snr = ubx_sat->sat[i].cno, |
||||
.elevation = ubx_sat->sat[i].elevation, |
||||
.azimuth = ubx_sat->sat[i].azimuth, |
||||
.system = gnss_system, |
||||
.is_tracked = (ubx_sat->sat[i].flags & UBX_NAV_SAT_FLAGS_SV_USED), |
||||
}; |
||||
|
||||
data->satellites.data[i] = sat; |
||||
} |
||||
|
||||
gnss_publish_satellites(dev, data->satellites.data, num_satellites); |
||||
} |
||||
#endif |
||||
|
||||
void gnss_ubx_common_init(struct gnss_ubx_common_data *data, |
||||
const struct gnss_ubx_common_config *config) |
||||
{ |
||||
data->gnss = config->gnss; |
||||
#if CONFIG_GNSS_SATELLITES |
||||
data->satellites.data = config->satellites.buf; |
||||
data->satellites.size = config->satellites.size; |
||||
#endif |
||||
} |
||||
@ -0,0 +1,44 @@
@@ -0,0 +1,44 @@
|
||||
/*
|
||||
* Copyright (c) 2023 Trackunit Corporation |
||||
* Copyright (c) 2025 Croxel Inc. |
||||
* Copyright (c) 2025 CogniPilot Foundation |
||||
* |
||||
* SPDX-License-Identifier: Apache-2.0 |
||||
*/ |
||||
|
||||
#ifndef ZEPHYR_DRIVERS_GNSS_GNSS_UBX_COMMON_H_ |
||||
#define ZEPHYR_DRIVERS_GNSS_GNSS_UBX_COMMON_H_ |
||||
|
||||
#include <zephyr/device.h> |
||||
#include <zephyr/drivers/gnss.h> |
||||
#include <zephyr/modem/ubx.h> |
||||
|
||||
struct gnss_ubx_common_data { |
||||
const struct device *gnss; |
||||
struct gnss_data data; |
||||
#if CONFIG_GNSS_SATELLITES |
||||
struct { |
||||
struct gnss_satellite *data; |
||||
size_t size; |
||||
} satellites; |
||||
#endif |
||||
}; |
||||
|
||||
struct gnss_ubx_common_config { |
||||
const struct device *gnss; |
||||
struct { |
||||
struct gnss_satellite *buf; |
||||
size_t size; |
||||
} satellites; |
||||
}; |
||||
|
||||
void gnss_ubx_common_pvt_callback(struct modem_ubx *ubx, const struct ubx_frame *frame, |
||||
size_t len, void *user_data); |
||||
|
||||
void gnss_ubx_common_satellite_callback(struct modem_ubx *ubx, const struct ubx_frame *frame, |
||||
size_t len, void *user_data); |
||||
|
||||
void gnss_ubx_common_init(struct gnss_ubx_common_data *data, |
||||
const struct gnss_ubx_common_config *config); |
||||
|
||||
#endif /* ZEPHYR_DRIVERS_GNSS_GNSS_UBX_COMMON_H_ */ |
||||
@ -0,0 +1,38 @@
@@ -0,0 +1,38 @@
|
||||
/*
|
||||
* Copyright (c) 2025 Croxel Inc. |
||||
* Copyright (c) 2025 CogniPilot Foundation |
||||
* |
||||
* SPDX-License-Identifier: Apache-2.0 |
||||
*/ |
||||
|
||||
#ifndef ZEPHYR_MODEM_UBX_CHECKSUM_ |
||||
#define ZEPHYR_MODEM_UBX_CHECKSUM_ |
||||
|
||||
/** Macrobatics to compute UBX checksum at compile time */ |
||||
|
||||
#define UBX_CSUM_A(...) UBX_CSUM_A_(__VA_ARGS__) |
||||
|
||||
#define UBX_CSUM_A_(...) UBX_CSUM_A_I(__VA_ARGS__, \ |
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) |
||||
|
||||
#define UBX_CSUM_A_I(a1, a2, a3, a4, a5, a6, a7, a8, a9, a10, \ |
||||
a11, a12, a13, a14, a15, a16, a17, a18, a19, a20, ...) \ |
||||
((a1) + (a2) + (a3) + (a4) + (a5) + (a6) + (a7) + (a8) + (a9) + (a10) + \ |
||||
(a11) + (a12) + (a13) + (a14) + (a15) + (a16) + (a17) + (a18) + (a19) + (a20)) & 0xFF |
||||
|
||||
#define UBX_CSUM_B(...) UBX_CSUM_B_(__VA_ARGS__) |
||||
|
||||
#define UBX_CSUM_B_(...) UBX_CSUM_B_I(NUM_VA_ARGS(__VA_ARGS__), __VA_ARGS__, \ |
||||
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) |
||||
|
||||
#define UBX_CSUM_B_I(len, a1, a2, a3, a4, a5, a6, a7, a8, a9, a10, \ |
||||
a11, a12, a13, a14, a15, a16, a17, a18, a19, a20, ...) \ |
||||
(((len) * a1) + ((len - 1) * a2) + ((len - 2) * a3) + ((len - 3) * a4) + \ |
||||
((len - 4) * a5) + ((len - 5) * a6) + ((len - 6) * a7) + ((len - 7) * a8) + \ |
||||
((len - 8) * a9) + ((len - 9) * a10) + ((len - 10) * a11) + ((len - 11) * a12) + \ |
||||
((len - 12) * a13) + ((len - 13) * a14) + ((len - 14) * a15) + ((len - 15) * a16) + \ |
||||
((len - 16) * a17) + ((len - 17) * a18) + ((len - 18) * a19) + ((len - 19) * a20)) & 0xFF |
||||
|
||||
#define UBX_CSUM(...) UBX_CSUM_A(__VA_ARGS__), UBX_CSUM_B(__VA_ARGS__) |
||||
|
||||
#endif /* ZEPHYR_MODEM_UBX_CHECKSUM_ */ |
||||
@ -0,0 +1,478 @@
@@ -0,0 +1,478 @@
|
||||
/*
|
||||
* Copyright (c) 2025 Croxel Inc. |
||||
* Copyright (c) 2025 CogniPilot Foundation |
||||
* |
||||
* SPDX-License-Identifier: Apache-2.0 |
||||
*/ |
||||
|
||||
#ifndef ZEPHYR_MODEM_UBX_PROTOCOL_ |
||||
#define ZEPHYR_MODEM_UBX_PROTOCOL_ |
||||
|
||||
#include <stdint.h> |
||||
#include <zephyr/modem/ubx/checksum.h> |
||||
|
||||
#define UBX_FRAME_HEADER_SZ 6 |
||||
#define UBX_FRAME_FOOTER_SZ 2 |
||||
#define UBX_FRAME_SZ_WITHOUT_PAYLOAD (UBX_FRAME_HEADER_SZ + UBX_FRAME_FOOTER_SZ) |
||||
#define UBX_FRAME_SZ(payload_size) (payload_size + UBX_FRAME_SZ_WITHOUT_PAYLOAD) |
||||
|
||||
#define UBX_PREAMBLE_SYNC_CHAR_1 0xB5 |
||||
#define UBX_PREAMBLE_SYNC_CHAR_2 0x62 |
||||
|
||||
#define UBX_FRAME_PREAMBLE_SYNC_CHAR_1_IDX 0 |
||||
#define UBX_FRAME_PREAMBLE_SYNC_CHAR_2_IDX 1 |
||||
#define UBX_FRAME_MSG_CLASS_IDX 2 |
||||
|
||||
#define UBX_PAYLOAD_SZ_MAX 512 |
||||
#define UBX_FRAME_SZ_MAX UBX_FRAME_SZ(UBX_PAYLOAD_SZ_MAX) |
||||
|
||||
struct ubx_frame { |
||||
uint8_t preamble_sync_char_1; |
||||
uint8_t preamble_sync_char_2; |
||||
uint8_t class; |
||||
uint8_t id; |
||||
uint16_t payload_size; |
||||
uint8_t payload_and_checksum[]; |
||||
}; |
||||
|
||||
struct ubx_frame_match { |
||||
uint8_t class; |
||||
uint8_t id; |
||||
struct { |
||||
uint8_t *buf; |
||||
uint16_t len; |
||||
} payload; |
||||
}; |
||||
|
||||
enum ubx_class_id { |
||||
UBX_CLASS_ID_NAV = 0x01, /* Navigation Results Messages */ |
||||
UBX_CLASS_ID_RXM = 0x02, /* Receiver Manager Messages */ |
||||
UBX_CLASS_ID_INF = 0x04, /* Information Messages */ |
||||
UBX_CLASS_ID_ACK = 0x05, /* Ack/Nak Messages */ |
||||
UBX_CLASS_ID_CFG = 0x06, /* Configuration Input Messages */ |
||||
UBX_CLASS_ID_UPD = 0x09, /* Firmware Update Messages */ |
||||
UBX_CLASS_ID_MON = 0x0A, /* Monitoring Messages */ |
||||
UBX_CLASS_ID_TIM = 0x0D, /* Timing Messages */ |
||||
UBX_CLASS_ID_MGA = 0x13, /* Multiple GNSS Assistance Messages */ |
||||
UBX_CLASS_ID_LOG = 0x21, /* Logging Messages */ |
||||
UBX_CLASS_ID_SEC = 0x27, /* Security Feature Messages */ |
||||
UBX_CLASS_ID_NMEA_STD = 0xF0, /* Note: Only used to configure message rate */ |
||||
UBX_CLASS_ID_NMEA_PUBX = 0xF1, /* Note: Only used to configure message rate */ |
||||
}; |
||||
|
||||
enum ubx_msg_id_nav { |
||||
UBX_MSG_ID_NAV_PVT = 0x07, |
||||
UBX_MSG_ID_NAV_SAT = 0x35, |
||||
}; |
||||
|
||||
enum ubx_nav_fix_type { |
||||
UBX_NAV_FIX_TYPE_NO_FIX = 0, |
||||
UBX_NAV_FIX_TYPE_DR = 1, |
||||
UBX_NAV_FIX_TYPE_2D = 2, |
||||
UBX_NAV_FIX_TYPE_3D = 3, |
||||
UBX_NAV_FIX_TYPE_GNSS_DR_COMBINED = 4, |
||||
UBX_NAV_FIX_TYPE_TIME_ONLY = 5, |
||||
}; |
||||
|
||||
#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_FLAGS3_INVALID_LLH BIT(0) |
||||
|
||||
struct ubx_nav_pvt { |
||||
struct { |
||||
uint32_t itow; |
||||
uint16_t year; |
||||
uint8_t month; |
||||
uint8_t day; |
||||
uint8_t hour; |
||||
uint8_t minute; |
||||
uint8_t second; |
||||
uint8_t valid; |
||||
uint32_t tacc; |
||||
int32_t nano; |
||||
} __packed time; |
||||
uint8_t fix_type; /** See ubx_nav_fix_type */ |
||||
uint8_t flags; |
||||
uint8_t flags2; |
||||
struct { |
||||
uint8_t num_sv; |
||||
int32_t longitude; /* Longitude. Degrees. scaling: 1e-7 */ |
||||
int32_t latitude; /* Latitude. Degrees. scaling: 1e-7 */ |
||||
int32_t height; /* Height above ellipsoid. mm */ |
||||
int32_t hmsl; /* Height above mean sea level. mm */ |
||||
uint32_t horiz_acc; /* Horizontal accuracy estimate. mm */ |
||||
uint32_t vert_acc; /* Vertical accuracy estimate. mm */ |
||||
int32_t vel_north; /* NED north velocity. mm/s */ |
||||
int32_t vel_east; /* NED east velocity. mm/s */ |
||||
int32_t vel_down; /* NED down velocity. mm/s */ |
||||
int32_t ground_speed; /* Ground Speed (2D). mm/s */ |
||||
int32_t head_motion; /* Heading of Motion (2D). Degrees. scaling: 1e-5 */ |
||||
uint32_t speed_acc; /* Speed accuracy estimated. mm/s */ |
||||
uint32_t head_acc; /** Heading accuracy estimate (both motion and vehicle).
|
||||
* Degrees. scaling: 1e-5. |
||||
*/ |
||||
uint16_t pdop; /* scaling: 1e-2 */ |
||||
uint16_t flags3; |
||||
uint32_t reserved; |
||||
int32_t head_vehicle; /* Heading of vehicle (2D). Degrees. Valid if
|
||||
* flags.head_vehicle_valid is set. |
||||
*/ |
||||
int16_t mag_decl; /* Magnetic declination. Degrees. */ |
||||
uint16_t magacc; /* Magnetic declination accuracy. Degrees. scaling: 1e-2 */ |
||||
} __packed nav; |
||||
} __packed; |
||||
|
||||
enum ubx_nav_sat_health { |
||||
UBX_NAV_SAT_HEALTH_UNKNOWN = 0, |
||||
UBX_NAV_SAT_HEALTH_HEALTHY = 1, |
||||
UBX_NAV_SAT_HEALTH_UNHEALTHY = 2, |
||||
}; |
||||
|
||||
enum ubx_gnss_id { |
||||
UBX_GNSS_ID_GPS = 0, |
||||
UBX_GNSS_ID_SBAS = 1, |
||||
UBX_GNSS_ID_GALILEO = 2, |
||||
UBX_GNSS_ID_BEIDOU = 3, |
||||
UBX_GNSS_ID_QZSS = 5, |
||||
UBX_GNSS_ID_GLONASS = 6, |
||||
}; |
||||
|
||||
#define UBX_NAV_SAT_FLAGS_SV_USED BIT(3) |
||||
|
||||
struct ubx_nav_sat { |
||||
uint32_t itow; |
||||
uint8_t version; /* Message version. */ |
||||
uint8_t num_sv; |
||||
uint16_t reserved1; |
||||
struct ubx_nav_sat_info { |
||||
uint8_t gnss_id; /* See ubx_gnss_id */ |
||||
uint8_t sv_id; |
||||
uint8_t cno; /* Carrier-to-noise ratio. dBHz */ |
||||
int8_t elevation; /* Elevation (range: +/- 90). Degrees */ |
||||
int16_t azimuth; /* Azimuth (range: 0 - 360). Degrees */ |
||||
int16_t pseu_res; /* Pseudorange Residual. Meters */ |
||||
uint32_t flags; |
||||
} sat[]; |
||||
}; |
||||
|
||||
enum ubx_msg_id_ack { |
||||
UBX_MSG_ID_ACK = 0x01, |
||||
UBX_MSG_ID_NAK = 0x00 |
||||
}; |
||||
|
||||
enum ubx_msg_id_cfg { |
||||
UBX_MSG_ID_CFG_PRT = 0x00, |
||||
UBX_MSG_ID_CFG_MSG = 0x01, |
||||
UBX_MSG_ID_CFG_RST = 0x04, |
||||
UBX_MSG_ID_CFG_RATE = 0x08, |
||||
UBX_MSG_ID_CFG_NAV5 = 0x24, |
||||
}; |
||||
|
||||
enum ubx_msg_id_mon { |
||||
UBX_MSG_ID_MON_VER = 0x04, |
||||
UBX_MSG_ID_MON_GNSS = 0x28, |
||||
}; |
||||
|
||||
struct ubx_ack { |
||||
uint8_t class; |
||||
uint8_t id; |
||||
}; |
||||
|
||||
#define UBX_GNSS_SELECTION_GPS BIT(0) |
||||
#define UBX_GNSS_SELECTION_GLONASS BIT(1) |
||||
#define UBX_GNSS_SELECTION_BEIDOU BIT(2) |
||||
#define UBX_GNSS_SELECTION_GALILEO BIT(3) |
||||
|
||||
struct ubx_mon_gnss { |
||||
uint8_t ver; |
||||
struct { |
||||
uint8_t supported; |
||||
uint8_t default_enabled; |
||||
uint8_t enabled; |
||||
} selection; |
||||
uint8_t simultaneous; |
||||
uint8_t reserved1[3]; |
||||
} __packed; |
||||
|
||||
enum ubx_cfg_port_id { |
||||
UBX_CFG_PORT_ID_DDC = 0, |
||||
UBX_CFG_PORT_ID_UART = 1, |
||||
UBX_CFG_PORT_ID_USB = 2, |
||||
UBX_CFG_PORT_ID_SPI = 3, |
||||
}; |
||||
|
||||
enum ubx_cfg_char_len { |
||||
UBX_CFG_PRT_PORT_MODE_CHAR_LEN_5 = 0, /* Not supported */ |
||||
UBX_CFG_PRT_PORT_MODE_CHAR_LEN_6 = 1, /* Not supported */ |
||||
UBX_CFG_PRT_PORT_MODE_CHAR_LEN_7 = 2, /* Supported only with parity */ |
||||
UBX_CFG_PRT_PORT_MODE_CHAR_LEN_8 = 3, |
||||
}; |
||||
|
||||
enum ubx_cfg_parity { |
||||
UBX_CFG_PRT_PORT_MODE_PARITY_EVEN = 0, |
||||
UBX_CFG_PRT_PORT_MODE_PARITY_ODD = 1, |
||||
UBX_CFG_PRT_PORT_MODE_PARITY_NONE = 4, |
||||
}; |
||||
|
||||
enum ubx_cfg_stop_bits { |
||||
UBX_CFG_PRT_PORT_MODE_STOP_BITS_1 = 0, |
||||
UBX_CFG_PRT_PORT_MODE_STOP_BITS_1_5 = 1, |
||||
UBX_CFG_PRT_PORT_MODE_STOP_BITS_2 = 2, |
||||
UBX_CFG_PRT_PORT_MODE_STOP_BITS_0_5 = 3, |
||||
}; |
||||
|
||||
#define UBX_CFG_PRT_MODE_CHAR_LEN(val) (((val) & BIT_MASK(2)) << 6) |
||||
#define UBX_CFG_PRT_MODE_PARITY(val) (((val) & BIT_MASK(3)) << 9) |
||||
#define UBX_CFG_PRT_MODE_STOP_BITS(val) (((val) & BIT_MASK(2)) << 12) |
||||
|
||||
#define UBX_CFG_PRT_PROTO_MASK_UBX BIT(0) |
||||
#define UBX_CFG_PRT_PROTO_MASK_NMEA BIT(1) |
||||
#define UBX_CFG_PRT_PROTO_MASK_RTCM3 BIT(5) |
||||
|
||||
struct ubx_cfg_prt { |
||||
uint8_t port_id; /* See ubx_cfg_port_id */ |
||||
uint8_t reserved1; |
||||
uint16_t rx_ready_pin; |
||||
uint32_t mode; |
||||
uint32_t baudrate; |
||||
uint16_t in_proto_mask; |
||||
uint16_t out_proto_mask; |
||||
uint16_t flags; |
||||
uint16_t reserved2; |
||||
}; |
||||
|
||||
enum ubx_dyn_model { |
||||
UBX_DYN_MODEL_PORTABLE = 0, |
||||
UBX_DYN_MODEL_STATIONARY = 2, |
||||
UBX_DYN_MODEL_PEDESTRIAN = 3, |
||||
UBX_DYN_MODEL_AUTOMOTIVE = 4, |
||||
UBX_DYN_MODEL_SEA = 5, |
||||
UBX_DYN_MODEL_AIRBORNE_1G = 6, |
||||
UBX_DYN_MODEL_AIRBORNE_2G = 7, |
||||
UBX_DYN_MODEL_AIRBORNE_4G = 8, |
||||
UBX_DYN_MODEL_WRIST = 9, |
||||
UBX_DYN_MODEL_BIKE = 10, |
||||
}; |
||||
|
||||
enum ubx_fix_mode { |
||||
UBX_FIX_MODE_2D_ONLY = 1, |
||||
UBX_FIX_MODE_3D_ONLY = 2, |
||||
UBX_FIX_MODE_AUTO = 3, |
||||
}; |
||||
|
||||
enum ubx_utc_standard { |
||||
UBX_UTC_STANDARD_AUTOMATIC = 0, |
||||
UBX_UTC_STANDARD_GPS = 3, |
||||
UBX_UTC_STANDARD_GALILEO = 5, |
||||
UBX_UTC_STANDARD_GLONASS = 6, |
||||
UBX_UTC_STANDARD_BEIDOU = 7, |
||||
}; |
||||
|
||||
#define UBX_CFG_NAV5_APPLY_DYN BIT(0) |
||||
#define UBX_CFG_NAV5_APPLY_FIX_MODE BIT(2) |
||||
|
||||
struct ubx_cfg_nav5 { |
||||
uint16_t apply; |
||||
uint8_t dyn_model; /* Dynamic platform model. See ubx_dyn_model */ |
||||
uint8_t fix_mode; /* Position fixing mode. See ubx_fix_mode */ |
||||
int32_t fixed_alt; /* Fixed altitude for 2D fix mode. Meters */ |
||||
uint32_t fixed_alt_var; /* Variance for Fixed altitude in 2D mode. Sq. meters */ |
||||
int8_t min_elev; /* Minimum Elevation to use a GNSS satellite in Navigation. Degrees */ |
||||
uint8_t dr_limit; /* Reserved */ |
||||
uint16_t p_dop; /* Position DOP mask */ |
||||
uint16_t t_dop; /* Time DOP mask */ |
||||
uint16_t p_acc; /* Position accuracy mask. Meters */ |
||||
uint16_t t_acc; /* Time accuracy mask. Meters */ |
||||
uint8_t static_hold_thresh; /* Static hold threshold. cm/s */ |
||||
uint8_t dgnss_timeout; /* DGNSS timeout. Seconds */ |
||||
uint8_t cno_thresh_num_svs; /* Number of satellites required above cno_thresh */ |
||||
uint8_t cno_thresh; /* C/N0 threshold for GNSS signals. dbHz */ |
||||
uint8_t reserved1[2]; |
||||
uint16_t static_hold_max_dist; /* Static hold distance threshold. Meters */ |
||||
uint8_t utc_standard; /* UTC standard to be used. See ubx_utc_standard */ |
||||
uint8_t reserved2[5]; |
||||
} __packed; |
||||
|
||||
enum ubx_cfg_rst_start_mode { |
||||
UBX_CFG_RST_HOT_START = 0x0000, |
||||
UBX_CFG_RST_WARM_START = 0x0001, |
||||
UBX_CFG_RST_COLD_START = 0xFFFF, |
||||
}; |
||||
|
||||
enum ubx_cfg_rst_mode { |
||||
UBX_CFG_RST_MODE_HW = 0x00, |
||||
UBX_CFG_RST_MODE_SW = 0x01, |
||||
UBX_CFG_RST_MODE_GNSS_STOP = 0x08, |
||||
UBX_CFG_RST_MODE_GNSS_START = 0x09, |
||||
}; |
||||
|
||||
struct ubx_cfg_rst { |
||||
uint16_t nav_bbr_mask; |
||||
uint8_t reset_mode; |
||||
uint8_t reserved; |
||||
}; |
||||
|
||||
enum ubx_cfg_rate_time_ref { |
||||
UBX_CFG_RATE_TIME_REF_UTC = 0, |
||||
UBX_CFG_RATE_TIME_REF_GPS = 1, |
||||
UBX_CFG_RATE_TIME_REF_GLONASS = 2, |
||||
UBX_CFG_RATE_TIME_REF_BEIDOU = 3, |
||||
UBX_CFG_RATE_TIME_REF_GALILEO = 4, |
||||
UBX_CFG_RATE_TIME_REF_NAVIC = 5, |
||||
}; |
||||
|
||||
struct ubx_cfg_rate { |
||||
uint16_t meas_rate_ms; |
||||
uint16_t nav_rate; |
||||
uint16_t time_ref; |
||||
}; |
||||
|
||||
enum ubx_msg_id_nmea_std { |
||||
UBX_MSG_ID_NMEA_STD_DTM = 0x0A, |
||||
UBX_MSG_ID_NMEA_STD_GBQ = 0x44, |
||||
UBX_MSG_ID_NMEA_STD_GBS = 0x09, |
||||
UBX_MSG_ID_NMEA_STD_GGA = 0x00, |
||||
UBX_MSG_ID_NMEA_STD_GLL = 0x01, |
||||
UBX_MSG_ID_NMEA_STD_GLQ = 0x43, |
||||
UBX_MSG_ID_NMEA_STD_GNQ = 0x42, |
||||
UBX_MSG_ID_NMEA_STD_GNS = 0x0D, |
||||
UBX_MSG_ID_NMEA_STD_GPQ = 0x40, |
||||
UBX_MSG_ID_NMEA_STD_GRS = 0x06, |
||||
UBX_MSG_ID_NMEA_STD_GSA = 0x02, |
||||
UBX_MSG_ID_NMEA_STD_GST = 0x07, |
||||
UBX_MSG_ID_NMEA_STD_GSV = 0x03, |
||||
UBX_MSG_ID_NMEA_STD_RMC = 0x04, |
||||
UBX_MSG_ID_NMEA_STD_THS = 0x0E, |
||||
UBX_MSG_ID_NMEA_STD_TXT = 0x41, |
||||
UBX_MSG_ID_NMEA_STD_VLW = 0x0F, |
||||
UBX_MSG_ID_NMEA_STD_VTG = 0x05, |
||||
UBX_MSG_ID_NMEA_STD_ZDA = 0x08, |
||||
}; |
||||
|
||||
enum ubx_msg_id_nmea_pubx { |
||||
UBX_MSG_ID_NMEA_PUBX_CONFIG = 0x41, |
||||
UBX_MSG_ID_NMEA_PUBX_POSITION = 0x00, |
||||
UBX_MSG_ID_NMEA_PUBX_RATE = 0x40, |
||||
UBX_MSG_ID_NMEA_PUBX_SVSTATUS = 0x03, |
||||
UBX_MSG_ID_NMEA_PUBX_TIME = 0x04, |
||||
}; |
||||
|
||||
struct ubx_cfg_msg_rate { |
||||
uint8_t class; |
||||
uint8_t id; |
||||
uint8_t rate; |
||||
}; |
||||
|
||||
struct ubx_mon_ver { |
||||
char sw_ver[30]; |
||||
char hw_ver[10]; |
||||
}; |
||||
|
||||
static inline uint16_t ubx_calc_checksum(const struct ubx_frame *frame, size_t len) |
||||
{ |
||||
uint8_t ck_a = 0; |
||||
uint8_t ck_b = 0; |
||||
const uint8_t *data = (const uint8_t *)frame; |
||||
|
||||
/** Mismatch in expected and actual length results in an invalid frame */ |
||||
if (len != UBX_FRAME_SZ(frame->payload_size)) { |
||||
return 0xFFFF; |
||||
} |
||||
|
||||
for (int i = UBX_FRAME_MSG_CLASS_IDX ; i < (UBX_FRAME_SZ(frame->payload_size) - 2) ; i++) { |
||||
ck_a = ck_a + data[i]; |
||||
ck_b = ck_b + ck_a; |
||||
} |
||||
|
||||
return ((ck_a & 0xFF) | ((ck_b & 0xFF) << 8)); |
||||
} |
||||
|
||||
static inline int ubx_frame_encode(uint8_t class, uint8_t id, |
||||
const uint8_t *payload, size_t payload_len, |
||||
uint8_t *buf, size_t buf_len) |
||||
{ |
||||
if (buf_len < UBX_FRAME_SZ(payload_len)) { |
||||
return -EINVAL; |
||||
} |
||||
|
||||
struct ubx_frame *frame = (struct ubx_frame *)buf; |
||||
|
||||
frame->preamble_sync_char_1 = UBX_PREAMBLE_SYNC_CHAR_1; |
||||
frame->preamble_sync_char_2 = UBX_PREAMBLE_SYNC_CHAR_2; |
||||
frame->class = class; |
||||
frame->id = id; |
||||
frame->payload_size = payload_len; |
||||
memcpy(frame->payload_and_checksum, payload, payload_len); |
||||
|
||||
uint16_t checksum = ubx_calc_checksum(frame, UBX_FRAME_SZ(payload_len)); |
||||
|
||||
frame->payload_and_checksum[payload_len] = checksum & 0xFF; |
||||
frame->payload_and_checksum[payload_len + 1] = (checksum >> 8) & 0xFF; |
||||
|
||||
return UBX_FRAME_SZ(payload_len); |
||||
} |
||||
|
||||
#define UBX_FRAME_DEFINE(_name, _frame) \ |
||||
const static struct ubx_frame _name = _frame |
||||
|
||||
#define UBX_FRAME_ARRAY_DEFINE(_name, ...) \ |
||||
const struct ubx_frame *_name[] = {__VA_ARGS__}; |
||||
|
||||
#define UBX_FRAME_ACK_INITIALIZER(_class_id, _msg_id) \ |
||||
UBX_FRAME_INITIALIZER_PAYLOAD(UBX_CLASS_ID_ACK, UBX_MSG_ID_ACK, _class_id, _msg_id) |
||||
|
||||
#define UBX_FRAME_NAK_INITIALIZER(_class_id, _msg_id) \ |
||||
UBX_FRAME_INITIALIZER_PAYLOAD(UBX_CLASS_ID_ACK, UBX_MSG_ID_NAK, _class_id, _msg_id) |
||||
|
||||
#define UBX_FRAME_CFG_RST_INITIALIZER(_start_mode, _reset_mode) \ |
||||
UBX_FRAME_INITIALIZER_PAYLOAD(UBX_CLASS_ID_CFG, UBX_MSG_ID_CFG_RST, \ |
||||
(_start_mode & 0xFF), ((_start_mode >> 8) & 0xFF), \ |
||||
_reset_mode, 0) |
||||
|
||||
#define UBX_FRAME_CFG_RATE_INITIALIZER(_meas_rate_ms, _nav_rate, _time_ref) \ |
||||
UBX_FRAME_INITIALIZER_PAYLOAD(UBX_CLASS_ID_CFG, UBX_MSG_ID_CFG_RATE, \ |
||||
(_meas_rate_ms & 0xFF), ((_meas_rate_ms >> 8) & 0xFF), \ |
||||
(_nav_rate & 0xFF), ((_nav_rate >> 8) & 0xFF), \ |
||||
(_time_ref & 0xFF), ((_time_ref >> 8) & 0xFF)) |
||||
|
||||
#define UBX_FRAME_CFG_MSG_RATE_INITIALIZER(_class_id, _msg_id, _rate) \ |
||||
UBX_FRAME_INITIALIZER_PAYLOAD(UBX_CLASS_ID_CFG, UBX_MSG_ID_CFG_MSG, \ |
||||
_class_id, _msg_id, _rate) |
||||
|
||||
#define UBX_FRAME_INITIALIZER_PAYLOAD(_class_id, _msg_id, ...) \ |
||||
_UBX_FRAME_INITIALIZER_PAYLOAD(_class_id, _msg_id, __VA_ARGS__) |
||||
|
||||
#define _UBX_FRAME_INITIALIZER_PAYLOAD(_class_id, _msg_id, ...) \ |
||||
{ \ |
||||
.preamble_sync_char_1 = UBX_PREAMBLE_SYNC_CHAR_1, \ |
||||
.preamble_sync_char_2 = UBX_PREAMBLE_SYNC_CHAR_2, \ |
||||
.class = _class_id, \ |
||||
.id = _msg_id, \ |
||||
.payload_size = (NUM_VA_ARGS(__VA_ARGS__)) & 0xFFFF, \ |
||||
.payload_and_checksum = { \ |
||||
__VA_ARGS__, \ |
||||
UBX_CSUM(_class_id, _msg_id, \ |
||||
((NUM_VA_ARGS(__VA_ARGS__)) & 0xFF), \ |
||||
(((NUM_VA_ARGS(__VA_ARGS__)) >> 8) & 0xFF), \ |
||||
__VA_ARGS__), \ |
||||
}, \ |
||||
} |
||||
|
||||
#define UBX_FRAME_GET_INITIALIZER(_class_id, _msg_id) \ |
||||
{ \ |
||||
.preamble_sync_char_1 = UBX_PREAMBLE_SYNC_CHAR_1, \ |
||||
.preamble_sync_char_2 = UBX_PREAMBLE_SYNC_CHAR_2, \ |
||||
.class = _class_id, \ |
||||
.id = _msg_id, \ |
||||
.payload_size = 0, \ |
||||
.payload_and_checksum = { \ |
||||
UBX_CSUM(_class_id, _msg_id, 0, 0), \ |
||||
}, \ |
||||
} |
||||
|
||||
#endif /* ZEPHYR_MODEM_UBX_PROTOCOL_ */ |
||||
@ -0,0 +1,11 @@
@@ -0,0 +1,11 @@
|
||||
# Copyright (c) 2023 Trackunit Corporation |
||||
# 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(modem_ubx_test) |
||||
|
||||
target_sources(app PRIVATE src/main.c ../mock/modem_backend_mock.c) |
||||
target_include_directories(app PRIVATE ../mock) |
||||
@ -0,0 +1,9 @@
@@ -0,0 +1,9 @@
|
||||
# Copyright (c) 2023 Trackunit Corporation |
||||
# Copyright (c) 2025 Croxel Inc. |
||||
# Copyright (c) 2025 CogniPilot Foundation |
||||
# SPDX-License-Identifier: Apache-2.0 |
||||
|
||||
CONFIG_MODEM_MODULES=y |
||||
CONFIG_MODEM_UBX=y |
||||
|
||||
CONFIG_ZTEST=y |
||||
@ -0,0 +1,561 @@
@@ -0,0 +1,561 @@
|
||||
/*
|
||||
* Copyright (c) 2022 Trackunit Corporation |
||||
* Copyright (c) 2025 Croxel Inc. |
||||
* Copyright (c) 2025 CogniPilot Foundation |
||||
* |
||||
* SPDX-License-Identifier: Apache-2.0 |
||||
*/ |
||||
|
||||
#include <zephyr/ztest.h> |
||||
#include <zephyr/kernel.h> |
||||
|
||||
#include <zephyr/modem/ubx.h> |
||||
#include <zephyr/modem/ubx/protocol.h> |
||||
#include <modem_backend_mock.h> |
||||
|
||||
static struct modem_ubx cmd; |
||||
|
||||
static uint32_t cmd_user_data = 0x145212; |
||||
static uint8_t cmd_receive_buf[128]; |
||||
|
||||
static uint8_t cmd_response[128]; |
||||
|
||||
static struct modem_backend_mock mock; |
||||
static uint8_t mock_rx_buf[128]; |
||||
static uint8_t mock_tx_buf[128]; |
||||
static struct modem_pipe *mock_pipe; |
||||
|
||||
#define MODEM_UBX_UTEST_ON_NAK_RECEIVED_BIT (0) |
||||
#define MODEM_UBX_UTEST_ON_ACK_RECEIVED_BIT (1) |
||||
|
||||
static atomic_t callback_called; |
||||
|
||||
static void on_nak_received(struct modem_ubx *ubx, const struct ubx_frame *frame, size_t len, |
||||
void *user_data) |
||||
{ |
||||
atomic_set_bit(&callback_called, MODEM_UBX_UTEST_ON_NAK_RECEIVED_BIT); |
||||
} |
||||
|
||||
static void on_ack_received(struct modem_ubx *ubx, const struct ubx_frame *frame, size_t len, |
||||
void *user_data) |
||||
{ |
||||
atomic_set_bit(&callback_called, MODEM_UBX_UTEST_ON_ACK_RECEIVED_BIT); |
||||
} |
||||
|
||||
MODEM_UBX_MATCH_ARRAY_DEFINE(unsol_matches, |
||||
MODEM_UBX_MATCH_DEFINE(UBX_CLASS_ID_ACK, UBX_MSG_ID_ACK, on_ack_received), |
||||
MODEM_UBX_MATCH_DEFINE(UBX_CLASS_ID_ACK, UBX_MSG_ID_NAK, on_nak_received) |
||||
); |
||||
|
||||
static struct ubx_frame test_req = UBX_FRAME_ACK_INITIALIZER(0x01, 0x02); |
||||
|
||||
struct script_runner { |
||||
struct modem_ubx_script script; |
||||
struct { |
||||
bool done; |
||||
int ret; |
||||
} result; |
||||
}; |
||||
|
||||
static struct script_runner test_script_runner; |
||||
|
||||
static void *test_setup(void) |
||||
{ |
||||
const struct modem_ubx_config cmd_config = { |
||||
.user_data = &cmd_user_data, |
||||
.receive_buf = cmd_receive_buf, |
||||
.receive_buf_size = ARRAY_SIZE(cmd_receive_buf), |
||||
.unsol_matches = { |
||||
.array = unsol_matches, |
||||
.size = ARRAY_SIZE(unsol_matches), |
||||
}, |
||||
}; |
||||
|
||||
zassert(modem_ubx_init(&cmd, &cmd_config) == 0, "Failed to init modem CMD"); |
||||
|
||||
const struct modem_backend_mock_config mock_config = { |
||||
.rx_buf = mock_rx_buf, |
||||
.rx_buf_size = ARRAY_SIZE(mock_rx_buf), |
||||
.tx_buf = mock_tx_buf, |
||||
.tx_buf_size = ARRAY_SIZE(mock_tx_buf), |
||||
.limit = 128, |
||||
}; |
||||
|
||||
mock_pipe = modem_backend_mock_init(&mock, &mock_config); |
||||
zassert(modem_pipe_open(mock_pipe, K_SECONDS(10)) == 0, "Failed to open mock pipe"); |
||||
zassert(modem_ubx_attach(&cmd, mock_pipe) == 0, "Failed to attach pipe mock to modem CMD"); |
||||
|
||||
return NULL; |
||||
} |
||||
|
||||
static inline void restore_ubx_script(void) |
||||
{ |
||||
static const struct ubx_frame frame_restored = UBX_FRAME_ACK_INITIALIZER(0x01, 0x02); |
||||
static const struct script_runner script_runner_restored = { |
||||
.script = { |
||||
.request = { |
||||
.buf = &test_req, |
||||
.len = UBX_FRAME_SZ(frame_restored.payload_size), |
||||
}, |
||||
.match = MODEM_UBX_MATCH_DEFINE(UBX_CLASS_ID_ACK, UBX_MSG_ID_ACK, NULL), |
||||
.response = { |
||||
.buf = cmd_response, |
||||
.buf_len = sizeof(cmd_response), |
||||
}, |
||||
.timeout = K_SECONDS(1), |
||||
}, |
||||
}; |
||||
|
||||
test_script_runner = script_runner_restored; |
||||
test_req = frame_restored; |
||||
} |
||||
|
||||
static void test_before(void *f) |
||||
{ |
||||
atomic_set(&callback_called, 0); |
||||
modem_backend_mock_reset(&mock); |
||||
restore_ubx_script(); |
||||
} |
||||
|
||||
ZTEST_SUITE(modem_ubx, NULL, test_setup, test_before, NULL, NULL); |
||||
|
||||
static K_THREAD_STACK_ARRAY_DEFINE(stacks, 3, 2048); |
||||
static struct k_thread threads[3]; |
||||
|
||||
static void script_runner_handler(void *val, void *unused1, void *unused2) |
||||
{ |
||||
struct script_runner *runner = (struct script_runner *)val; |
||||
|
||||
int ret = modem_ubx_run_script(&cmd, &runner->script); |
||||
|
||||
runner->result.done = true; |
||||
runner->result.ret = ret; |
||||
} |
||||
|
||||
static inline void script_runner_start(struct script_runner *runner, uint8_t idx) |
||||
{ |
||||
k_thread_create(&threads[idx], |
||||
stacks[idx], |
||||
K_THREAD_STACK_SIZEOF(stacks[idx]), |
||||
script_runner_handler, |
||||
runner, NULL, NULL, |
||||
K_PRIO_COOP(CONFIG_NUM_COOP_PRIORITIES - 1), |
||||
0, |
||||
K_NO_WAIT); |
||||
|
||||
k_thread_start(&threads[idx]); |
||||
} |
||||
|
||||
static inline void test_thread_yield(void) |
||||
{ |
||||
/** Used instead of k_yield() since internals of modem pipe may rely on
|
||||
* multiple thread interactions which may not be served by simply |
||||
* yielding. |
||||
*/ |
||||
k_sleep(K_MSEC(1)); |
||||
} |
||||
|
||||
ZTEST(modem_ubx, test_cmd_no_rsp_is_non_blocking) |
||||
{ |
||||
/** Keep in mind this only happens if there isn't an on-going transfer
|
||||
* already. If that happens, it will wait until the other script |
||||
* finishes or this request times out. Check test-case: |
||||
* test_script_is_thread_safe for details. |
||||
*/ |
||||
uint8_t buf[256]; |
||||
int len; |
||||
|
||||
/* Setting filter class to 0 means no response is to be awaited */ |
||||
test_script_runner.script.match.filter.class = 0; |
||||
|
||||
script_runner_start(&test_script_runner, 0); |
||||
test_thread_yield(); |
||||
|
||||
len = modem_backend_mock_get(&mock, buf, sizeof(buf)); |
||||
|
||||
zassert_true(test_script_runner.result.done, "Script should be done"); |
||||
zassert_ok(test_script_runner.result.ret, "%d", test_script_runner.result.ret); |
||||
zassert_equal(UBX_FRAME_SZ(test_req.payload_size), len, "expected: %d, got: %d", |
||||
UBX_FRAME_SZ(test_req.payload_size), len); |
||||
} |
||||
|
||||
ZTEST(modem_ubx, test_cmd_rsp_retries_and_times_out) |
||||
{ |
||||
uint8_t buf[512]; |
||||
|
||||
test_script_runner.script.timeout = K_SECONDS(3); |
||||
test_script_runner.script.retry_count = 2; /* 2 Retries -> 3 Tries */ |
||||
|
||||
script_runner_start(&test_script_runner, 0); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
for (size_t i = 0 ; i < (test_script_runner.script.retry_count + 1) ; i++) { |
||||
|
||||
int len = modem_backend_mock_get(&mock, buf, sizeof(buf)); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done. " |
||||
"Iteration: %d", i); |
||||
zassert_equal(UBX_FRAME_SZ(test_req.payload_size), len, |
||||
"Payload Sent does not match. " |
||||
"Expected: %d, Received: %d, Iteration: %d", |
||||
UBX_FRAME_SZ(test_req.payload_size), len, i); |
||||
|
||||
k_sleep(K_SECONDS(1)); |
||||
} |
||||
|
||||
zassert_true(test_script_runner.result.done, "Script should be done"); |
||||
zassert_equal(test_script_runner.result.ret, -EAGAIN, "Script should time out"); |
||||
} |
||||
|
||||
ZTEST(modem_ubx, test_cmd_rsp_blocks_and_receives_rsp) |
||||
{ |
||||
static struct ubx_frame test_rsp = UBX_FRAME_ACK_INITIALIZER(0x02, 0x03); |
||||
|
||||
script_runner_start(&test_script_runner, 0); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
modem_backend_mock_put(&mock, (uint8_t *)&test_rsp, UBX_FRAME_SZ(test_rsp.payload_size)); |
||||
test_thread_yield(); |
||||
|
||||
zassert_true(test_script_runner.result.done, "Script should be done"); |
||||
zassert_ok(test_script_runner.result.ret); |
||||
zassert_equal(UBX_FRAME_SZ(test_rsp.payload_size), |
||||
test_script_runner.script.response.received_len, |
||||
"expected: %d, got: %d", |
||||
UBX_FRAME_SZ(test_rsp.payload_size), |
||||
test_script_runner.script.response.received_len); |
||||
} |
||||
|
||||
ZTEST(modem_ubx, test_script_is_thread_safe) |
||||
{ |
||||
static struct ubx_frame test_rsp = UBX_FRAME_ACK_INITIALIZER(0x02, 0x03); |
||||
struct script_runner script_runner_1 = { |
||||
.script = { |
||||
.request = { |
||||
.buf = &test_req, |
||||
.len = UBX_FRAME_SZ(test_req.payload_size), |
||||
}, |
||||
.match = MODEM_UBX_MATCH_DEFINE(UBX_CLASS_ID_ACK, UBX_MSG_ID_ACK, NULL), |
||||
.response = { |
||||
.buf = cmd_response, |
||||
.buf_len = sizeof(cmd_response), |
||||
}, |
||||
.timeout = K_SECONDS(1), |
||||
}, |
||||
}; |
||||
struct script_runner script_runner_2 = { |
||||
.script = { |
||||
.request = { |
||||
.buf = &test_req, |
||||
.len = UBX_FRAME_SZ(test_req.payload_size), |
||||
}, |
||||
.match = MODEM_UBX_MATCH_DEFINE(UBX_CLASS_ID_ACK, UBX_MSG_ID_ACK, NULL), |
||||
.response = { |
||||
.buf = cmd_response, |
||||
.buf_len = sizeof(cmd_response), |
||||
}, |
||||
.timeout = K_SECONDS(1), |
||||
}, |
||||
}; |
||||
|
||||
script_runner_start(&script_runner_1, 0); |
||||
script_runner_start(&script_runner_2, 1); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(script_runner_1.result.done); |
||||
zassert_false(script_runner_2.result.done); |
||||
|
||||
modem_backend_mock_put(&mock, (uint8_t *)&test_rsp, UBX_FRAME_SZ(test_rsp.payload_size)); |
||||
test_thread_yield(); |
||||
|
||||
zassert_true(script_runner_1.result.done); |
||||
zassert_ok(script_runner_1.result.ret); |
||||
zassert_false(script_runner_2.result.done); |
||||
|
||||
modem_backend_mock_put(&mock, (uint8_t *)&test_rsp, UBX_FRAME_SZ(test_rsp.payload_size)); |
||||
test_thread_yield(); |
||||
|
||||
zassert_true(script_runner_2.result.done); |
||||
zassert_ok(script_runner_2.result.ret); |
||||
} |
||||
|
||||
ZTEST(modem_ubx, test_rsp_filters_out_bytes_before_payload) |
||||
{ |
||||
static struct ubx_frame test_rsp = UBX_FRAME_ACK_INITIALIZER(0x02, 0x03); |
||||
|
||||
/** Create a buf that contains an "AT command" followed by the UBX frame
|
||||
* we're expecting. This should be handled by the modem_ubx. |
||||
*/ |
||||
char atcmd[] = "Here's an AT command: AT\r\nOK."; |
||||
uint8_t buf[256]; |
||||
size_t buf_len = 0; |
||||
|
||||
memcpy(buf, atcmd, sizeof(atcmd)); |
||||
buf_len += sizeof(atcmd); |
||||
memcpy(buf + buf_len, &test_rsp, UBX_FRAME_SZ(test_rsp.payload_size)); |
||||
buf_len += UBX_FRAME_SZ(test_rsp.payload_size); |
||||
|
||||
script_runner_start(&test_script_runner, 0); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
modem_backend_mock_put(&mock, (uint8_t *)buf, buf_len); |
||||
test_thread_yield(); |
||||
|
||||
zassert_true(test_script_runner.result.done, "Script should be done"); |
||||
zassert_ok(test_script_runner.result.ret); |
||||
zassert_equal(UBX_FRAME_SZ(test_rsp.payload_size), |
||||
test_script_runner.script.response.received_len, |
||||
"expected: %d, got: %d", |
||||
UBX_FRAME_SZ(test_rsp.payload_size), |
||||
test_script_runner.script.response.received_len); |
||||
zassert_mem_equal(&test_rsp, |
||||
test_script_runner.script.response.buf, |
||||
UBX_FRAME_SZ(test_rsp.payload_size)); |
||||
} |
||||
|
||||
ZTEST(modem_ubx, test_rsp_incomplete_packet_discarded) |
||||
{ |
||||
static struct ubx_frame test_rsp = UBX_FRAME_ACK_INITIALIZER(0x02, 0x03); |
||||
|
||||
uint8_t buf[256]; |
||||
size_t buf_len = 0; |
||||
|
||||
memcpy(buf, &test_rsp, UBX_FRAME_SZ(test_rsp.payload_size) - 5); |
||||
buf_len += UBX_FRAME_SZ(test_rsp.payload_size) - 5; |
||||
|
||||
script_runner_start(&test_script_runner, 0); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
modem_backend_mock_put(&mock, (uint8_t *)buf, buf_len); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
k_sleep(K_SECONDS(1)); |
||||
|
||||
zassert_true(test_script_runner.result.done, "Script should be done"); |
||||
zassert_equal(-EAGAIN, test_script_runner.result.ret); |
||||
} |
||||
|
||||
ZTEST(modem_ubx, test_rsp_discards_invalid_len) |
||||
{ |
||||
static struct ubx_frame test_rsp = UBX_FRAME_ACK_INITIALIZER(0x02, 0x03); |
||||
|
||||
/** Invalidate checksum */ |
||||
size_t frame_size = UBX_FRAME_SZ(test_rsp.payload_size); |
||||
|
||||
test_rsp.payload_size = 0xFFFF; |
||||
|
||||
script_runner_start(&test_script_runner, 0); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
modem_backend_mock_put(&mock, (uint8_t *)&test_rsp, frame_size); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
k_sleep(K_SECONDS(1)); |
||||
|
||||
zassert_true(test_script_runner.result.done, "Script should be done"); |
||||
zassert_equal(-EAGAIN, test_script_runner.result.ret); |
||||
} |
||||
|
||||
ZTEST(modem_ubx, test_rsp_discards_invalid_checksum) |
||||
{ |
||||
static struct ubx_frame test_rsp = UBX_FRAME_ACK_INITIALIZER(0x02, 0x03); |
||||
|
||||
/** Invalidate checksum */ |
||||
test_rsp.payload_and_checksum[test_rsp.payload_size] = 0xDE; |
||||
test_rsp.payload_and_checksum[test_rsp.payload_size + 1] = 0xAD; |
||||
|
||||
script_runner_start(&test_script_runner, 0); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
modem_backend_mock_put(&mock, (uint8_t *)&test_rsp, UBX_FRAME_SZ(test_rsp.payload_size)); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
k_sleep(K_SECONDS(1)); |
||||
|
||||
zassert_true(test_script_runner.result.done, "Script should be done"); |
||||
zassert_equal(-EAGAIN, test_script_runner.result.ret); |
||||
} |
||||
|
||||
ZTEST(modem_ubx, test_rsp_split_in_two_events) |
||||
{ |
||||
static struct ubx_frame test_rsp = UBX_FRAME_ACK_INITIALIZER(0x02, 0x03); |
||||
uint8_t *data_ptr = (uint8_t *)&test_rsp; |
||||
|
||||
script_runner_start(&test_script_runner, 0); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
/** The first portion of the packet. At this point the data should not be discarded,
|
||||
* understanding there's more data to come. |
||||
*/ |
||||
modem_backend_mock_put(&mock, data_ptr, UBX_FRAME_SZ(test_rsp.payload_size) - 5); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
/** The other portion of the packet. This should complete the packet reception */ |
||||
modem_backend_mock_put(&mock, &data_ptr[UBX_FRAME_SZ(test_rsp.payload_size) - 5], 5); |
||||
test_thread_yield(); |
||||
|
||||
zassert_true(test_script_runner.result.done, "Script should be done"); |
||||
zassert_ok(test_script_runner.result.ret); |
||||
zassert_equal(UBX_FRAME_SZ(test_rsp.payload_size), |
||||
test_script_runner.script.response.received_len, |
||||
"expected: %d, got: %d", |
||||
UBX_FRAME_SZ(test_rsp.payload_size), |
||||
test_script_runner.script.response.received_len); |
||||
} |
||||
|
||||
ZTEST(modem_ubx, test_rsp_filters_out_non_matches) |
||||
{ |
||||
static struct ubx_frame test_rsp_non_match = UBX_FRAME_NAK_INITIALIZER(0x02, 0x03); |
||||
static struct ubx_frame test_rsp_match = UBX_FRAME_ACK_INITIALIZER(0x02, 0x03); |
||||
|
||||
uint8_t buf[256]; |
||||
size_t buf_len = 0; |
||||
|
||||
/** We're passing a valid packet, but not what we're expecing. We
|
||||
* should not get an event out of this one. |
||||
*/ |
||||
memcpy(buf, &test_rsp_non_match, UBX_FRAME_SZ(test_rsp_non_match.payload_size)); |
||||
buf_len += UBX_FRAME_SZ(test_rsp_non_match.payload_size); |
||||
|
||||
script_runner_start(&test_script_runner, 0); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
modem_backend_mock_put(&mock, (uint8_t *)buf, buf_len); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
/** Now we're passing two valid packets, on the same event: one which
|
||||
* does not match, one which matches. We should get the latter. |
||||
*/ |
||||
memcpy(buf, &test_rsp_match, UBX_FRAME_SZ(test_rsp_match.payload_size)); |
||||
buf_len += UBX_FRAME_SZ(test_rsp_match.payload_size); |
||||
|
||||
modem_backend_mock_put(&mock, (uint8_t *)buf, buf_len); |
||||
test_thread_yield(); |
||||
|
||||
zassert_true(test_script_runner.result.done, "Script should be done"); |
||||
zassert_ok(test_script_runner.result.ret); |
||||
zassert_equal(UBX_FRAME_SZ(test_rsp_match.payload_size), |
||||
test_script_runner.script.response.received_len, |
||||
"expected: %d, got: %d", |
||||
UBX_FRAME_SZ(test_rsp_match.payload_size), |
||||
test_script_runner.script.response.received_len); |
||||
zassert_mem_equal(&test_rsp_match, |
||||
test_script_runner.script.response.buf, |
||||
UBX_FRAME_SZ(test_rsp_match.payload_size)); |
||||
} |
||||
|
||||
ZTEST(modem_ubx, test_rsp_match_with_payload) |
||||
{ |
||||
static struct ubx_frame test_rsp_non_match = UBX_FRAME_ACK_INITIALIZER(0x02, 0x03); |
||||
static struct ubx_frame test_rsp_match = UBX_FRAME_ACK_INITIALIZER(0x03, 0x04); |
||||
|
||||
test_script_runner.script.match.filter.payload.buf = test_rsp_match.payload_and_checksum; |
||||
test_script_runner.script.match.filter.payload.len = test_rsp_match.payload_size; |
||||
|
||||
uint8_t buf[256]; |
||||
size_t buf_len = 0; |
||||
|
||||
/** We're passing a valid packet, but not what we're expecing. We
|
||||
* should not get an event out of this one. |
||||
*/ |
||||
memcpy(buf, &test_rsp_non_match, UBX_FRAME_SZ(test_rsp_non_match.payload_size)); |
||||
buf_len += UBX_FRAME_SZ(test_rsp_non_match.payload_size); |
||||
|
||||
script_runner_start(&test_script_runner, 0); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
modem_backend_mock_put(&mock, (uint8_t *)buf, buf_len); |
||||
test_thread_yield(); |
||||
|
||||
zassert_false(test_script_runner.result.done, "Script should not be done"); |
||||
|
||||
/** Now we're passing two valid packets, on the same event: one which
|
||||
* does not match, one which matches. We should get the latter. |
||||
*/ |
||||
memcpy(buf, &test_rsp_match, UBX_FRAME_SZ(test_rsp_match.payload_size)); |
||||
buf_len += UBX_FRAME_SZ(test_rsp_match.payload_size); |
||||
|
||||
modem_backend_mock_put(&mock, (uint8_t *)buf, buf_len); |
||||
test_thread_yield(); |
||||
|
||||
zassert_true(test_script_runner.result.done, "Script should be done"); |
||||
zassert_ok(test_script_runner.result.ret); |
||||
zassert_equal(UBX_FRAME_SZ(test_rsp_match.payload_size), |
||||
test_script_runner.script.response.received_len, |
||||
"expected: %d, got: %d", |
||||
UBX_FRAME_SZ(test_rsp_match.payload_size), |
||||
test_script_runner.script.response.received_len); |
||||
zassert_mem_equal(&test_rsp_match, |
||||
test_script_runner.script.response.buf, |
||||
UBX_FRAME_SZ(test_rsp_match.payload_size)); |
||||
} |
||||
|
||||
ZTEST(modem_ubx, test_unsol_matches_trigger_cb) |
||||
{ |
||||
static struct ubx_frame ack_frame = UBX_FRAME_ACK_INITIALIZER(0x01, 0x02); |
||||
static struct ubx_frame nak_frame = UBX_FRAME_NAK_INITIALIZER(0x01, 0x02); |
||||
|
||||
zassert_false(atomic_test_bit(&callback_called, MODEM_UBX_UTEST_ON_ACK_RECEIVED_BIT)); |
||||
zassert_false(atomic_test_bit(&callback_called, MODEM_UBX_UTEST_ON_NAK_RECEIVED_BIT)); |
||||
|
||||
modem_backend_mock_put(&mock, |
||||
(const uint8_t *)&ack_frame, |
||||
UBX_FRAME_SZ(ack_frame.payload_size)); |
||||
test_thread_yield(); |
||||
|
||||
zassert_true(atomic_test_bit(&callback_called, MODEM_UBX_UTEST_ON_ACK_RECEIVED_BIT)); |
||||
zassert_false(atomic_test_bit(&callback_called, MODEM_UBX_UTEST_ON_NAK_RECEIVED_BIT)); |
||||
|
||||
modem_backend_mock_put(&mock, |
||||
(const uint8_t *)&nak_frame, |
||||
UBX_FRAME_SZ(nak_frame.payload_size)); |
||||
test_thread_yield(); |
||||
|
||||
zassert_true(atomic_test_bit(&callback_called, MODEM_UBX_UTEST_ON_NAK_RECEIVED_BIT)); |
||||
} |
||||
|
||||
ZTEST(modem_ubx, test_ubx_frame_encode_matches_compile_time_macro) |
||||
{ |
||||
static struct ubx_frame ack_frame = UBX_FRAME_ACK_INITIALIZER(0x01, 0x02); |
||||
|
||||
uint8_t buf[256]; |
||||
struct ubx_ack ack = { |
||||
.class = 0x01, |
||||
.id = 0x02, |
||||
}; |
||||
|
||||
int len = ubx_frame_encode(UBX_CLASS_ID_ACK, UBX_MSG_ID_ACK, |
||||
(const uint8_t *)&ack, sizeof(ack), |
||||
buf, sizeof(buf)); |
||||
zassert_equal(len, UBX_FRAME_SZ(sizeof(ack)), "Expected: %d, got: %d", |
||||
UBX_FRAME_SZ(sizeof(ack)), len); |
||||
zassert_mem_equal(buf, &ack_frame, UBX_FRAME_SZ(sizeof(ack))); |
||||
} |
||||
@ -0,0 +1,13 @@
@@ -0,0 +1,13 @@
|
||||
# Copyright (c) 2023 Trackunit Corporation |
||||
# Copyright (c) 2025 Croxel Inc. |
||||
# Copyright (c) 2025 CogniPilot Foundation |
||||
# SPDX-License-Identifier: Apache-2.0 |
||||
|
||||
tests: |
||||
modem.modem_ubx: |
||||
tags: modem_ubx |
||||
harness: ztest |
||||
platform_allow: |
||||
- native_sim |
||||
integration_platforms: |
||||
- native_sim |
||||
Loading…
Reference in new issue