diff --git a/cmake/linker_script/common/common-rom.cmake b/cmake/linker_script/common/common-rom.cmake index 601e987bd9696..1df506522d472 100644 --- a/cmake/linker_script/common/common-rom.cmake +++ b/cmake/linker_script/common/common-rom.cmake @@ -273,3 +273,7 @@ endif() if(CONFIG_GNSS_RTK) zephyr_iterable_section(NAME gnss_rtk_data_callback KVMA RAM_REGION GROUP RODATA_REGION) endif() + +if(CONFIG_GNSS_VELNED) + zephyr_iterable_section(NAME gnss_velned_callback KVMA RAM_REGION GROUP RODATA_REGION) +endif() diff --git a/drivers/gnss/Kconfig b/drivers/gnss/Kconfig index cbb504add46f4..9bfa10e1b9731 100644 --- a/drivers/gnss/Kconfig +++ b/drivers/gnss/Kconfig @@ -14,6 +14,11 @@ config GNSS_SATELLITES help Enable GNSS satellites callback. +config GNSS_VELNED + bool "GNSS velocity support" + help + Enable GNSS velocity callback. + config GNSS_DUMP bool "GNSS dump support" depends on LOG diff --git a/drivers/gnss/gnss_publish.c b/drivers/gnss/gnss_publish.c index 309c8560dd1f6..34628169a9b56 100644 --- a/drivers/gnss/gnss_publish.c +++ b/drivers/gnss/gnss_publish.c @@ -38,3 +38,18 @@ void gnss_publish_satellites(const struct device *dev, const struct gnss_satelli k_sem_give(&semlock); } #endif + +#if CONFIG_GNSS_VELNED +void gnss_publish_velned(const struct device *dev, const struct gnss_velned *velned) +{ + (void)k_sem_take(&semlock, K_FOREVER); + + STRUCT_SECTION_FOREACH(gnss_velned_callback, callback) { + if (callback->dev == NULL || callback->dev == dev) { + callback->callback(dev, velned); + } + } + + k_sem_give(&semlock); +} +#endif diff --git a/drivers/gnss/gnss_u_blox_m8.c b/drivers/gnss/gnss_u_blox_m8.c index 62d3c065b7722..550570f7c9873 100644 --- a/drivers/gnss/gnss_u_blox_m8.c +++ b/drivers/gnss/gnss_u_blox_m8.c @@ -85,6 +85,10 @@ UBX_FRAME_DEFINE(enable_nav, UBX_FRAME_DEFINE(enable_sat, UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NAV, UBX_MSG_ID_NAV_SAT, 1)); #endif +#if CONFIG_GNSS_VELNED +UBX_FRAME_DEFINE(enable_velned, + UBX_FRAME_CFG_MSG_RATE_INITIALIZER(UBX_CLASS_ID_NAV, UBX_MSG_ID_NAV_VELNED, 1)); +#endif UBX_FRAME_ARRAY_DEFINE(u_blox_m8_init_seq, &disable_gga, &disable_rmc, &disable_gsv, &disable_dtm, &disable_gbs, @@ -93,6 +97,9 @@ UBX_FRAME_ARRAY_DEFINE(u_blox_m8_init_seq, #if CONFIG_GNSS_SATELLITES &enable_sat, #endif +#if CONFIG_GNSS_VELNED + &enable_velned, +#endif ); MODEM_UBX_MATCH_ARRAY_DEFINE(u_blox_m8_unsol_messages, @@ -102,6 +109,10 @@ MODEM_UBX_MATCH_ARRAY_DEFINE(u_blox_m8_unsol_messages, MODEM_UBX_MATCH_DEFINE(UBX_CLASS_ID_NAV, UBX_MSG_ID_NAV_SAT, gnss_ubx_common_satellite_callback), #endif +#if CONFIG_GNSS_VELNED + MODEM_UBX_MATCH_DEFINE(UBX_CLASS_ID_NAV, UBX_MSG_ID_NAV_VELNED, + gnss_ubx_common_velned_callback), +#endif ); static int ubx_m8_msg_get(const struct device *dev, const struct ubx_frame *req, diff --git a/drivers/gnss/gnss_ubx_common.c b/drivers/gnss/gnss_ubx_common.c index 35d0769098cbf..0f9e20667ca69 100644 --- a/drivers/gnss/gnss_ubx_common.c +++ b/drivers/gnss/gnss_ubx_common.c @@ -147,6 +147,32 @@ void gnss_ubx_common_satellite_callback(struct modem_ubx *ubx, const struct ubx_ } #endif +#if CONFIG_GNSS_VELNED +void gnss_ubx_common_velned_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_velned))) { + return; + } + + const struct ubx_nav_velned *velned = + (const struct ubx_nav_velned *)frame->payload_and_checksum; + struct gnss_ubx_common_data *data = user_data; + const struct device *dev = data->gnss; + + data->velned.velocity_n = velned->vel_n; + data->velned.velocity_e = velned->vel_e; + data->velned.velocity_d = velned->vel_d; + data->velned.speed = velned->speed; + data->velned.ground_speed = velned->ground_speed; + data->velned.heading = velned->heading; + data->velned.speed_acc = velned->speed_acc; + data->velned.heading_acc = velned->heading_acc; + + gnss_publish_velned(dev, &data->velned); +} +#endif + void gnss_ubx_common_init(struct gnss_ubx_common_data *data, const struct gnss_ubx_common_config *config) { @@ -155,4 +181,14 @@ void gnss_ubx_common_init(struct gnss_ubx_common_data *data, data->satellites.data = config->satellites.buf; data->satellites.size = config->satellites.size; #endif +#if CONFIG_GNSS_VELNED + data->velned.velocity_n = 0; + data->velned.velocity_e = 0; + data->velned.velocity_d = 0; + data->velned.speed = 0; + data->velned.ground_speed = 0; + data->velned.heading = 0; + data->velned.speed_acc = 0; + data->velned.heading_acc = 0; +#endif } diff --git a/drivers/gnss/gnss_ubx_common.h b/drivers/gnss/gnss_ubx_common.h index 0d59179540394..5549b1d00c685 100644 --- a/drivers/gnss/gnss_ubx_common.h +++ b/drivers/gnss/gnss_ubx_common.h @@ -22,6 +22,9 @@ struct gnss_ubx_common_data { size_t size; } satellites; #endif +#if CONFIG_GNSS_VELNED + struct gnss_velned velned; +#endif }; struct gnss_ubx_common_config { @@ -38,6 +41,9 @@ void gnss_ubx_common_pvt_callback(struct modem_ubx *ubx, const struct ubx_frame 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_velned_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); diff --git a/include/zephyr/drivers/gnss.h b/include/zephyr/drivers/gnss.h index d27a639848226..00a6ce8f9e81e 100644 --- a/include/zephyr/drivers/gnss.h +++ b/include/zephyr/drivers/gnss.h @@ -229,6 +229,37 @@ struct gnss_satellites_callback { gnss_satellites_callback_t callback; }; +/** GNSS velocity structure */ +struct gnss_velned { + /** Velocity North in cm/s */ + int32_t velocity_n; + /** Velocity East in cm/s */ + int32_t velocity_e; + /** Velocity Down in cm/s */ + int32_t velocity_d; + /** Speed in cm/s */ + uint32_t speed; + /** Ground speed in cm/s */ + uint32_t ground_speed; + /** Heading in degrees */ + int32_t heading; + /** Speed accuracy in cm/s */ + uint32_t speed_acc; + /** Heading accuracy in degrees */ + uint32_t heading_acc; +}; + +/** Template for GNSS velocity callback */ +typedef void (*gnss_velned_callback_t)(const struct device *dev, const struct gnss_velned *velned); + +/** GNSS callback structure */ +struct gnss_velned_callback { + /** Filter callback to GNSS data from this device if not NULL */ + const struct device *dev; + /** Callback called when GNSS velocity is published */ + gnss_velned_callback_t callback; +}; + /** * @brief Set the GNSS fix rate * @@ -451,6 +482,23 @@ static inline int z_impl_gnss_get_latest_timepulse(const struct device *dev, #define GNSS_SATELLITES_CALLBACK_DEFINE(_dev, _callback) #endif +/** + * @brief Register a callback structure for GNSS velocity published + * + * @param _dev Device pointer + * @param _callback The callback function + */ +#if CONFIG_GNSS_VELNED +#define GNSS_VELNED_CALLBACK_DEFINE(_dev, _callback) \ + static const STRUCT_SECTION_ITERABLE(gnss_velned_callback, \ + _gnss_velned_callback__##_callback) = { \ + .dev = _dev, \ + .callback = _callback, \ + } +#else +#define GNSS_VELNED_CALLBACK_DEFINE(_dev, _callback) +#endif + /** * @} */ diff --git a/include/zephyr/drivers/gnss/gnss_publish.h b/include/zephyr/drivers/gnss/gnss_publish.h index b686c7eaed112..d96145e3672d8 100644 --- a/include/zephyr/drivers/gnss/gnss_publish.h +++ b/include/zephyr/drivers/gnss/gnss_publish.h @@ -16,4 +16,7 @@ void gnss_publish_data(const struct device *dev, const struct gnss_data *data); void gnss_publish_satellites(const struct device *dev, const struct gnss_satellite *satellites, uint16_t size); +/** Internal function used by GNSS drivers to publish GNSS velocity */ +void gnss_publish_velned(const struct device *dev, const struct gnss_velned *velned); + #endif /* ZEPHYR_DRIVERS_GNSS_GNSS_H_ */ diff --git a/include/zephyr/linker/common-rom/common-rom-misc.ld b/include/zephyr/linker/common-rom/common-rom-misc.ld index 41c5c4f5d5b61..daa68cf867c78 100644 --- a/include/zephyr/linker/common-rom/common-rom-misc.ld +++ b/include/zephyr/linker/common-rom/common-rom-misc.ld @@ -76,3 +76,7 @@ #if defined(CONFIG_GNSS_RTK) ITERABLE_SECTION_ROM(gnss_rtk_data_callback, Z_LINK_ITERABLE_SUBALIGN) #endif + +#if defined(CONFIG_GNSS_VELNED) + ITERABLE_SECTION_ROM(gnss_velned_callback, Z_LINK_ITERABLE_SUBALIGN) +#endif diff --git a/include/zephyr/modem/ubx/protocol.h b/include/zephyr/modem/ubx/protocol.h index 747edad32b10d..e4977db414765 100644 --- a/include/zephyr/modem/ubx/protocol.h +++ b/include/zephyr/modem/ubx/protocol.h @@ -62,6 +62,7 @@ enum ubx_class_id { enum ubx_msg_id_nav { UBX_MSG_ID_NAV_PVT = 0x07, + UBX_MSG_ID_NAV_VELNED = 0x12, UBX_MSG_ID_NAV_SAT = 0x35, }; @@ -168,6 +169,18 @@ struct ubx_nav_sat { } sat[]; }; +struct ubx_nav_velned { + uint32_t itow; + int32_t vel_n; + int32_t vel_e; + int32_t vel_d; + uint32_t speed; + uint32_t ground_speed; + int32_t heading; + uint32_t speed_acc; + uint32_t heading_acc; +} __packed; + enum ubx_msg_id_ack { UBX_MSG_ID_ACK = 0x01, UBX_MSG_ID_NAK = 0x00