diff --git a/drivers/gnss/CMakeLists.txt b/drivers/gnss/CMakeLists.txt index fea4e0bfe35..b751f308263 100644 --- a/drivers/gnss/CMakeLists.txt +++ b/drivers/gnss/CMakeLists.txt @@ -10,6 +10,6 @@ zephyr_library_sources_ifdef(CONFIG_GNSS_NMEA0183 gnss_nmea0183.c) zephyr_library_sources_ifdef(CONFIG_GNSS_NMEA0183_MATCH gnss_nmea0183_match.c) zephyr_library_sources_ifdef(CONFIG_GNSS_NMEA_GENERIC gnss_nmea_generic.c) zephyr_library_sources_ifdef(CONFIG_GNSS_QUECTEL_LCX6G gnss_quectel_lcx6g.c) -zephyr_library_sources_ifdef(CONFIG_GNSS_U_BLOX_M8 gnss_u_blox_m8.c) -zephyr_library_sources_ifdef(CONFIG_GNSS_U_BLOX_PROTOCOL gnss_u_blox_protocol/gnss_u_blox_protocol.c) +zephyr_library_sources_ifdef(CONFIG_GNSS_U_BLOX_M8 gnss_u_blox_m8.c + gnss_ubx_common.c) zephyr_library_sources_ifdef(CONFIG_GNSS_LUATOS_AIR530Z gnss_luatos_air530z.c) diff --git a/drivers/gnss/Kconfig.u_blox_m8 b/drivers/gnss/Kconfig.u_blox_m8 index 599963f4831..d4c21c12aa2 100644 --- a/drivers/gnss/Kconfig.u_blox_m8 +++ b/drivers/gnss/Kconfig.u_blox_m8 @@ -9,11 +9,8 @@ config GNSS_U_BLOX_M8 depends on GNSS_REFERENCE_FRAME_WGS84 select MODEM_MODULES select MODEM_BACKEND_UART - select MODEM_CHAT select MODEM_UBX select GNSS_PARSE - select GNSS_NMEA0183 - select GNSS_NMEA0183_MATCH select GNSS_U_BLOX_PROTOCOL select UART_USE_RUNTIME_CONFIGURE help diff --git a/drivers/gnss/gnss_u_blox_m8.c b/drivers/gnss/gnss_u_blox_m8.c index 0139443ded7..b85394d5b75 100644 --- a/drivers/gnss/gnss_u_blox_m8.c +++ b/drivers/gnss/gnss_u_blox_m8.c @@ -1,917 +1,561 @@ /* - * Copyright 2024 NXP + * Copyright (c) 2024 NXP + * Copyright (c) 2025 Croxel Inc. + * Copyright (c) 2025 CogniPilot Foundation * * SPDX-License-Identifier: Apache-2.0 */ +#define DT_DRV_COMPAT u_blox_m8 + +#include +#include #include #include -#include + #include #include -#include -#include -#include -#include - -#include "gnss_nmea0183.h" -#include "gnss_nmea0183_match.h" -#include "gnss_parse.h" -#include "gnss_u_blox_protocol/gnss_u_blox_protocol.h" +#include "gnss_ubx_common.h" #include LOG_MODULE_REGISTER(ubx_m8, CONFIG_GNSS_LOG_LEVEL); -#define DT_DRV_COMPAT u_blox_m8 - -#define UART_RECV_BUF_SZ 128 -#define UART_TRNF_BUF_SZ 128 - -#define CHAT_RECV_BUF_SZ 256 -#define CHAT_ARGV_SZ 32 - -#define UBX_RECV_BUF_SZ UBX_FRM_SZ_MAX -#define UBX_TRNS_BUF_SZ UBX_FRM_SZ_MAX -#define UBX_WORK_BUF_SZ UBX_FRM_SZ_MAX - -#define UBX_FRM_BUF_SZ UBX_FRM_SZ_MAX - -#define MODEM_UBX_SCRIPT_TIMEOUT_MS 1000 -#define UBX_M8_SCRIPT_RETRY_DEFAULT 10 - -#define UBX_M8_GNSS_SYS_CNT 8 -#define UBX_M8_GNSS_SUPP_SYS_CNT 6 - struct ubx_m8_config { - const struct device *uart; - const uint32_t uart_baudrate; + const struct device *bus; + uint16_t fix_rate_ms; + struct { + uint32_t initial; + uint32_t desired; + } baudrate; }; struct ubx_m8_data { - struct gnss_nmea0183_match_data match_data; + struct gnss_ubx_common_data common_data; + struct { + struct modem_pipe *pipe; + struct modem_backend_uart uart_backend; + uint8_t receive_buf[1024]; + uint8_t transmit_buf[256]; + } backend; + struct { + struct modem_ubx inst; + uint8_t receive_buf[1024]; + } ubx; + struct { + struct modem_ubx_script inst; + uint8_t response_buf[512]; + uint8_t request_buf[256]; + struct k_mutex lock; + } script; #if CONFIG_GNSS_SATELLITES struct gnss_satellite satellites[CONFIG_GNSS_U_BLOX_M8_SATELLITES_COUNT]; #endif - - /* UART backend */ - struct modem_pipe *uart_pipe; - struct modem_backend_uart uart_backend; - uint8_t uart_backend_receive_buf[UART_RECV_BUF_SZ]; - uint8_t uart_backend_transmit_buf[UART_TRNF_BUF_SZ]; - - /* Modem chat */ - struct modem_chat chat; - uint8_t chat_receive_buf[CHAT_RECV_BUF_SZ]; - uint8_t *chat_argv[CHAT_ARGV_SZ]; - - /* Modem ubx */ - struct modem_ubx ubx; - uint8_t ubx_receive_buf[UBX_RECV_BUF_SZ]; - uint8_t ubx_work_buf[UBX_WORK_BUF_SZ]; - - /* Modem ubx script */ - struct modem_ubx_script script; - uint8_t request_buf[UBX_FRM_BUF_SZ]; - uint8_t response_buf[UBX_FRM_BUF_SZ]; - uint8_t match_buf[UBX_FRM_BUF_SZ]; - - struct k_spinlock lock; }; -MODEM_CHAT_MATCHES_DEFINE(unsol_matches, - MODEM_CHAT_MATCH_WILDCARD("$??GGA,", ",*", gnss_nmea0183_match_gga_callback), - MODEM_CHAT_MATCH_WILDCARD("$??RMC,", ",*", gnss_nmea0183_match_rmc_callback), +UBX_FRAME_DEFINE(disable_gga, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NMEA_STD, UBX_MSG_ID_NMEA_STD_GGA, 0)); +UBX_FRAME_DEFINE(disable_rmc, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NMEA_STD, UBX_MSG_ID_NMEA_STD_RMC, 0)); +UBX_FRAME_DEFINE(disable_gsv, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NMEA_STD, UBX_MSG_ID_NMEA_STD_GSV, 0)); +UBX_FRAME_DEFINE(disable_dtm, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NMEA_STD, UBX_MSG_ID_NMEA_STD_DTM, 0)); +UBX_FRAME_DEFINE(disable_gbs, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NMEA_STD, UBX_MSG_ID_NMEA_STD_GBS, 0)); +UBX_FRAME_DEFINE(disable_gll, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NMEA_STD, UBX_MSG_ID_NMEA_STD_GLL, 0)); +UBX_FRAME_DEFINE(disable_gns, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NMEA_STD, UBX_MSG_ID_NMEA_STD_GNS, 0)); +UBX_FRAME_DEFINE(disable_grs, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NMEA_STD, UBX_MSG_ID_NMEA_STD_GRS, 0)); +UBX_FRAME_DEFINE(disable_gsa, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NMEA_STD, UBX_MSG_ID_NMEA_STD_GSA, 0)); +UBX_FRAME_DEFINE(disable_gst, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NMEA_STD, UBX_MSG_ID_NMEA_STD_GST, 0)); +UBX_FRAME_DEFINE(disable_vlw, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NMEA_STD, UBX_MSG_ID_NMEA_STD_VLW, 0)); +UBX_FRAME_DEFINE(disable_vtg, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NMEA_STD, UBX_MSG_ID_NMEA_STD_VTG, 0)); +UBX_FRAME_DEFINE(disable_zda, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NMEA_STD, UBX_MSG_ID_NMEA_STD_ZDA, 0)); +UBX_FRAME_DEFINE(enable_nav, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NAV, UBX_MSG_ID_NAV_PVT, 1)); #if CONFIG_GNSS_SATELLITES - MODEM_CHAT_MATCH_WILDCARD("$??GSV,", ",*", gnss_nmea0183_match_gsv_callback), +UBX_FRAME_DEFINE(enable_sat, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NAV, UBX_MSG_ID_NAV_SAT, 1)); +#endif + +UBX_FRAME_ARRAY_DEFINE(u_blox_m8_init_seq, + &disable_gga, &disable_rmc, &disable_gsv, &disable_dtm, &disable_gbs, + &disable_gll, &disable_gns, &disable_grs, &disable_gsa, &disable_gst, + &disable_vlw, &disable_vtg, &disable_zda, &enable_nav, +#if CONFIG_GNSS_SATELLITES + &enable_sat, #endif ); -static int ubx_m8_resume(const struct device *dev) +MODEM_UBX_MATCH_ARRAY_DEFINE(u_blox_m8_unsol_messages, + MODEM_UBX_MATCH_DEFINE(UBX_CLASS_ID_NAV, UBX_MSG_ID_NAV_PVT, + gnss_ubx_common_pvt_callback), +#if CONFIG_GNSS_SATELLITES + MODEM_UBX_MATCH_DEFINE(UBX_CLASS_ID_NAV, UBX_MSG_ID_NAV_SAT, + gnss_ubx_common_satellite_callback), +#endif +); + +static int ubx_m8_msg_get(const struct device *dev, const struct ubx_frame *req, + size_t len, void *rsp, size_t min_rsp_size) { struct ubx_m8_data *data = dev->data; - int ret; + struct ubx_frame *rsp_frame = (struct ubx_frame *)data->script.inst.response.buf; + int err; - ret = modem_pipe_open(data->uart_pipe, K_SECONDS(10)); - if (ret < 0) { - return ret; + err = k_mutex_lock(&data->script.lock, K_SECONDS(3)); + if (err != 0) { + LOG_ERR("Failed to take script lock: %d", err); + return err; } - ret = modem_chat_attach(&data->chat, data->uart_pipe); - if (ret < 0) { - (void)modem_pipe_close(data->uart_pipe, K_SECONDS(10)); - return ret; + data->script.inst.timeout = K_SECONDS(3); + data->script.inst.retry_count = 2; + data->script.inst.match.filter.class = req->class; + data->script.inst.match.filter.id = req->id; + data->script.inst.request.buf = req; + data->script.inst.request.len = len; + + err = modem_ubx_run_script(&data->ubx.inst, &data->script.inst); + if (err != 0 || (data->script.inst.response.buf_len < UBX_FRAME_SZ(min_rsp_size))) { + return -EIO; } - return ret; -} + memcpy(rsp, rsp_frame->payload_and_checksum, min_rsp_size); -static int ubx_m8_turn_off(const struct device *dev) -{ - struct ubx_m8_data *data = dev->data; + (void)k_mutex_unlock(&data->script.lock); - return modem_pipe_close(data->uart_pipe, K_SECONDS(10)); + return 0; } -static int ubx_m8_init_nmea0183_match(const struct device *dev) +static int ubx_m8_msg_send(const struct device *dev, const struct ubx_frame *req, + size_t len, bool wait_for_ack) { struct ubx_m8_data *data = dev->data; + int err; - const struct gnss_nmea0183_match_config match_config = { - .gnss = dev, -#if CONFIG_GNSS_SATELLITES - .satellites = data->satellites, - .satellites_size = ARRAY_SIZE(data->satellites), -#endif - }; + err = k_mutex_lock(&data->script.lock, K_SECONDS(3)); + if (err != 0) { + LOG_ERR("Failed to take script lock: %d", err); + return err; + } - return gnss_nmea0183_match_init(&data->match_data, &match_config); -} + data->script.inst.timeout = K_SECONDS(3); + 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.len = len; -static void ubx_m8_init_pipe(const struct device *dev) -{ - const struct ubx_m8_config *cfg = dev->config; - struct ubx_m8_data *data = dev->data; + err = modem_ubx_run_script(&data->ubx.inst, &data->script.inst); - const struct modem_backend_uart_config uart_backend_config = { - .uart = cfg->uart, - .receive_buf = data->uart_backend_receive_buf, - .receive_buf_size = sizeof(data->uart_backend_receive_buf), - .transmit_buf = data->uart_backend_transmit_buf, - .transmit_buf_size = ARRAY_SIZE(data->uart_backend_transmit_buf), - }; + (void)k_mutex_unlock(&data->script.lock); - data->uart_pipe = modem_backend_uart_init(&data->uart_backend, &uart_backend_config); + return err; } -static uint8_t ubx_m8_char_delimiter[] = {'\r', '\n'}; - -static int ubx_m8_init_chat(const struct device *dev) +static int ubx_m8_msg_payload_send(const struct device *dev, uint8_t class, uint8_t id, + const uint8_t *payload, size_t payload_size, bool wait_for_ack) { struct ubx_m8_data *data = dev->data; + struct ubx_frame *frame = (struct ubx_frame *)data->script.request_buf; + int err; - const struct modem_chat_config chat_config = { - .user_data = data, - .receive_buf = data->chat_receive_buf, - .receive_buf_size = sizeof(data->chat_receive_buf), - .delimiter = ubx_m8_char_delimiter, - .delimiter_size = ARRAY_SIZE(ubx_m8_char_delimiter), - .filter = NULL, - .filter_size = 0, - .argv = data->chat_argv, - .argv_size = ARRAY_SIZE(data->chat_argv), - .unsol_matches = unsol_matches, - .unsol_matches_size = ARRAY_SIZE(unsol_matches), - }; + err = k_mutex_lock(&data->script.lock, K_SECONDS(3)); + if (err != 0) { + LOG_ERR("Failed to take script lock: %d", err); + return err; + } + + err = ubx_frame_encode(class, id, payload, payload_size, + (uint8_t *)frame, sizeof(data->script.request_buf)); + if (err > 0) { + err = ubx_m8_msg_send(dev, frame, err, wait_for_ack); + } + + (void)k_mutex_unlock(&data->script.lock); - return modem_chat_init(&data->chat, &chat_config); + return err; } -static int ubx_m8_init_ubx(const struct device *dev) +static inline int init_modem(const struct device *dev) { + int err; struct ubx_m8_data *data = dev->data; + const struct ubx_m8_config *cfg = dev->config; const struct modem_ubx_config ubx_config = { - .user_data = data, - .receive_buf = data->ubx_receive_buf, - .receive_buf_size = sizeof(data->ubx_receive_buf), - .work_buf = data->ubx_work_buf, - .work_buf_size = sizeof(data->ubx_work_buf), + .user_data = (void *)&data->common_data, + .receive_buf = data->ubx.receive_buf, + .receive_buf_size = sizeof(data->ubx.receive_buf), + .unsol_matches = { + .array = u_blox_m8_unsol_messages, + .size = ARRAY_SIZE(u_blox_m8_unsol_messages), + }, }; - return modem_ubx_init(&data->ubx, &ubx_config); -} + (void)modem_ubx_init(&data->ubx.inst, &ubx_config); -/** - * @brief Changes modem module (chat or ubx) attached to the uart pipe. - * @param dev Dev instance - * @param change_from_to 0 for changing from "chat" to "ubx", 1 for changing from "ubx" to "chat" - * @returns 0 if successful - * @returns negative errno code if failure - */ -static int ubx_m8_modem_module_change(const struct device *dev, bool change_from_to) -{ - struct ubx_m8_data *data = dev->data; - int ret; - - if (change_from_to == 0) { - modem_chat_release(&data->chat); - ret = modem_ubx_attach(&data->ubx, data->uart_pipe); - } else { /* change_from_to == 1 */ - modem_ubx_release(&data->ubx); - ret = modem_chat_attach(&data->chat, data->uart_pipe); - } + const struct modem_backend_uart_config uart_backend_config = { + .uart = cfg->bus, + .receive_buf = data->backend.receive_buf, + .receive_buf_size = sizeof(data->backend.receive_buf), + .transmit_buf = data->backend.transmit_buf, + .transmit_buf_size = sizeof(data->backend.transmit_buf), + }; - if (ret < 0) { - (void)modem_pipe_close(data->uart_pipe, K_SECONDS(10)); + data->backend.pipe = modem_backend_uart_init(&data->backend.uart_backend, + &uart_backend_config); + err = modem_pipe_open(data->backend.pipe, K_SECONDS(1)); + if (err != 0) { + LOG_ERR("Failed to open Modem pipe: %d", err); + return err; } - return ret; -} - -static int ubx_m8_modem_ubx_run_script(const struct device *dev, - struct modem_ubx_script *modem_ubx_script_tx) -{ - struct ubx_m8_data *data = dev->data; - int ret; - - ret = ubx_m8_modem_module_change(dev, 0); - if (ret < 0) { - goto reset_modem_module; + err = modem_ubx_attach(&data->ubx.inst, data->backend.pipe); + if (err != 0) { + LOG_ERR("Failed to attach UBX inst to modem pipe: %d", err); + return err; } - ret = modem_ubx_run_script(&data->ubx, modem_ubx_script_tx); + (void)k_mutex_init(&data->script.lock); -reset_modem_module: - ret |= ubx_m8_modem_module_change(dev, 1); + data->script.inst.response.buf = data->script.response_buf; + data->script.inst.response.buf_len = sizeof(data->script.response_buf); - return ret; + return err; } -static void ubx_m8_modem_ubx_script_fill(const struct device *dev) +static inline int reattach_modem(const struct device *dev) { + int err; struct ubx_m8_data *data = dev->data; - data->script.request = (struct ubx_frame *)data->request_buf; - data->script.response = (struct ubx_frame *)data->response_buf; - data->script.match = (struct ubx_frame *)data->match_buf; - data->script.retry_count = UBX_M8_SCRIPT_RETRY_DEFAULT; - data->script.timeout = K_MSEC(MODEM_UBX_SCRIPT_TIMEOUT_MS); -} - -static int ubx_m8_modem_ubx_script_init(const struct device *dev, void *payload, uint16_t payld_sz, - enum ubx_msg_class msg_cls, enum ubx_config_message msg_id) -{ - int ret; - struct ubx_m8_data *data = dev->data; - struct ubx_cfg_ack_payload match_payload = { - .message_class = msg_cls, - .message_id = msg_id, - }; + (void)modem_ubx_release(&data->ubx.inst); + (void)modem_pipe_close(data->backend.pipe, K_SECONDS(1)); - ubx_m8_modem_ubx_script_fill(dev); - - ret = ubx_create_and_validate_frame(data->match_buf, sizeof(data->match_buf), UBX_CLASS_ACK, - UBX_ACK_ACK, &match_payload, UBX_CFG_ACK_PAYLOAD_SZ); - if (ret < 0) { - return ret; + err = modem_pipe_open(data->backend.pipe, K_SECONDS(1)); + if (err != 0) { + LOG_ERR("Failed to re-open modem pipe: %d", err); + return err; } - ret = ubx_create_and_validate_frame(data->request_buf, sizeof(data->request_buf), msg_cls, - msg_id, payload, payld_sz); + err = modem_ubx_attach(&data->ubx.inst, data->backend.pipe); + if (err != 0) { + LOG_ERR("Failed to re-attach modem pipe to UBX inst: %d", err); + } - return ret; + return 0; } -static int ubx_m8_ubx_cfg_rate(const struct device *dev) +static inline int configure_baudrate(const struct device *dev) { - int ret; - k_spinlock_key_t key; - struct ubx_m8_data *data = dev->data; - struct ubx_cfg_rate_payload payload; - - key = k_spin_lock(&data->lock); - - ubx_cfg_rate_payload_default(&payload); + int err = 0; + const struct ubx_m8_config *cfg = dev->config; + struct uart_config uart_cfg; - ret = ubx_m8_modem_ubx_script_init(dev, &payload, UBX_CFG_RATE_PAYLOAD_SZ, UBX_CLASS_CFG, - UBX_CFG_RATE); - if (ret < 0) { - goto unlock; + err = uart_config_get(cfg->bus, &uart_cfg); + if (err < 0) { + LOG_ERR("Failed to get UART config: %d", err); + return err; } - ret = ubx_m8_modem_ubx_run_script(dev, &(data->script)); - -unlock: - k_spin_unlock(&data->lock, key); + uint32_t desired_baudrate = cfg->baudrate.desired; + uint32_t initial_baudrate = cfg->baudrate.initial; - return ret; -} - -static int ubx_m8_ubx_cfg_prt_set(const struct device *dev, uint32_t target_baudrate, - uint8_t retry) -{ - int ret; - k_spinlock_key_t key; - struct ubx_m8_data *data = dev->data; - struct ubx_cfg_prt_set_payload payload; + uart_cfg.baudrate = initial_baudrate; + err = uart_configure(cfg->bus, &uart_cfg); + if (err < 0) { + LOG_ERR("Failed to configure UART: %d", err); + } - key = k_spin_lock(&data->lock); + struct ubx_cfg_prt port_config = { + .port_id = UBX_CFG_PORT_ID_UART, + .baudrate = desired_baudrate, + .mode = UBX_CFG_PRT_MODE_CHAR_LEN(UBX_CFG_PRT_PORT_MODE_CHAR_LEN_8) | + UBX_CFG_PRT_MODE_PARITY(UBX_CFG_PRT_PORT_MODE_PARITY_NONE) | + UBX_CFG_PRT_MODE_STOP_BITS(UBX_CFG_PRT_PORT_MODE_STOP_BITS_1), + .in_proto_mask = UBX_CFG_PRT_PROTO_MASK_UBX, + .out_proto_mask = UBX_CFG_PRT_PROTO_MASK_UBX, + }; + (void)ubx_m8_msg_payload_send(dev, UBX_CLASS_ID_CFG, UBX_MSG_ID_CFG_PRT, + (const uint8_t *)&port_config, + sizeof(port_config), true); - ubx_cfg_prt_set_payload_default(&payload); - payload.baudrate = target_baudrate; - payload.out_proto_mask = UBX_CFG_PRT_OUT_PROTO_NMEA; + uart_cfg.baudrate = desired_baudrate; - ret = ubx_m8_modem_ubx_script_init(dev, &payload, UBX_CFG_PRT_SET_PAYLOAD_SZ, - UBX_CLASS_CFG, UBX_CFG_PRT); - if (ret < 0) { - goto unlock; + err = uart_configure(cfg->bus, &uart_cfg); + if (err < 0) { + LOG_ERR("Failed to configure UART: %d", err); } - data->script.retry_count = retry; - /* Returns failure if "target_baudrate" is different than device's currently set baudrate, - * because the device will change its baudrate and respond with UBX-ACK with new baudrate, - * which we will miss. Hence, we need to change uart's baudrate after sending the frame - * (in order to receive response as well), which we are not doing right now. - */ - ret = ubx_m8_modem_ubx_run_script(dev, &(data->script)); - -unlock: - k_spin_unlock(&data->lock, key); - - return ret; + return err; } -static int ubx_m8_ubx_cfg_rst(const struct device *dev, uint8_t reset_mode) +static inline int init_match(const struct device *dev) { - int ret; - k_spinlock_key_t key; struct ubx_m8_data *data = dev->data; - struct ubx_cfg_rst_payload payload; - - key = k_spin_lock(&data->lock); - - ubx_cfg_rst_payload_default(&payload); - - payload.nav_bbr_mask = UBX_CFG_RST_NAV_BBR_MASK_HOT_START; - payload.reset_mode = reset_mode; - - ret = ubx_m8_modem_ubx_script_init(dev, &payload, UBX_CFG_RST_PAYLOAD_SZ, UBX_CLASS_CFG, - UBX_CFG_RST); - if (ret < 0) { - goto unlock; - } - - data->script.match = NULL; - ret = ubx_m8_modem_ubx_run_script(dev, &(data->script)); - if (ret < 0) { - goto unlock; - } - - if (reset_mode == UBX_CFG_RST_RESET_MODE_CONTROLLED_GNSS_STOP) { - k_sleep(K_MSEC(UBX_CFG_RST_WAIT_MS)); - } + struct gnss_ubx_common_config match_config = { + .gnss = dev, +#if CONFIG_GNSS_SATELLITES + .satellites = { + .buf = data->satellites, + .size = ARRAY_SIZE(data->satellites), + }, +#endif + }; -unlock: - k_spin_unlock(&data->lock, key); + gnss_ubx_common_init(&data->common_data, &match_config); - return ret; + return 0; } -static int ubx_m8_set_uart_baudrate(const struct device *dev, uint32_t baudrate) +static int ubx_m8_init(const struct device *dev) { - int ret; - k_spinlock_key_t key; - struct ubx_m8_data *data = dev->data; - const struct ubx_m8_config *config = dev->config; - struct uart_config uart_cfg; + int err = 0; + const struct ubx_m8_config *cfg = dev->config; - key = k_spin_lock(&data->lock); + (void)init_match(dev); - ret = ubx_m8_turn_off(dev); - if (ret < 0) { - goto reset_and_unlock; + err = init_modem(dev); + if (err < 0) { + LOG_ERR("Failed to initialize modem: %d", err); } - ret = uart_config_get(config->uart, &uart_cfg); - if (ret < 0) { - goto reset_and_unlock; + err = configure_baudrate(dev); + if (err < 0) { + LOG_ERR("Failed to configure baud-rate: %d", err); + return err; } - uart_cfg.baudrate = baudrate; - - ret = uart_configure(config->uart, &uart_cfg); - -reset_and_unlock: - ret |= ubx_m8_resume(dev); - k_spin_unlock(&data->lock, key); + err = reattach_modem(dev); + if (err < 0) { + LOG_ERR("Failed to re-attach modem: %d", err); + return err; + } - return ret; -} + const static struct ubx_frame version_get = UBX_FRAME_GET_INITIALIZER( + UBX_CLASS_ID_MON, + UBX_MSG_ID_MON_VER); + struct ubx_mon_ver ver; -static bool ubx_m8_validate_baudrate(const struct device *dev, uint32_t baudrate) -{ - for (int i = 0; i < UBX_BAUDRATE_COUNT; ++i) { - if (baudrate == ubx_baudrate[i]) { - return true; - } + err = ubx_m8_msg_get(dev, &version_get, + UBX_FRAME_SZ(version_get.payload_size), + (void *)&ver, sizeof(ver)); + if (err != 0) { + LOG_ERR("Failed to get Modem Version info: %d", err); + return err; } + LOG_INF("SW Version: %s, HW Version: %s", ver.sw_ver, ver.hw_ver); - return false; -} + const static struct ubx_frame stop_gnss = UBX_FRAME_CFG_RST_INITIALIZER( + UBX_CFG_RST_HOT_START, + UBX_CFG_RST_MODE_GNSS_STOP); -/* This function will return failure if "target_baudrate" != device's current baudrate. - * Refer the function description of ubx_m8_ubx_cfg_prt_set for a detailed explanation. - */ -static int ubx_m8_configure_gnss_device_baudrate_prerequisite(const struct device *dev) -{ - /* Retry = 1 should be enough, but setting 2 just to be safe. */ - int ret, retry = 2; - const struct ubx_m8_config *config = dev->config; - uint32_t target_baudrate = config->uart_baudrate; - - ret = ubx_m8_validate_baudrate(dev, target_baudrate); - if (ret < 0) { - return ret; + err = ubx_m8_msg_send(dev, &stop_gnss, UBX_FRAME_SZ(stop_gnss.payload_size), false); + if (err != 0) { + LOG_ERR("Failed to stop GNSS module: %d", err); + return err; } + k_sleep(K_MSEC(1000)); - /* Try communication with device with all possible baudrates, because initially we don't - * know the currently set baudrate of the device. We will match the baudrate in one of the - * following attempts and the device will thus change its baudrate to "target_baudrate". - */ - for (int i = 0; i < UBX_BAUDRATE_COUNT; ++i) { - /* Set baudrate of UART pipe as ubx_baudrate[i]. */ - ret = ubx_m8_set_uart_baudrate(dev, ubx_baudrate[i]); - if (ret < 0) { - return ret; - } + err = gnss_set_fix_rate(dev, cfg->fix_rate_ms); + if (err != 0) { + LOG_ERR("Failed to set fix-rate: %d", err); + return err; + } - /* Try setting baudrate of device as target_baudrate. */ - ret = ubx_m8_ubx_cfg_prt_set(dev, target_baudrate, retry); - if (ret == 0) { - break; + for (size_t i = 0 ; i < ARRAY_SIZE(u_blox_m8_init_seq) ; i++) { + err = ubx_m8_msg_send(dev, + u_blox_m8_init_seq[i], + UBX_FRAME_SZ(u_blox_m8_init_seq[i]->payload_size), + true); + if (err < 0) { + LOG_ERR("Failed to send init sequence - idx: %d, result: %d", i, err); + return err; } } - /* Reset baudrate of UART pipe as target_baudrate. */ - ret = ubx_m8_set_uart_baudrate(dev, target_baudrate); - if (ret < 0) { - return ret; + const static struct ubx_frame start_gnss = UBX_FRAME_CFG_RST_INITIALIZER( + UBX_CFG_RST_HOT_START, + UBX_CFG_RST_MODE_GNSS_START); + + err = ubx_m8_msg_send(dev, &start_gnss, UBX_FRAME_SZ(start_gnss.payload_size), false); + if (err != 0) { + LOG_ERR("Failed to start GNSS module: %d", err); + return err; } return 0; } -static int ubx_m8_configure_gnss_device_baudrate(const struct device *dev) +static int ubx_m8_set_fix_rate(const struct device *dev, uint32_t fix_interval_ms) { - int ret; - const struct ubx_m8_config *config = dev->config; - uint32_t target_baudrate = config->uart_baudrate; - - ret = ubx_m8_validate_baudrate(dev, target_baudrate); - if (ret < 0) { - return ret; + if (fix_interval_ms < 50 || fix_interval_ms > 65535) { + return -EINVAL; } - ret = ubx_m8_ubx_cfg_prt_set(dev, target_baudrate, UBX_M8_SCRIPT_RETRY_DEFAULT); - if (ret < 0) { - return ret; - } + struct ubx_cfg_rate rate = { + .meas_rate_ms = (uint16_t)fix_interval_ms, + .nav_rate = 1, + .time_ref = UBX_CFG_RATE_TIME_REF_GPS, + }; - return 0; + return ubx_m8_msg_payload_send(dev, UBX_CLASS_ID_CFG, UBX_MSG_ID_CFG_RATE, + (const uint8_t *)&rate, sizeof(rate), + true); } -static int ubx_m8_configure_messages(const struct device *dev) +static int ubx_m8_get_fix_rate(const struct device *dev, uint32_t *fix_interval_ms) { - int ret = 0; - k_spinlock_key_t key; - struct ubx_m8_data *data = dev->data; - struct ubx_cfg_msg_payload payload; - - key = k_spin_lock(&data->lock); - - ubx_cfg_msg_payload_default(&payload); - - /* Enabling GGA, RMC and GSV messages. */ - payload.rate = 1; - uint8_t message_enable[] = {UBX_NMEA_GGA, UBX_NMEA_RMC, UBX_NMEA_GSV}; - - for (int i = 0; i < sizeof(message_enable); ++i) { - payload.message_id = message_enable[i]; - ret = ubx_m8_modem_ubx_script_init(dev, &payload, UBX_CFG_MSG_PAYLOAD_SZ, - UBX_CLASS_CFG, UBX_CFG_MSG); - if (ret < 0) { - goto unlock; - } + struct ubx_cfg_rate rate; + int err; - ret = ubx_m8_modem_ubx_run_script(dev, &(data->script)); - if (ret < 0) { - goto unlock; - } - } - - /* Disabling DTM, GBS, GLL, GNS, GRS, GSA, GST, VLW, VTG and ZDA messages. */ - payload.rate = 0; - uint8_t message_disable[] = {UBX_NMEA_DTM, UBX_NMEA_GBS, UBX_NMEA_GLL, UBX_NMEA_GNS, - UBX_NMEA_GRS, UBX_NMEA_GSA, UBX_NMEA_GST, UBX_NMEA_VLW, - UBX_NMEA_VTG, UBX_NMEA_ZDA}; - - for (int i = 0; i < sizeof(message_disable); ++i) { - payload.message_id = message_disable[i]; - ret = ubx_m8_modem_ubx_script_init(dev, &payload, UBX_CFG_MSG_PAYLOAD_SZ, - UBX_CLASS_CFG, UBX_CFG_MSG); - if (ret < 0) { - goto unlock; - } + const static struct ubx_frame get_fix_rate = UBX_FRAME_GET_INITIALIZER(UBX_CLASS_ID_CFG, + UBX_MSG_ID_CFG_RATE); - ret = ubx_m8_modem_ubx_run_script(dev, &(data->script)); - if (ret < 0) { - goto unlock; - } + err = ubx_m8_msg_get(dev, &get_fix_rate, + UBX_FRAME_SZ(get_fix_rate.payload_size), + (void *)&rate, sizeof(rate)); + if (err == 0) { + *fix_interval_ms = rate.meas_rate_ms; } -unlock: - k_spin_unlock(&data->lock, key); - - return ret; + return err; } -static int ubx_m8_navigation_mode_to_ubx_dynamic_model(const struct device *dev, - enum gnss_navigation_mode mode) +static int ubx_m8_set_navigation_mode(const struct device *dev, enum gnss_navigation_mode mode) { + enum ubx_dyn_model nav_model; + switch (mode) { case GNSS_NAVIGATION_MODE_ZERO_DYNAMICS: - return UBX_DYN_MODEL_STATIONARY; + nav_model = UBX_DYN_MODEL_STATIONARY; + break; case GNSS_NAVIGATION_MODE_LOW_DYNAMICS: - return UBX_DYN_MODEL_PORTABLE; + nav_model = UBX_DYN_MODEL_PEDESTRIAN; + break; case GNSS_NAVIGATION_MODE_BALANCED_DYNAMICS: - return UBX_DYN_MODEL_AIRBORNE1G; + nav_model = UBX_DYN_MODEL_PORTABLE; + break; case GNSS_NAVIGATION_MODE_HIGH_DYNAMICS: - return UBX_DYN_MODEL_AIRBORNE4G; - default: - return -EINVAL; - } -} - -static int ubx_m8_ubx_dynamic_model_to_navigation_mode(const struct device *dev, - enum ubx_dynamic_model dynamic_model) -{ - switch (dynamic_model) { - case UBX_DYN_MODEL_PORTABLE: - return GNSS_NAVIGATION_MODE_LOW_DYNAMICS; - case UBX_DYN_MODEL_STATIONARY: - return GNSS_NAVIGATION_MODE_ZERO_DYNAMICS; - case UBX_DYN_MODEL_PEDESTRIAN: - return GNSS_NAVIGATION_MODE_LOW_DYNAMICS; - case UBX_DYN_MODEL_AUTOMOTIVE: - return GNSS_NAVIGATION_MODE_LOW_DYNAMICS; - case UBX_DYN_MODEL_SEA: - return GNSS_NAVIGATION_MODE_BALANCED_DYNAMICS; - case UBX_DYN_MODEL_AIRBORNE1G: - return GNSS_NAVIGATION_MODE_BALANCED_DYNAMICS; - case UBX_DYN_MODEL_AIRBORNE2G: - return GNSS_NAVIGATION_MODE_BALANCED_DYNAMICS; - case UBX_DYN_MODEL_AIRBORNE4G: - return GNSS_NAVIGATION_MODE_HIGH_DYNAMICS; - case UBX_DYN_MODEL_WRIST: - return GNSS_NAVIGATION_MODE_BALANCED_DYNAMICS; - case UBX_DYN_MODEL_BIKE: - return GNSS_NAVIGATION_MODE_HIGH_DYNAMICS; + nav_model = UBX_DYN_MODEL_AIRBORNE_2G; + break; default: return -EINVAL; } -} - -static int ubx_m8_set_navigation_mode(const struct device *dev, enum gnss_navigation_mode mode) -{ - int ret; - k_spinlock_key_t key; - struct ubx_m8_data *data = dev->data; - struct ubx_cfg_nav5_payload payload; - - key = k_spin_lock(&data->lock); - - ubx_cfg_nav5_payload_default(&payload); - - ret = ubx_m8_navigation_mode_to_ubx_dynamic_model(dev, mode); - if (ret < 0) { - goto unlock; - } - - payload.dyn_model = ret; - - ret = ubx_m8_modem_ubx_script_init(dev, &payload, UBX_CFG_NAV5_PAYLOAD_SZ, UBX_CLASS_CFG, - UBX_CFG_NAV5); - if (ret < 0) { - goto unlock; - } - - ret = ubx_m8_modem_ubx_run_script(dev, &(data->script)); - if (ret < 0) { - goto unlock; - } - - k_sleep(K_MSEC(UBX_CFG_NAV5_WAIT_MS)); -unlock: - k_spin_unlock(&data->lock, key); + /** The zero'd elements won't be applied as long as their apply bit is not set. */ + struct ubx_cfg_nav5 nav_mode = { + .apply = (UBX_CFG_NAV5_APPLY_DYN | UBX_CFG_NAV5_APPLY_FIX_MODE), + .dyn_model = nav_model, + .fix_mode = UBX_FIX_MODE_AUTO, + }; - return ret; + return ubx_m8_msg_payload_send(dev, UBX_CLASS_ID_CFG, UBX_MSG_ID_CFG_NAV5, + (const uint8_t *)&nav_mode, sizeof(nav_mode), + true); } static int ubx_m8_get_navigation_mode(const struct device *dev, enum gnss_navigation_mode *mode) { - int ret; - k_spinlock_key_t key; - struct ubx_m8_data *data = dev->data; - enum ubx_dynamic_model dynamic_model; - - key = k_spin_lock(&data->lock); - - ret = ubx_m8_modem_ubx_script_init(dev, NULL, UBX_FRM_GET_PAYLOAD_SZ, UBX_CLASS_CFG, - UBX_CFG_NAV5); - if (ret < 0) { - goto unlock; - } + struct ubx_cfg_nav5 nav_mode; + int err; - ret = ubx_m8_modem_ubx_run_script(dev, &(data->script)); - if (ret < 0) { - goto unlock; - } - - struct ubx_frame *response = data->script.response; - - dynamic_model = ((struct ubx_cfg_nav5_payload *)response->payload_and_checksum)->dyn_model; - ret = ubx_m8_ubx_dynamic_model_to_navigation_mode(dev, dynamic_model); - if (ret < 0) { - goto unlock; - } + const static struct ubx_frame get_nav_mode = UBX_FRAME_GET_INITIALIZER(UBX_CLASS_ID_CFG, + UBX_MSG_ID_CFG_NAV5); - *mode = ret; + err = ubx_m8_msg_get(dev, &get_nav_mode, + UBX_FRAME_SZ(get_nav_mode.payload_size), + &nav_mode, sizeof(nav_mode)); -unlock: - k_spin_unlock(&data->lock, key); - - return ret; -} - -static int ubx_m8_get_supported_systems(const struct device *dev, gnss_systems_t *systems) -{ - *systems = (GNSS_SYSTEM_GPS | GNSS_SYSTEM_GLONASS | GNSS_SYSTEM_GALILEO | - GNSS_SYSTEM_BEIDOU | GNSS_SYSTEM_SBAS | GNSS_SYSTEM_QZSS); - - return 0; -} - -static int ubx_m8_ubx_gnss_id_to_gnss_system(const struct device *dev, enum ubx_gnss_id gnss_id) -{ - switch (gnss_id) { - case UBX_GNSS_ID_GPS: - return GNSS_SYSTEM_GPS; - case UBX_GNSS_ID_SBAS: - return GNSS_SYSTEM_SBAS; - case UBX_GNSS_ID_GALILEO: - return GNSS_SYSTEM_GALILEO; - case UBX_GNSS_ID_BEIDOU: - return GNSS_SYSTEM_BEIDOU; - case UBX_GNSS_ID_QZSS: - return GNSS_SYSTEM_QZSS; - case UBX_GNSS_ID_GLONASS: - return GNSS_SYSTEM_GLONASS; - default: - return -EINVAL; - }; -} - -static int ubx_m8_config_block_fill(const struct device *dev, gnss_systems_t gnss_system, - struct ubx_cfg_gnss_payload *payload, uint8_t index, - uint32_t enable) -{ - uint32_t signal_config; - - switch (gnss_system) { - case GNSS_SYSTEM_GPS: - payload->config_blocks[index].gnss_id = UBX_GNSS_ID_GPS; - signal_config = UBX_CFG_GNSS_FLAG_SGN_CNF_GPS_L1C_A; - break; - case GNSS_SYSTEM_GLONASS: - payload->config_blocks[index].gnss_id = UBX_GNSS_ID_GLONASS; - signal_config = UBX_CFG_GNSS_FLAG_SGN_CNF_GLONASS_L1; - break; - case GNSS_SYSTEM_GALILEO: - payload->config_blocks[index].gnss_id = UBX_GNSS_ID_GALILEO; - signal_config = UBX_CFG_GNSS_FLAG_SGN_CNF_GALILEO_E1; + switch (nav_mode.dyn_model) { + case UBX_DYN_MODEL_STATIONARY: + *mode = GNSS_NAVIGATION_MODE_ZERO_DYNAMICS; break; - case GNSS_SYSTEM_BEIDOU: - payload->config_blocks[index].gnss_id = UBX_GNSS_ID_BEIDOU; - signal_config = UBX_CFG_GNSS_FLAG_SGN_CNF_BEIDOU_B1I; + case UBX_DYN_MODEL_PEDESTRIAN: + *mode = GNSS_NAVIGATION_MODE_LOW_DYNAMICS; break; - case GNSS_SYSTEM_QZSS: - payload->config_blocks[index].gnss_id = UBX_GNSS_ID_QZSS; - signal_config = UBX_CFG_GNSS_FLAG_SGN_CNF_QZSS_L1C_A; + case UBX_DYN_MODEL_PORTABLE: + *mode = GNSS_NAVIGATION_MODE_BALANCED_DYNAMICS; break; - case GNSS_SYSTEM_SBAS: - payload->config_blocks[index].gnss_id = UBX_GNSS_ID_SBAS; - signal_config = UBX_CFG_GNSS_FLAG_SGN_CNF_SBAS_L1C_A; + case UBX_DYN_MODEL_AIRBORNE_2G: + *mode = GNSS_NAVIGATION_MODE_HIGH_DYNAMICS; break; default: - return -EINVAL; - }; - - payload->config_blocks[index].flags = enable | signal_config; + return -EIO; + } return 0; } static int ubx_m8_set_enabled_systems(const struct device *dev, gnss_systems_t systems) { - int ret; - k_spinlock_key_t key; - struct ubx_m8_data *data = dev->data; - - key = k_spin_lock(&data->lock); - - struct ubx_cfg_gnss_payload *payload; - - /* Get number of tracking channels for each supported gnss system by sending CFG-GNSS. */ - ret = ubx_m8_modem_ubx_script_init(dev, NULL, UBX_FRM_GET_PAYLOAD_SZ, UBX_CLASS_CFG, - UBX_CFG_GNSS); - if (ret < 0) { - goto unlock; - } - - ret = ubx_m8_modem_ubx_run_script(dev, &(data->script)); - if (ret < 0) { - goto unlock; - } - - struct ubx_frame *response = data->script.response; - uint16_t res_trk_ch_sum = 0, max_trk_ch_sum = 0; - - /* Calculate sum of reserved and maximum tracking channels for each supported gnss system, - * and assert that the sum is not greater than the number of tracking channels in use. - */ - payload = (struct ubx_cfg_gnss_payload *) response->payload_and_checksum; - for (int i = 0; i < payload->num_config_blocks; ++i) { - ret = ubx_m8_ubx_gnss_id_to_gnss_system(dev, payload->config_blocks[i].gnss_id); - if (ret < 0) { - goto unlock; - } - - if (ret & systems) { - res_trk_ch_sum += payload->config_blocks[i].num_res_trk_ch; - max_trk_ch_sum += payload->config_blocks[i].max_num_trk_ch; - } - - if (res_trk_ch_sum > payload->num_trk_ch_use || - max_trk_ch_sum > payload->num_trk_ch_use) { - ret = -EINVAL; - goto unlock; - } - } - - /* Prepare payload (payload) for sending CFG-GNSS for enabling the gnss systems. */ - payload = malloc(sizeof(*payload) + - sizeof(struct ubx_cfg_gnss_payload_config_block) * UBX_M8_GNSS_SUPP_SYS_CNT); - if (!payload) { - ret = -ENOMEM; - goto unlock; - } - - payload->num_config_blocks = UBX_M8_GNSS_SUPP_SYS_CNT; - - ubx_cfg_gnss_payload_default(payload); - - uint8_t filled_blocks = 0; - gnss_systems_t supported_systems; - - ret = ubx_m8_get_supported_systems(dev, &supported_systems); - if (ret < 0) { - goto free_and_unlock; - } - - for (int i = 0; i < UBX_M8_GNSS_SYS_CNT; ++i) { - gnss_systems_t gnss_system = 1 << i; - - if (gnss_system & supported_systems) { - uint32_t enable = (systems & gnss_system) ? - UBX_CFG_GNSS_FLAG_ENABLE : UBX_CFG_GNSS_FLAG_DISABLE; - - ret = ubx_m8_config_block_fill(dev, gnss_system, payload, filled_blocks, - enable); - if (ret < 0) { - goto free_and_unlock; - } - - ++filled_blocks; - } - } - - ret = ubx_m8_modem_ubx_script_init(dev, payload, - UBX_CFG_GNSS_PAYLOAD_SZ(UBX_M8_GNSS_SUPP_SYS_CNT), - UBX_CLASS_CFG, UBX_CFG_GNSS); - if (ret < 0) { - goto free_and_unlock; - } - - ret = ubx_m8_modem_ubx_run_script(dev, &(data->script)); - if (ret < 0) { - goto free_and_unlock; - } - - k_sleep(K_MSEC(UBX_CFG_GNSS_WAIT_MS)); - -free_and_unlock: - free(payload); - -unlock: - k_spin_unlock(&data->lock, key); - - return ret; + return -ENOTSUP; } static int ubx_m8_get_enabled_systems(const struct device *dev, gnss_systems_t *systems) { - int ret; - k_spinlock_key_t key; - struct ubx_m8_data *data = dev->data; + static const struct ubx_frame get_enabled_systems = UBX_FRAME_GET_INITIALIZER( + UBX_CLASS_ID_MON, + UBX_MSG_ID_MON_GNSS); + struct ubx_mon_gnss gnss_selection; + int err; - key = k_spin_lock(&data->lock); - - ret = ubx_m8_modem_ubx_script_init(dev, NULL, UBX_FRM_GET_PAYLOAD_SZ, UBX_CLASS_CFG, - UBX_CFG_GNSS); - if (ret < 0) { - goto unlock; + err = ubx_m8_msg_get(dev, &get_enabled_systems, + UBX_FRAME_SZ(get_enabled_systems.payload_size), + (void *)&gnss_selection, sizeof(gnss_selection)); + if (err != 0) { + return err; } - ret = ubx_m8_modem_ubx_run_script(dev, &(data->script)); - if (ret < 0) { - goto unlock; - } - - struct ubx_frame *response = data->script.response; - struct ubx_cfg_gnss_payload *payload = - (struct ubx_cfg_gnss_payload *) response->payload_and_checksum; - *systems = 0; - for (int i = 0; i < payload->num_config_blocks; ++i) { - if (payload->config_blocks[i].flags & UBX_CFG_GNSS_FLAG_ENABLE) { - enum ubx_gnss_id gnss_id = payload->config_blocks[i].gnss_id; - - ret = ubx_m8_ubx_gnss_id_to_gnss_system(dev, gnss_id); - if (ret < 0) { - goto unlock; - } - - *systems |= ret; - } - } - -unlock: - k_spin_unlock(&data->lock, key); - - return ret; -} + *systems |= (gnss_selection.selection.enabled & UBX_GNSS_SELECTION_GPS) ? + GNSS_SYSTEM_GPS : 0; + *systems |= (gnss_selection.selection.enabled & UBX_GNSS_SELECTION_GLONASS) ? + GNSS_SYSTEM_GLONASS : 0; + *systems |= (gnss_selection.selection.enabled & UBX_GNSS_SELECTION_BEIDOU) ? + GNSS_SYSTEM_BEIDOU : 0; + *systems |= (gnss_selection.selection.enabled & UBX_GNSS_SELECTION_GALILEO) ? + GNSS_SYSTEM_GALILEO : 0; -static int ubx_m8_set_fix_rate(const struct device *dev, uint32_t fix_interval_ms) -{ - int ret; - k_spinlock_key_t key; - struct ubx_m8_data *data = dev->data; - struct ubx_cfg_rate_payload payload; - - if (fix_interval_ms < 50) { - return -1; - } - - key = k_spin_lock(&data->lock); - - ubx_cfg_rate_payload_default(&payload); - payload.meas_rate_ms = fix_interval_ms; - - ret = ubx_m8_modem_ubx_script_init(dev, &payload, UBX_CFG_RATE_PAYLOAD_SZ, UBX_CLASS_CFG, - UBX_CFG_RATE); - if (ret < 0) { - goto unlock; - } - - ret = ubx_m8_modem_ubx_run_script(dev, &(data->script)); - -unlock: - k_spin_unlock(&data->lock, key); - - return ret; + return 0; } -static int ubx_m8_get_fix_rate(const struct device *dev, uint32_t *fix_interval_ms) +static int ubx_m8_get_supported_systems(const struct device *dev, gnss_systems_t *systems) { - int ret; - k_spinlock_key_t key; - struct ubx_m8_data *data = dev->data; - struct ubx_cfg_rate_payload *payload; + static const struct ubx_frame get_enabled_systems = UBX_FRAME_GET_INITIALIZER( + UBX_CLASS_ID_MON, + UBX_MSG_ID_MON_GNSS); + struct ubx_mon_gnss gnss_selection; + int err; - key = k_spin_lock(&data->lock); - - ret = ubx_m8_modem_ubx_script_init(dev, NULL, UBX_FRM_GET_PAYLOAD_SZ, UBX_CLASS_CFG, - UBX_CFG_RATE); - if (ret < 0) { - goto unlock; + err = ubx_m8_msg_get(dev, &get_enabled_systems, + UBX_FRAME_SZ(get_enabled_systems.payload_size), + (void *)&gnss_selection, sizeof(gnss_selection)); + if (err != 0) { + return err; } - ret = ubx_m8_modem_ubx_run_script(dev, &(data->script)); - if (ret < 0) { - goto unlock; - } - - struct ubx_frame *response = data->script.response; - - payload = (struct ubx_cfg_rate_payload *) response->payload_and_checksum; - *fix_interval_ms = payload->meas_rate_ms; - -unlock: - k_spin_unlock(&data->lock, key); + *systems = 0; + *systems |= (gnss_selection.selection.supported & UBX_GNSS_SELECTION_GPS) ? + GNSS_SYSTEM_GPS : 0; + *systems |= (gnss_selection.selection.supported & UBX_GNSS_SELECTION_GLONASS) ? + GNSS_SYSTEM_GLONASS : 0; + *systems |= (gnss_selection.selection.supported & UBX_GNSS_SELECTION_BEIDOU) ? + GNSS_SYSTEM_BEIDOU : 0; + *systems |= (gnss_selection.selection.supported & UBX_GNSS_SELECTION_GALILEO) ? + GNSS_SYSTEM_GALILEO : 0; - return ret; + return 0; } static DEVICE_API(gnss, gnss_api) = { @@ -924,97 +568,42 @@ static DEVICE_API(gnss, gnss_api) = { .get_supported_systems = ubx_m8_get_supported_systems, }; -static int ubx_m8_configure(const struct device *dev) -{ - int ret; - - /* The return value could be ignored. See function description for more details. */ - (void)ubx_m8_configure_gnss_device_baudrate_prerequisite(dev); - - /* Stopping GNSS messages for clearer communication while configuring the device. */ - ret = ubx_m8_ubx_cfg_rst(dev, UBX_CFG_RST_RESET_MODE_CONTROLLED_GNSS_STOP); - if (ret < 0) { - goto reset; - } - - ret = ubx_m8_ubx_cfg_rate(dev); - if (ret < 0) { - LOG_ERR("Configuring rate failed. Returned %d.", ret); - goto reset; - } - - ret = ubx_m8_configure_gnss_device_baudrate(dev); - if (ret < 0) { - LOG_ERR("Configuring baudrate failed. Returned %d.", ret); - goto reset; - } - - ret = ubx_m8_configure_messages(dev); - if (ret < 0) { - LOG_ERR("Configuring messages failed. Returned %d.", ret); - } - -reset: - ret = ubx_m8_ubx_cfg_rst(dev, UBX_CFG_RST_RESET_MODE_CONTROLLED_GNSS_START); - - return ret; -} - -static int ubx_m8_init(const struct device *dev) -{ - int ret; - - ret = ubx_m8_init_nmea0183_match(dev); - if (ret < 0) { - return ret; - } - - ubx_m8_init_pipe(dev); - - ret = ubx_m8_init_chat(dev); - if (ret < 0) { - return ret; - } - - ret = ubx_m8_init_ubx(dev); - if (ret < 0) { - return ret; - } - - ret = ubx_m8_resume(dev); - if (ret < 0) { - return ret; - } - - ret = ubx_m8_configure(dev); - if (ret < 0) { - return ret; - } - - return 0; -} - -#define UBX_M8(inst) \ - static const struct ubx_m8_config ubx_m8_cfg_##inst = { \ - .uart = DEVICE_DT_GET(DT_INST_BUS(inst)), \ - .uart_baudrate = DT_PROP(DT_DRV_INST(inst), uart_baudrate), \ - }; \ - \ - static struct ubx_m8_data ubx_m8_data_##inst = { \ - .script.request = (struct ubx_frame *)ubx_m8_data_##inst.request_buf, \ - .script.response = (struct ubx_frame *)ubx_m8_data_##inst.response_buf, \ - .script.match = (struct ubx_frame *)ubx_m8_data_##inst.match_buf, \ - .script.retry_count = UBX_M8_SCRIPT_RETRY_DEFAULT, \ - .script.timeout = K_MSEC(MODEM_UBX_SCRIPT_TIMEOUT_MS), \ - }; \ - \ - DEVICE_DT_INST_DEFINE(inst, \ - ubx_m8_init, \ - NULL, \ - &ubx_m8_data_##inst, \ - &ubx_m8_cfg_##inst, \ - POST_KERNEL, \ - CONFIG_GNSS_INIT_PRIORITY, \ +#define UBX_M8(inst) \ + \ + BUILD_ASSERT( \ + (DT_PROP(DT_INST_BUS(inst), current_speed) == 9600) || \ + (DT_PROP(DT_INST_BUS(inst), current_speed) == 19200) || \ + (DT_PROP(DT_INST_BUS(inst), current_speed) == 38400) || \ + (DT_PROP(DT_INST_BUS(inst), current_speed) == 57600) || \ + (DT_PROP(DT_INST_BUS(inst), current_speed) == 115200) || \ + (DT_PROP(DT_INST_BUS(inst), current_speed) == 230400) || \ + (DT_PROP(DT_INST_BUS(inst), current_speed) == 460800), \ + "Invalid current-speed. Please set the UART current-speed to a baudrate " \ + "compatible with the modem."); \ + \ + BUILD_ASSERT((DT_INST_PROP(inst, fix_rate) >= 50) && \ + (DT_INST_PROP(inst, fix_rate) < 65536), \ + "Invalid fix-rate. Please set it higher than 50-ms" \ + " and must fit in 16-bits."); \ + \ + static const struct ubx_m8_config ubx_m8_cfg_##inst = { \ + .bus = DEVICE_DT_GET(DT_INST_BUS(inst)), \ + .baudrate = { \ + .initial = DT_INST_PROP(inst, initial_baudrate), \ + .desired = DT_PROP(DT_INST_BUS(inst), current_speed), \ + }, \ + .fix_rate_ms = DT_INST_PROP(inst, fix_rate), \ + }; \ + \ + static struct ubx_m8_data ubx_m8_data_##inst; \ + \ + DEVICE_DT_INST_DEFINE(inst, \ + ubx_m8_init, \ + NULL, \ + &ubx_m8_data_##inst, \ + &ubx_m8_cfg_##inst, \ + POST_KERNEL, \ + CONFIG_GNSS_INIT_PRIORITY, \ &gnss_api); DT_INST_FOREACH_STATUS_OKAY(UBX_M8) diff --git a/drivers/gnss/gnss_u_blox_protocol/gnss_u_blox_protocol.c b/drivers/gnss/gnss_u_blox_protocol/gnss_u_blox_protocol.c deleted file mode 100644 index d9a42baa1ba..00000000000 --- a/drivers/gnss/gnss_u_blox_protocol/gnss_u_blox_protocol.c +++ /dev/null @@ -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; -} diff --git a/drivers/gnss/gnss_u_blox_protocol/gnss_u_blox_protocol.h b/drivers/gnss/gnss_u_blox_protocol/gnss_u_blox_protocol.h deleted file mode 100644 index 6842a708def..00000000000 --- a/drivers/gnss/gnss_u_blox_protocol/gnss_u_blox_protocol.h +++ /dev/null @@ -1,251 +0,0 @@ -/* - * Copyright 2024 NXP - * - * SPDX-License-Identifier: Apache-2.0 - */ - -#include -#include -#include -#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_ */ diff --git a/drivers/gnss/gnss_u_blox_protocol/gnss_u_blox_protocol_defines.h b/drivers/gnss/gnss_u_blox_protocol/gnss_u_blox_protocol_defines.h deleted file mode 100644 index 977081a439d..00000000000 --- a/drivers/gnss/gnss_u_blox_protocol/gnss_u_blox_protocol_defines.h +++ /dev/null @@ -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_ */ diff --git a/drivers/gnss/gnss_ubx_common.c b/drivers/gnss/gnss_ubx_common.c new file mode 100644 index 00000000000..5f3a90da146 --- /dev/null +++ b/drivers/gnss/gnss_ubx_common.c @@ -0,0 +1,147 @@ +/* + * Copyright (c) 2025 Croxel Inc. + * Copyright (c) 2025 CogniPilot Foundation + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include + +#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 +} diff --git a/drivers/gnss/gnss_ubx_common.h b/drivers/gnss/gnss_ubx_common.h new file mode 100644 index 00000000000..0d591795403 --- /dev/null +++ b/drivers/gnss/gnss_ubx_common.h @@ -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 +#include +#include + +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_ */ diff --git a/dts/bindings/gnss/u-blox,m8.yaml b/dts/bindings/gnss/u-blox,m8.yaml index 13d7f466c11..d0286468e07 100644 --- a/dts/bindings/gnss/u-blox,m8.yaml +++ b/dts/bindings/gnss/u-blox,m8.yaml @@ -9,9 +9,20 @@ include: - uart-device.yaml properties: - uart-baudrate: + initial-baudrate: type: int description: | - Baudrate for communication on the UART port. - default: 115200 - enum: [4800, 9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600] + Initial baudrate to establish Baudrate for communication on the UART port. + This will be used for initial modem communication, which afterwards will + be changed to the baudrate set on the peripheral. For instance: Starting + at 9600 bps, but then switched up to 115200. + default: 9600 + enum: [4800, 9600, 19200, 38400, 57600, 115200, 230400, 460800] + + fix-rate: + type: int + default: 1000 + description: | + Initial fix-rate GNSS modem will be operating on. May be adjusted at + run-time through GNSS APIs. Must be greater than 50-ms. + Default is power-on setting. diff --git a/include/zephyr/modem/ubx.h b/include/zephyr/modem/ubx.h index e51cb5cd404..3b6493ee80b 100644 --- a/include/zephyr/modem/ubx.h +++ b/include/zephyr/modem/ubx.h @@ -1,5 +1,7 @@ /* - * Copyright 2024 NXP + * Copyright (c) 2024 NXP + * Copyright (c) 2025 Croxel Inc. + * Copyright (c) 2025 CogniPilot Foundation * * SPDX-License-Identifier: Apache-2.0 */ @@ -9,6 +11,7 @@ #include #include +#include #ifndef ZEPHYR_MODEM_UBX_ #define ZEPHYR_MODEM_UBX_ @@ -24,76 +27,70 @@ extern "C" { * @{ */ -#define UBX_FRM_HEADER_SZ 6 -#define UBX_FRM_FOOTER_SZ 2 -#define UBX_FRM_SZ_WITHOUT_PAYLOAD (UBX_FRM_HEADER_SZ + UBX_FRM_FOOTER_SZ) -#define UBX_FRM_SZ(payload_size) (payload_size + UBX_FRM_SZ_WITHOUT_PAYLOAD) - -#define UBX_PREAMBLE_SYNC_CHAR_1 0xB5 -#define UBX_PREAMBLE_SYNC_CHAR_2 0x62 - -#define UBX_FRM_PREAMBLE_SYNC_CHAR_1_IDX 0 -#define UBX_FRM_PREAMBLE_SYNC_CHAR_2_IDX 1 -#define UBX_FRM_MSG_CLASS_IDX 2 -#define UBX_FRM_MSG_ID_IDX 3 -#define UBX_FRM_PAYLOAD_SZ_L_IDX 4 -#define UBX_FRM_PAYLOAD_SZ_H_IDX 5 -#define UBX_FRM_PAYLOAD_IDX 6 -#define UBX_FRM_CHECKSUM_START_IDX 2 -#define UBX_FRM_CHECKSUM_STOP_IDX(frame_len) (frame_len - 2) - -#define UBX_PAYLOAD_SZ_MAX 256 -#define UBX_FRM_SZ_MAX UBX_FRM_SZ(UBX_PAYLOAD_SZ_MAX) - -struct ubx_frame { - uint8_t preamble_sync_char_1; - uint8_t preamble_sync_char_2; - uint8_t message_class; - uint8_t message_id; - uint8_t payload_size_low; - uint8_t payload_size_high; - uint8_t payload_and_checksum[]; +struct modem_ubx; + +typedef void (*modem_ubx_match_callback)(struct modem_ubx *ubx, + const struct ubx_frame *frame, + size_t len, + void *user_data); + +struct modem_ubx_match { + struct ubx_frame_match filter; + modem_ubx_match_callback handler; }; -struct modem_ubx_script { - struct ubx_frame *request; - struct ubx_frame *response; - struct ubx_frame *match; +#define MODEM_UBX_MATCH_ARRAY_DEFINE(_name, ...) \ + struct modem_ubx_match _name[] = {__VA_ARGS__}; + +#define MODEM_UBX_MATCH_DEFINE(_class_id, _msg_id, _handler) \ +{ \ + .filter = { \ + .class = _class_id, \ + .id = _msg_id, \ + }, \ + .handler = _handler, \ +} +struct modem_ubx_script { + struct { + const struct ubx_frame *buf; + uint16_t len; + } request; + struct { + uint8_t *buf; + uint16_t buf_len; + uint16_t received_len; + } response; + struct modem_ubx_match match; uint16_t retry_count; k_timeout_t timeout; }; struct modem_ubx { void *user_data; - - atomic_t state; - + atomic_t attached; uint8_t *receive_buf; uint16_t receive_buf_size; - - uint8_t *work_buf; - uint16_t work_buf_size; - uint16_t work_buf_len; - bool ubx_preamble_sync_chars_received; - - const struct modem_ubx_script *script; - + uint16_t receive_buf_offset; + struct modem_ubx_script *script; struct modem_pipe *pipe; - - struct k_work send_work; struct k_work process_work; struct k_sem script_stopped_sem; struct k_sem script_running_sem; + struct { + const struct modem_ubx_match *array; + size_t size; + } unsol_matches; }; struct modem_ubx_config { void *user_data; - uint8_t *receive_buf; uint16_t receive_buf_size; - uint8_t *work_buf; - uint16_t work_buf_size; + struct { + const struct modem_ubx_match *array; + size_t size; + } unsol_matches; }; /** @@ -143,27 +140,16 @@ int modem_ubx_init(struct modem_ubx *ubx, const struct modem_ubx_config *config) * 2. timeout (denoted by script.timeout) occurs. * @param ubx Modem Ubx instance * @param script Script to be executed - * @note The length of ubx frame in the script.request should not exceed UBX_FRM_SZ_MAX + * @note The length of ubx frame in the script.request should not exceed UBX_FRAME_SZ_MAX * @note Modem Ubx instance must be attached to a pipe instance - * @returns 0 if device acknowledged via UBX-ACK and no "get" response was received - * @returns positive integer denoting the length of "get" response that was received + * @returns 0 if successful * @returns negative errno code if failure */ -int modem_ubx_run_script(struct modem_ubx *ubx, const struct modem_ubx_script *script); +int modem_ubx_run_script(struct modem_ubx *ubx, struct modem_ubx_script *script); + +int modem_ubx_run_script_for_each(struct modem_ubx *ubx, struct modem_ubx_script *script, + struct ubx_frame *array, size_t array_size); -/** - * @brief Initialize ubx frame - * @param ubx_frame Ubx frame buffer - * @param ubx_frame_size Ubx frame buffer size - * @param msg_cls Message class - * @param msg_id Message id - * @param payload Payload buffer - * @param payload_size Payload buffer size - * @returns positive integer denoting the length of the ubx frame created - * @returns negative errno code if failure - */ -int modem_ubx_create_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); /** * @} */ diff --git a/include/zephyr/modem/ubx/checksum.h b/include/zephyr/modem/ubx/checksum.h new file mode 100644 index 00000000000..d228f0b2dec --- /dev/null +++ b/include/zephyr/modem/ubx/checksum.h @@ -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_ */ diff --git a/include/zephyr/modem/ubx/protocol.h b/include/zephyr/modem/ubx/protocol.h new file mode 100644 index 00000000000..0ba1861ef58 --- /dev/null +++ b/include/zephyr/modem/ubx/protocol.h @@ -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 +#include + +#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_ */ diff --git a/samples/drivers/gnss/boards/mimxrt1062_fmurt6.overlay b/samples/drivers/gnss/boards/mimxrt1062_fmurt6.overlay index cfcae77c1bf..d4292bdeee7 100644 --- a/samples/drivers/gnss/boards/mimxrt1062_fmurt6.overlay +++ b/samples/drivers/gnss/boards/mimxrt1062_fmurt6.overlay @@ -20,6 +20,6 @@ gnss: u_blox_m10 { status = "okay"; compatible = "u-blox,m8"; - uart-baudrate = <115200>; + initial-baudrate = <115200>; }; }; diff --git a/samples/drivers/gnss/boards/vmu_rt1170_mimxrt1176_cm7.overlay b/samples/drivers/gnss/boards/vmu_rt1170_mimxrt1176_cm7.overlay index 18c4d2d7850..481314e7e1f 100644 --- a/samples/drivers/gnss/boards/vmu_rt1170_mimxrt1176_cm7.overlay +++ b/samples/drivers/gnss/boards/vmu_rt1170_mimxrt1176_cm7.overlay @@ -17,6 +17,6 @@ gnss: gnss { status = "okay"; compatible = "u-blox,m8"; - uart-baudrate = <115200>; + initial-baudrate = <115200>; }; }; diff --git a/subsys/modem/modem_ubx.c b/subsys/modem/modem_ubx.c index b463fb790b3..22586b8444d 100644 --- a/subsys/modem/modem_ubx.c +++ b/subsys/modem/modem_ubx.c @@ -1,262 +1,138 @@ /* - * Copyright 2024 NXP + * Copyright (c) 2024 NXP + * Copyright (c) 2025 Croxel Inc. + * Copyright (c) 2025 CogniPilot Foundation * * SPDX-License-Identifier: Apache-2.0 */ #include -#include +#include #include LOG_MODULE_REGISTER(modem_ubx, CONFIG_MODEM_MODULES_LOG_LEVEL); -#define MODEM_UBX_STATE_ATTACHED_BIT 0 - -static int modem_ubx_validate_frame_size(uint16_t ubx_frame_size, uint8_t msg_cls, uint8_t msg_id, - uint16_t payload_size) -{ - if (ubx_frame_size > UBX_FRM_SZ_MAX || - ubx_frame_size < UBX_FRM_SZ_WITHOUT_PAYLOAD || - ubx_frame_size < UBX_FRM_SZ_WITHOUT_PAYLOAD + payload_size) { - return -1; - } - - return 0; -} - -int modem_ubx_create_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 (modem_ubx_validate_frame_size(ubx_frame_size, msg_cls, msg_id, payload_size)) { - return -1; - } - - struct ubx_frame *frame = (struct ubx_frame *) ubx_frame; - - frame->preamble_sync_char_1 = UBX_PREAMBLE_SYNC_CHAR_1; - frame->preamble_sync_char_2 = UBX_PREAMBLE_SYNC_CHAR_2; - frame->message_class = msg_cls; - frame->message_id = msg_id; - frame->payload_size_low = payload_size; - frame->payload_size_high = payload_size >> 8; - - memcpy(frame->payload_and_checksum, payload, payload_size); - - uint16_t ubx_frame_len = payload_size + UBX_FRM_SZ_WITHOUT_PAYLOAD; - - uint8_t ckA = 0, ckB = 0; - - for (unsigned int i = UBX_FRM_CHECKSUM_START_IDX; - i < (UBX_FRM_CHECKSUM_STOP_IDX(ubx_frame_len)); i++) { - ckA += ubx_frame[i]; - ckB += ckA; - } - - frame->payload_and_checksum[payload_size] = ckA; - frame->payload_and_checksum[payload_size + 1] = ckB; - - return ubx_frame_len; -} - -static void modem_ubx_reset_received_ubx_preamble_sync_chars(struct modem_ubx *ubx) -{ - ubx->ubx_preamble_sync_chars_received = false; -} - -static void modem_ubx_reset_parser(struct modem_ubx *ubx) -{ - modem_ubx_reset_received_ubx_preamble_sync_chars(ubx); -} - -static int modem_ubx_get_payload_length(struct ubx_frame *frame) -{ - uint16_t payload_len = frame->payload_size_high; - - payload_len = payload_len << 8; - - return payload_len | frame->payload_size_low; -} - -static int modem_ubx_get_frame_length(struct ubx_frame *frame) -{ - return modem_ubx_get_payload_length(frame) + UBX_FRM_SZ_WITHOUT_PAYLOAD; -} - -static bool modem_ubx_match_frame_type(struct ubx_frame *frame_1, struct ubx_frame *frame_2) -{ - if (frame_1->message_class == frame_2->message_class - && frame_1->message_id == frame_2->message_id) { - return true; - } else { - return false; - } -} - -static bool modem_ubx_match_frame_full(struct ubx_frame *frame_1, struct ubx_frame *frame_2) +static void modem_ubx_pipe_callback(struct modem_pipe *pipe, + enum modem_pipe_event event, + void *user_data) { - if (modem_ubx_get_frame_length(frame_1) != modem_ubx_get_frame_length(frame_2)) { - return false; - } + struct modem_ubx *ubx = (struct modem_ubx *)user_data; - if (memcmp(frame_1, frame_2, modem_ubx_get_frame_length(frame_1)) == 0) { - return true; - } else { - return false; + if (event == MODEM_PIPE_EVENT_RECEIVE_READY) { + k_work_submit(&ubx->process_work); } } -static void modem_ubx_script_init(struct modem_ubx *ubx, const struct modem_ubx_script *script) -{ - ubx->script = script; -} - -static int modem_ubx_run_script_helper(struct modem_ubx *ubx, const struct modem_ubx_script *script) +int modem_ubx_run_script(struct modem_ubx *ubx, struct modem_ubx_script *script) { int ret; + bool wait_for_rsp = script->match.filter.class != 0; - if (ubx->pipe == NULL) { - return -EPERM; + ret = k_sem_take(&ubx->script_running_sem, script->timeout); + if (ret != 0) { + return -EBUSY; } + ubx->script = script; k_sem_reset(&ubx->script_stopped_sem); - modem_ubx_reset_parser(ubx); + int tries = ubx->script->retry_count + 1; + int32_t ms_per_attempt = (uint64_t)k_ticks_to_ms_floor64(script->timeout.ticks) / tries; - k_work_submit(&ubx->send_work); - - if (ubx->script->match == NULL) { - return 0; - } - - ret = k_sem_take(&ubx->script_stopped_sem, script->timeout); - if (ret < 0) { - return ret; - } + do { + ret = modem_pipe_transmit(ubx->pipe, + (const uint8_t *)ubx->script->request.buf, + ubx->script->request.len); - return 0; -} - -int modem_ubx_run_script(struct modem_ubx *ubx, const struct modem_ubx_script *script) -{ - int ret, attempt; - - if (modem_ubx_get_frame_length(script->request) > UBX_FRM_SZ_MAX) { - return -EFBIG; - } - - if (atomic_test_bit(&ubx->state, MODEM_UBX_STATE_ATTACHED_BIT) == false) { - return -EPERM; - } - - ret = k_sem_take(&ubx->script_running_sem, K_FOREVER); - if (ret < 0) { - return ret; - } - - modem_ubx_script_init(ubx, script); - - for (attempt = 0; attempt < script->retry_count; ++attempt) { - ret = modem_ubx_run_script_helper(ubx, script); - if (ret > -1) { - LOG_INF("Successfully executed script on attempt: %d.", attempt); - break; - } else if (ret == -EPERM) { - break; + if (wait_for_rsp) { + ret = k_sem_take(&ubx->script_stopped_sem, K_MSEC(ms_per_attempt)); } - } + tries--; + } while ((tries > 0) && (ret < 0)); - if (ret < 0) { - LOG_ERR("Failed to execute script successfully. Attempts: %d.", attempt); - goto unlock; - } - -unlock: k_sem_give(&ubx->script_running_sem); - return ret; -} - -static void modem_ubx_pipe_callback(struct modem_pipe *pipe, enum modem_pipe_event event, - void *user_data) -{ - struct modem_ubx *ubx = (struct modem_ubx *)user_data; - - if (event == MODEM_PIPE_EVENT_RECEIVE_READY) { - k_work_submit(&ubx->process_work); - } -} - -static void modem_ubx_send_handler(struct k_work *item) -{ - struct modem_ubx *ubx = CONTAINER_OF(item, struct modem_ubx, send_work); - int ret, tx_frame_len; - - tx_frame_len = modem_ubx_get_frame_length(ubx->script->request); - ret = modem_pipe_transmit(ubx->pipe, (const uint8_t *) ubx->script->request, tx_frame_len); - if (ret < tx_frame_len) { - LOG_ERR("Ubx frame transmission failed. Returned %d.", ret); - return; - } -} - -static int modem_ubx_process_received_ubx_frame(struct modem_ubx *ubx) -{ - int ret; - struct ubx_frame *received = (struct ubx_frame *) ubx->work_buf; - - if (modem_ubx_match_frame_full(received, ubx->script->match) == true) { - /* Frame matched successfully. Terminate the script. */ - k_sem_give(&ubx->script_stopped_sem); - ret = 0; - } else if (modem_ubx_match_frame_type(received, ubx->script->request) == true) { - /* Response received successfully. Script not ended. */ - memcpy(ubx->script->response, ubx->work_buf, ubx->work_buf_len); - ret = -1; - } else { - /* Ignore the received frame. The device may automatically send periodic frames. - * These frames are not relevant for our script's execution and must be ignored. - */ - ret = -1; + return (ret > 0) ? 0 : ret; +} + +enum ubx_process_result { + UBX_PROCESS_RESULT_NO_DATA_FOUND, + UBX_PROCESS_RESULT_FRAME_INCOMPLETE, + UBX_PROCESS_RESULT_FRAME_FOUND +}; + +static inline enum ubx_process_result process_incoming_data(const uint8_t *data, + size_t len, + const struct ubx_frame **frame_start, + size_t *frame_len, + size_t *iterator) +{ + for (int i = (*iterator) ; i < len ; i++) { + if (data[i] == UBX_PREAMBLE_SYNC_CHAR_1) { + + const struct ubx_frame *frame = (const struct ubx_frame *)&data[i]; + size_t remaining_bytes = len - i; + + /* Wait until we've got the full header to keep processing data */ + if (UBX_FRAME_HEADER_SZ > remaining_bytes) { + *frame_start = frame; + *frame_len = remaining_bytes; + return UBX_PROCESS_RESULT_FRAME_INCOMPLETE; + } + + /* Filter false-positive: Sync-byte 1 contained in payload */ + if (frame->preamble_sync_char_2 != UBX_PREAMBLE_SYNC_CHAR_2) { + continue; + } + + /* Invalid length filtering */ + if (UBX_FRAME_SZ(frame->payload_size) > UBX_FRAME_SZ_MAX) { + continue; + } + + /* Check if we should wait until packet is completely received */ + if (UBX_FRAME_SZ(frame->payload_size) > remaining_bytes) { + *frame_start = frame; + *frame_len = remaining_bytes; + return UBX_PROCESS_RESULT_FRAME_INCOMPLETE; + } + + /* We should have all the packet, so we validate checksum. */ + uint16_t valid_checksum = ubx_calc_checksum(frame, + UBX_FRAME_SZ(frame->payload_size)); + uint16_t ck_a = frame->payload_and_checksum[frame->payload_size]; + uint16_t ck_b = frame->payload_and_checksum[frame->payload_size + 1]; + uint16_t actual_checksum = ck_a | (ck_b << 8); + + if (valid_checksum != actual_checksum) { + continue; + } + + *frame_start = frame; + *frame_len = UBX_FRAME_SZ(frame->payload_size); + + *iterator = i + 1; + return UBX_PROCESS_RESULT_FRAME_FOUND; + } } - modem_ubx_reset_parser(ubx); - - return ret; + return UBX_PROCESS_RESULT_NO_DATA_FOUND; } -static int modem_ubx_process_received_byte(struct modem_ubx *ubx, uint8_t byte) +static inline bool matches_filter(const struct ubx_frame *frame, + const struct ubx_frame_match *filter) { - static uint8_t prev_byte; - static uint16_t rx_ubx_frame_len; - - if (ubx->ubx_preamble_sync_chars_received == false) { - if (prev_byte == UBX_PREAMBLE_SYNC_CHAR_1 && byte == UBX_PREAMBLE_SYNC_CHAR_2) { - ubx->ubx_preamble_sync_chars_received = true; - ubx->work_buf[0] = UBX_PREAMBLE_SYNC_CHAR_1; - ubx->work_buf[1] = UBX_PREAMBLE_SYNC_CHAR_2; - ubx->work_buf_len = 2; - } + if ((frame->class == filter->class) && + (frame->id == filter->id) && + ((filter->payload.len == 0) || + ((frame->payload_size == filter->payload.len) && + (0 == memcmp(frame->payload_and_checksum, + filter->payload.buf, + filter->payload.len))))) { + return true; } else { - ubx->work_buf[ubx->work_buf_len] = byte; - ++ubx->work_buf_len; - - if (ubx->work_buf_len == UBX_FRM_HEADER_SZ) { - uint16_t rx_ubx_payload_len = ubx->work_buf[UBX_FRM_PAYLOAD_SZ_H_IDX]; - - rx_ubx_payload_len = ubx->work_buf[UBX_FRM_PAYLOAD_SZ_H_IDX] << 8; - rx_ubx_payload_len |= ubx->work_buf[UBX_FRM_PAYLOAD_SZ_L_IDX]; - - rx_ubx_frame_len = rx_ubx_payload_len + UBX_FRM_SZ_WITHOUT_PAYLOAD; - } - - if (ubx->work_buf_len == rx_ubx_frame_len) { - return modem_ubx_process_received_ubx_frame(ubx); - } + return false; } - - prev_byte = byte; - - return -1; } static void modem_ubx_process_handler(struct k_work *item) @@ -264,26 +140,59 @@ static void modem_ubx_process_handler(struct k_work *item) struct modem_ubx *ubx = CONTAINER_OF(item, struct modem_ubx, process_work); int ret; - ret = modem_pipe_receive(ubx->pipe, ubx->receive_buf, ubx->receive_buf_size); - if (ret < 1) { - return; - } - - const size_t length = ret; - - for (int i = 0; i < length; i++) { - ret = modem_ubx_process_received_byte(ubx, ubx->receive_buf[i]); - if (ret == 0) { /* Frame matched successfully. Terminate the script. */ + ret = modem_pipe_receive(ubx->pipe, + &ubx->receive_buf[ubx->receive_buf_offset], + (ubx->receive_buf_size - ubx->receive_buf_offset)); + + const uint8_t *received_data = ubx->receive_buf; + size_t length = ret > 0 ? (ret + ubx->receive_buf_offset) : 0; + const struct ubx_frame *frame = NULL; + size_t frame_len = 0; + size_t iterator = 0; + enum ubx_process_result process_result; + + do { + process_result = process_incoming_data(received_data, length, + &frame, &frame_len, + &iterator); + switch (process_result) { + case UBX_PROCESS_RESULT_FRAME_FOUND: + /** Serve script first */ + if (matches_filter(frame, &ubx->script->match.filter)) { + memcpy(ubx->script->response.buf, frame, frame_len); + ubx->script->response.received_len = frame_len; + + k_sem_give(&ubx->script_stopped_sem); + } + /** Check for unsolicited matches */ + for (size_t i = 0 ; i < ubx->unsol_matches.size ; i++) { + if (ubx->unsol_matches.array[i].handler && + matches_filter(frame, &ubx->unsol_matches.array[i].filter)) { + ubx->unsol_matches.array[i].handler(ubx, frame, frame_len, + ubx->user_data); + } + } + break; + case UBX_PROCESS_RESULT_FRAME_INCOMPLETE: + /** If we had an incomplete packet, discard prior data + * and offset next pipe-receive to process remaining + * info. + */ + memcpy(ubx->receive_buf, frame, frame_len); + ubx->receive_buf_offset = frame_len; + break; + case UBX_PROCESS_RESULT_NO_DATA_FOUND: + ubx->receive_buf_offset = 0; break; + default: + CODE_UNREACHABLE; } - } - - k_work_submit(&ubx->process_work); + } while (process_result == UBX_PROCESS_RESULT_FRAME_FOUND); } int modem_ubx_attach(struct modem_ubx *ubx, struct modem_pipe *pipe) { - if (atomic_test_and_set_bit(&ubx->state, MODEM_UBX_STATE_ATTACHED_BIT) == true) { + if (atomic_test_and_set_bit(&ubx->attached, 0) == true) { return 0; } @@ -298,17 +207,14 @@ void modem_ubx_release(struct modem_ubx *ubx) { struct k_work_sync sync; - if (atomic_test_and_clear_bit(&ubx->state, MODEM_UBX_STATE_ATTACHED_BIT) == false) { + if (atomic_test_and_clear_bit(&ubx->attached, 0) == false) { return; } modem_pipe_release(ubx->pipe); - k_work_cancel_sync(&ubx->send_work, &sync); k_work_cancel_sync(&ubx->process_work, &sync); k_sem_reset(&ubx->script_stopped_sem); k_sem_reset(&ubx->script_running_sem); - ubx->work_buf_len = 0; - modem_ubx_reset_parser(ubx); ubx->pipe = NULL; } @@ -318,20 +224,18 @@ int modem_ubx_init(struct modem_ubx *ubx, const struct modem_ubx_config *config) __ASSERT_NO_MSG(config != NULL); __ASSERT_NO_MSG(config->receive_buf != NULL); __ASSERT_NO_MSG(config->receive_buf_size > 0); - __ASSERT_NO_MSG(config->work_buf != NULL); - __ASSERT_NO_MSG(config->work_buf_size > 0); memset(ubx, 0x00, sizeof(*ubx)); ubx->user_data = config->user_data; ubx->receive_buf = config->receive_buf; ubx->receive_buf_size = config->receive_buf_size; - ubx->work_buf = config->work_buf; - ubx->work_buf_size = config->work_buf_size; ubx->pipe = NULL; - k_work_init(&ubx->send_work, modem_ubx_send_handler); + ubx->unsol_matches.array = config->unsol_matches.array; + ubx->unsol_matches.size = config->unsol_matches.size; + k_work_init(&ubx->process_work, modem_ubx_process_handler); k_sem_init(&ubx->script_stopped_sem, 0, 1); k_sem_init(&ubx->script_running_sem, 1, 1); diff --git a/tests/drivers/build_all/gnss/app.overlay b/tests/drivers/build_all/gnss/app.overlay index cd3aed7fbf1..d390aea7c32 100644 --- a/tests/drivers/build_all/gnss/app.overlay +++ b/tests/drivers/build_all/gnss/app.overlay @@ -12,6 +12,7 @@ test_uart: uart@0 { compatible = "vnd,serial"; reg = <0x0 0x1000>; + current-speed = <9600>; status = "okay"; gnss_nmea_generic: gnss-nmea-generic { diff --git a/tests/subsys/modem/modem_ubx/CMakeLists.txt b/tests/subsys/modem/modem_ubx/CMakeLists.txt new file mode 100644 index 00000000000..f2fa110c626 --- /dev/null +++ b/tests/subsys/modem/modem_ubx/CMakeLists.txt @@ -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) diff --git a/tests/subsys/modem/modem_ubx/prj.conf b/tests/subsys/modem/modem_ubx/prj.conf new file mode 100644 index 00000000000..3ca6ddea3c7 --- /dev/null +++ b/tests/subsys/modem/modem_ubx/prj.conf @@ -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 diff --git a/tests/subsys/modem/modem_ubx/src/main.c b/tests/subsys/modem/modem_ubx/src/main.c new file mode 100644 index 00000000000..8b04f561190 --- /dev/null +++ b/tests/subsys/modem/modem_ubx/src/main.c @@ -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 +#include + +#include +#include +#include + +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))); +} diff --git a/tests/subsys/modem/modem_ubx/testcase.yaml b/tests/subsys/modem/modem_ubx/testcase.yaml new file mode 100644 index 00000000000..2bbe5f994d1 --- /dev/null +++ b/tests/subsys/modem/modem_ubx/testcase.yaml @@ -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