From ef20c6ca8ce3a969cef507e4334ddbf022fa410a Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Wed, 19 Jul 2023 19:19:20 -0400 Subject: [PATCH] Add UBX GNSS Driver Signed-off-by: Peter van der Perk --- app/mrbuggy3/boards/mimxrt1170_fmurt7.conf | 12 +++ app/mrbuggy3/boards/mimxrt1170_fmurt7.overlay | 17 ++-- drivers/sense/CMakeLists.txt | 1 + drivers/sense/Kconfig | 1 + drivers/sense/ubx_gnss/CMakeLists.txt | 14 +++ drivers/sense/ubx_gnss/Kconfig | 11 +++ drivers/sense/ubx_gnss/main.c | 86 +++++++++++++++++++ drivers/synapse/zbus/channels.c | 15 ++++ include/synapse/zbus/channels.h | 1 + lib/control/diffdrive/main.c | 2 + west.yml | 8 ++ 11 files changed, 157 insertions(+), 11 deletions(-) create mode 100644 drivers/sense/ubx_gnss/CMakeLists.txt create mode 100644 drivers/sense/ubx_gnss/Kconfig create mode 100644 drivers/sense/ubx_gnss/main.c diff --git a/app/mrbuggy3/boards/mimxrt1170_fmurt7.conf b/app/mrbuggy3/boards/mimxrt1170_fmurt7.conf index 1082514a..d2ddad63 100644 --- a/app/mrbuggy3/boards/mimxrt1170_fmurt7.conf +++ b/app/mrbuggy3/boards/mimxrt1170_fmurt7.conf @@ -52,3 +52,15 @@ CONFIG_SHELL_ARGC_MAX=72 CONFIG_I2C=y CONFIG_SENSOR=y CONFIG_SENSE_INA226=y +CONFIG_SENSE_UBX_GNSS=y + +CONFIG_HEAP_MEM_POOL_SIZE=8192 + +# Debugging +CONFIG_DEBUG_THREAD_INFO=y + +# Logging +CONFIG_LOG=y +CONFIG_LOG_MODE_DEFERRED=y +CONFIG_LOG_BACKEND_NET=y +CONFIG_LOG_BACKEND_NET_SERVER="192.0.2.2" diff --git a/app/mrbuggy3/boards/mimxrt1170_fmurt7.overlay b/app/mrbuggy3/boards/mimxrt1170_fmurt7.overlay index 9f5d7829..0381781f 100644 --- a/app/mrbuggy3/boards/mimxrt1170_fmurt7.overlay +++ b/app/mrbuggy3/boards/mimxrt1170_fmurt7.overlay @@ -32,13 +32,14 @@ }; chosen { - zephyr,canbus = &flexcan1; + zephyr,canbus = &flexcan3; }; aliases { can0 = &flexcan1; can1 = &flexcan2; can2 = &flexcan3; + telem1 = &lpuart4; }; }; @@ -110,14 +111,8 @@ }; }; -&lpi2c2 { - status = "disabled"; - pinctrl-0 = <&pinmux_lpi2c2>; - pinctrl-names = "default"; - - bmp388: bmp388@76 { - compatible = "bosch,bmp388"; - reg = <0x76>; - status = "disabled"; - }; +/* GNSS */ +uart0: &lpuart3 { + status = "okay"; + current-speed = <38400>; }; diff --git a/drivers/sense/CMakeLists.txt b/drivers/sense/CMakeLists.txt index 38af0a98..522b6009 100644 --- a/drivers/sense/CMakeLists.txt +++ b/drivers/sense/CMakeLists.txt @@ -2,3 +2,4 @@ add_subdirectory_ifdef(CONFIG_SYNAPSE_VESC_CAN_STATUS vesc_can_status) add_subdirectory_ifdef(CONFIG_SENSE_INA226 ina226) +add_subdirectory_ifdef(CONFIG_SENSE_UBX_GNSS ubx_gnss) diff --git a/drivers/sense/Kconfig b/drivers/sense/Kconfig index c2bd1c73..44379f6f 100644 --- a/drivers/sense/Kconfig +++ b/drivers/sense/Kconfig @@ -5,5 +5,6 @@ menu "Sense" rsource "vesc_can_status/Kconfig" rsource "ina226/Kconfig" +rsource "ubx_gnss/Kconfig" endmenu diff --git a/drivers/sense/ubx_gnss/CMakeLists.txt b/drivers/sense/ubx_gnss/CMakeLists.txt new file mode 100644 index 00000000..021cf74a --- /dev/null +++ b/drivers/sense/ubx_gnss/CMakeLists.txt @@ -0,0 +1,14 @@ +# Copyright (c) 2023, CogniPilot Foundation +# SPDX-License-Identifier: Apache-2.0 + +zephyr_library_named(sense_ubx_gnss) + + +# we need to be able to include generated header files +zephyr_include_directories() + +zephyr_library_sources( + main.c + ) + +add_dependencies(sense_ubx_gnss synapse_protobuf) diff --git a/drivers/sense/ubx_gnss/Kconfig b/drivers/sense/ubx_gnss/Kconfig new file mode 100644 index 00000000..47dba22a --- /dev/null +++ b/drivers/sense/ubx_gnss/Kconfig @@ -0,0 +1,11 @@ +config SENSE_UBX_GNSS + bool "Use U-blox GNSS" + default n + select UBXLIB + help + This option enables U-blox GNSS driver. + + +module = UBX_GNSS +module-str = ubx_gnss +source "subsys/logging/Kconfig.template.log_config" diff --git a/drivers/sense/ubx_gnss/main.c b/drivers/sense/ubx_gnss/main.c new file mode 100644 index 00000000..9170741e --- /dev/null +++ b/drivers/sense/ubx_gnss/main.c @@ -0,0 +1,86 @@ +/* + * Copyright CogniPilot Foundation 2023 + * SPDX-License-Identifier: Apache-2.0 + */ +#include "ubxlib.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +uDeviceCfg_t gDeviceCfg; + +uNetworkCfgGnss_t gNetworkCfg = { + .type = U_NETWORK_TYPE_GNSS +}; + +LOG_MODULE_REGISTER(ubx_gnss, CONFIG_UBX_GNSS_LOG_LEVEL); + +#define MY_STACK_SIZE 8192 +#define MY_PRIORITY 6 + +void publish_gnss_data_zbus(uLocation_t* location) +{ + synapse_msgs_NavSatFix nav_sat_fix; + + nav_sat_fix.latitude = location->longitudeX1e7 / 1e6; + nav_sat_fix.longitude = location->longitudeX1e7 / 1e6; + nav_sat_fix.altitude = location->altitudeMillimetres / 1e3; + + // TODO Covariance + + zbus_chan_pub(&chan_out_nav_sat_fix, &nav_sat_fix, K_NO_WAIT); +} + +void sense_ubx_gnss_entry_point() +{ + int32_t errorCode; + + // Remove the line below if you want the log printouts from ubxlib + uPortLogOff(); + // Initiate ubxlib + uPortInit(); + uDeviceInit(); + // And the U-blox GNSS module + uDeviceHandle_t deviceHandle; + uDeviceGetDefaults(U_DEVICE_TYPE_GNSS, &gDeviceCfg); + + gDeviceCfg.transportCfg.cfgUart.uart = 0; + gDeviceCfg.transportCfg.cfgUart.baudRate = 38400; + gDeviceCfg.deviceCfg.cfgGnss.moduleType = U_GNSS_MODULE_TYPE_M8; + + errorCode = uDeviceOpen(&gDeviceCfg, &deviceHandle); + LOG_DBG("Opened the GNSS device %i\n", errorCode); + if (errorCode == 0) { + // Bring up the GNSS + errorCode = uNetworkInterfaceUp(deviceHandle, U_NETWORK_TYPE_GNSS, &gNetworkCfg); + if (errorCode == 0) { + while (true) { + uLocation_t location; + errorCode = uLocationGet(deviceHandle, U_LOCATION_TYPE_GNSS, + NULL, NULL, &location, NULL); + if (errorCode == 0) { + publish_gnss_data_zbus(&location); + } else if (errorCode == U_ERROR_COMMON_TIMEOUT) { + LOG_ERR("Location Timeout\n"); + } else { + LOG_ERR("Failed to get position: %d\n", errorCode); + } + } + } else { + LOG_ERR("Failed to bring up the GNSS: %d", errorCode); + } + uDeviceClose(deviceHandle, true); + } else { + LOG_ERR("Failed to initiate the module: %d", errorCode); + } +} + +K_THREAD_DEFINE(ubx_gnss_thread, MY_STACK_SIZE, + sense_ubx_gnss_entry_point, NULL, NULL, NULL, + MY_PRIORITY, 0, 0); diff --git a/drivers/synapse/zbus/channels.c b/drivers/synapse/zbus/channels.c index 7c3a40d2..7efb069a 100644 --- a/drivers/synapse/zbus/channels.c +++ b/drivers/synapse/zbus/channels.c @@ -178,3 +178,18 @@ ZBUS_CHAN_DEFINE(chan_out_odometry, // Name ), // observers ZBUS_MSG_INIT(0) // Initial value {0} ); + +ZBUS_CHAN_DEFINE(chan_out_nav_sat_fix, // Name + synapse_msgs_NavSatFix, // Message type + NULL, // Validator + NULL, // User Data + ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_ETHERNET) + listener_synapse_ethernet, +#endif +#if defined(CONFIG_SYNAPSE_UART) + listener_synapse_uart, +#endif + ), // observers + ZBUS_MSG_INIT(0) // Initial value {0} +); diff --git a/include/synapse/zbus/channels.h b/include/synapse/zbus/channels.h index 25caacf2..f9de1139 100644 --- a/include/synapse/zbus/channels.h +++ b/include/synapse/zbus/channels.h @@ -29,5 +29,6 @@ ZBUS_CHAN_DECLARE(chan_in_odometry); ZBUS_CHAN_DECLARE(chan_out_actuators); ZBUS_CHAN_DECLARE(chan_out_odometry); +ZBUS_CHAN_DECLARE(chan_out_nav_sat_fix); #endif // SYNAPSE_ZBUS_CHANNELS_H diff --git a/lib/control/diffdrive/main.c b/lib/control/diffdrive/main.c index 50ee47cc..d6b12352 100644 --- a/lib/control/diffdrive/main.c +++ b/lib/control/diffdrive/main.c @@ -16,6 +16,8 @@ #define MY_STACK_SIZE 1024 #define MY_PRIORITY 4 +LOG_MODULE_REGISTER(zbus_topic_list, LOG_LEVEL_INF); + enum control_mode_t { MODE_INIT = 0, MODE_MANUAL = 1, diff --git a/west.yml b/west.yml index da89e976..d33288ba 100644 --- a/west.yml +++ b/west.yml @@ -8,6 +8,8 @@ manifest: remotes: - name: zephyrproject-rtos url-base: https://github.com/zephyrproject-rtos + - name: cogniPilot + url-base: https://github.com/cogniPilot projects: - name: zephyr @@ -21,6 +23,7 @@ manifest: - mbedtls - hal_nxp - cmsis + - ubxlib - name: synapse_tinyframe url: https://github.com/CogniPilot/synapse_tinyframe clone-depth: 1 @@ -31,3 +34,8 @@ manifest: clone-depth: 1 revision: main path: modules/lib/synapse_protobuf + - name: ubxlib + remote: cogniPilot + path: ubxlib + revision: zephyr-3.4-compile-fix + clone-depth: 1