Add UBX GNSS Driver

Signed-off-by: Peter van der Perk <[email protected]>

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"

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>;
 };

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)

drivers/sense/Kconfig

@@ -5,5 +5,6 @@ menu "Sense"
 
 rsource "vesc_can_status/Kconfig"
 rsource "ina226/Kconfig"
+rsource "ubx_gnss/Kconfig"
 
 endmenu

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)

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"

drivers/sense/ubx_gnss/main.c

@@ -0,0 +1,86 @@
+/*
+ * Copyright CogniPilot Foundation 2023
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#include "ubxlib.h"
+#include <stdio.h>
+#include <string.h>
+#include <synapse/zbus/channels.h>
+#include <time.h>
+#include <zephyr/device.h>
+#include <zephyr/drivers/sensor.h>
+#include <zephyr/kernel.h>
+#include <zephyr/logging/log.h>
+#include <zephyr/shell/shell.h>
+
+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);

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}
+);

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

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,

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