Sending protobuf messages from a test target to the other

worldcoin · Oct 5, 2021 · d408af9 · d408af9
1 parent 788d07b
commit d408af9
Show file tree

Hide file tree

Showing 11 changed files with 282 additions and 82 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -18,9 +18,9 @@ endif ()
 # or look for it in the parent directory
 if (DEFINED ENV{ZEPHYR_BASE})
     set(ZEPHYR_BASE $ENV{ZEPHYR_BASE})
-else()
+else ()
     set(ZEPHYR_BASE ${WORKSPACE_DIR}/zephyr)
-endif()
+endif ()
 
 # Get custom boards definitions from that repo
 list(APPEND BOARD_ROOT ${CMAKE_CURRENT_SOURCE_DIR})
@@ -30,11 +30,20 @@ project(orb-firmware)
 
 set(SOURCES_FILES
         src/main.c
-        src/canbus.c
+
+        src/messaging/canbus.c
+        src/messaging/messaging.c
 
         src/power_sequence/power_sequence.c
         src/fan/fan.c
-)
+        )
+
+# set(TEST_TARGET ON)
+
+if (TEST_TARGET)
+    zephyr_compile_options(-DTEST_TARGET=1)
+    list(APPEND SOURCES_FILES src/messaging/tests.c)
+endif ()
 
 set(INCLUDE_DIRS
         include
@@ -43,5 +52,5 @@ set(INCLUDE_DIRS
 target_sources(${TARGET} PRIVATE ${SOURCES_FILES})
 target_include_directories(${TARGET} PRIVATE ${INCLUDE_DIRS})
 
-zephyr_link_libraries(ORB_MCU_LIBRARIES)
+zephyr_link_libraries(ORB_MCU_LIBRARIES ORB_PROTOBUF_DEFINITIONS_LIB)
 
diff --git a/Kconfig b/Kconfig
@@ -5,6 +5,11 @@ comment "App-specific configuration options"
 # Shared libraries used on Orb
 config ORB_MCU_LIBRARIES_LIB
     default y
-    bool "Enable usage of Orb generic libraries"
+    bool "Import Orb libraries"
+
+config ORB_PROTOBUF_DEFINITIONS_LIB
+    default y
+    bool "Import Orb Protobuf definitions"
+    select NANOPB
 
 source "Kconfig.zephyr"
diff --git a/include/app_config.h b/include/app_config.h
@@ -0,0 +1,17 @@
+//
+// Created by Cyril on 05/10/2021.
+//
+
+#ifndef ORB_FIRMWARE_APP_MAIN_INCLUDE_APP_CONFIG_H
+#define ORB_FIRMWARE_APP_MAIN_INCLUDE_APP_CONFIG_H
+
+#ifndef CONFIG_CAN_ADDRESS_MCU
+#define CONFIG_CAN_ADDRESS_MCU 0x01
+#endif
+
+#ifndef CONFIG_CAN_ADDRESS_JETSON
+#define CONFIG_CAN_ADDRESS_JETSON 0x80
+#endif
+
+
+#endif //ORB_FIRMWARE_APP_MAIN_INCLUDE_APP_CONFIG_H
diff --git a/prj.conf b/prj.conf
@@ -53,4 +53,6 @@ CONFIG_REGULATOR=y
 CONFIG_REGULATOR_FIXED=y
 CONFIG_REGULATOR_FIXED_INIT_PRIORITY=75
 
+# Our custom modules
 CONFIG_ORB_MCU_LIBRARIES_LIB=y
+CONFIG_ORB_PROTOBUF_DEFINITIONS_LIB=y
diff --git a/src/main.c b/src/main.c
@@ -1,12 +1,16 @@
 #include <zephyr.h>
 #include <device.h>
 #include <drivers/gpio.h>
+#include "power_sequence/power_sequence.h"
+#include "fan/fan.h"
+#include "messaging/messaging.h"
+
 #include <logging/log.h>
-#include <canbus.h>
 LOG_MODULE_REGISTER(main);
 
-#include "power_sequence/power_sequence.h"
-#include "fan/fan.h"
+#if TEST_TARGET
+#include "messaging/tests.h"
+#endif
 
 #define BUTTON_PRESS_TIME_MS 5000
 #define BUTTON_SAMPLE_PERIOD_MS 10
@@ -69,5 +73,13 @@ main(void)
         return;
     }
 
-    canbus_init();
+    messaging_init();
+
+    // the target is now up and running
+
+#if TEST_TARGET
+    LOG_WRN("Running test target");
+
+    tests_init();
+#endif
 }
diff --git a/src/canbus.c → src/messaging/canbus.c b/src/canbus.c → src/messaging/canbus.c
@@ -3,23 +3,28 @@
 //
 
 #include "canbus.h"
+#include <logging/log.h>
+LOG_MODULE_REGISTER(canbus);
 
+#include "mcu_messaging.pb.h"
+#include "messaging.h"
 #include <device.h>
 #include <zephyr.h>
 #include <canbus/isotp.h>
+#include <pb.h>
+#include <pb_decode.h>
 
-#include <logging/log.h>
-LOG_MODULE_REGISTER(canbus);
+#include <app_config.h>
 
-#define RX_ADDR         0x01
-#define TX_ADDR         0x80
+#define RX_ADDR         CONFIG_CAN_ADDRESS_MCU     // MCU
+#define TX_ADDR         CONFIG_CAN_ADDRESS_JETSON  // Jetson address
 
 #define RX_BUF_SIZE     256
 
 const struct device *can_dev;
 const struct isotp_fc_opts flow_control_opts = {.bs = 8, .stmin = 0};
 
-#define RX_THREAD_STACK_SIZE 1024
+#define RX_THREAD_STACK_SIZE 2048
 #define RX_THREAD_PRIORITY 5
 
 K_THREAD_STACK_DEFINE(rx_thread_stack, RX_THREAD_STACK_SIZE);
@@ -36,27 +41,20 @@ const struct isotp_msg_id tx_addr = {
     .use_ext_addr = 0
 };
 
-static void
+_Noreturn static void
 rx_thread(void *arg1, void *arg2, void *arg3)
 {
-    struct net_buf *buf;
+    struct net_buf *buf = NULL;
     int ret, rem_len;
     struct isotp_recv_ctx recv_ctx;
     uint8_t rx_buffer[RX_BUF_SIZE] = {0};
     size_t wr_idx = 0;
 
     ret = isotp_bind(&recv_ctx, can_dev,
-                         &tx_addr, &rx_addr,
-                         &flow_control_opts, K_FOREVER);
-    if (ret != ISOTP_N_OK)
-    {
-        LOG_ERR("Failed to bind to rx ID %d [%d]",
-                rx_addr.std_id, ret);
-        return;
-    }
+                     &tx_addr, &rx_addr,
+                     &flow_control_opts, K_FOREVER);
+    __ASSERT(ret == ISOTP_N_OK, "Failed to bind to rx ID %d [%d]", rx_addr.std_id, ret);
 
-#pragma clang diagnostic push
-#pragma ide diagnostic ignored "EndlessLoop"
     while (1) {
         wr_idx = 0;
         memset(rx_buffer, 0, sizeof(rx_buffer));
@@ -72,32 +70,39 @@ rx_thread(void *arg1, void *arg2, void *arg3)
                 break;
             }
 
-            if (wr_idx + buf->len < sizeof(rx_buffer))
-            {
+            if (wr_idx + buf->len < sizeof(rx_buffer)) {
                 memcpy(&rx_buffer[wr_idx], buf->data, buf->len);
                 wr_idx += buf->len;
-            }
-            else
-            {
+            } else {
                 // todo push data and reset rx buf
             }
 
-            net_buf_unref(buf);
+            if (buf != NULL)
+            {
+                net_buf_unref(buf);
+            }
         } while (rem_len);
 
-        if (rem_len == ISOTP_N_OK)
-        {
+        if (rem_len == ISOTP_N_OK) {
             LOG_INF("Got %d bytes in total", wr_idx);
-        }
-        else
-        {
-            LOG_WRN("Error while receiving data, resetting RX buf");
+
+            pb_istream_t stream = pb_istream_from_buffer(rx_buffer, wr_idx);
+            McuMessage data = {0};
+
+            bool decoded = pb_decode(&stream, McuMessage_fields, &data);
+            if (decoded) {
+                messaging_push(&data);
+            } else {
+                LOG_ERR("Error parsing data, discarding");
+            }
+
+        } else {
+            LOG_DBG("Data not received: %d", rem_len);
         }
     }
-#pragma clang diagnostic pop
 }
 
-static void
+__unused static void
 send_complete_cb(int error_nr, void *arg)
 {
     ARG_UNUSED(arg);
@@ -110,12 +115,13 @@ canbus_send(const char *data, size_t len)
     static struct isotp_send_ctx send_ctx;
     memset(&send_ctx, 0, sizeof(send_ctx));
 
+    // canbus_send is currently in blocking mode (no callback specified)
+    // to be modified when needed, see `canbus_send` doc
     int ret = isotp_send(&send_ctx, can_dev,
                          data, len,
                          &tx_addr, &rx_addr,
                          NULL, NULL);
-    if (ret != ISOTP_N_OK)
-    {
+    if (ret != ISOTP_N_OK) {
         LOG_ERR("Error while sending data to ID %d [%d]",
                 tx_addr.std_id, ret);
 
@@ -125,56 +131,25 @@ canbus_send(const char *data, size_t len)
     return RET_SUCCESS;
 }
 
-void
+ret_code_t
 canbus_init(void)
 {
     can_dev = device_get_binding(DT_CHOSEN_ZEPHYR_CAN_PRIMARY_LABEL);
-    if (!can_dev)
-    {
+    if (!can_dev) {
         LOG_ERR("CAN: Device driver not found.");
-        return;
+        return RET_ERROR_NOT_FOUND;
     }
 
     k_tid_t tid = k_thread_create(&rx_thread_data, rx_thread_stack,
                                   K_THREAD_STACK_SIZEOF(rx_thread_stack),
                                   rx_thread, NULL, NULL, NULL,
                                   RX_THREAD_PRIORITY, 0, K_NO_WAIT);
-    if (!tid)
-    {
+    if (!tid) {
         LOG_ERR("ERROR spawning rx thread");
-        return;
+        return RET_ERROR_NO_MEM;
     }
 
     LOG_INF("CAN bus init ok: TX addr: 0x%x, RX addr: 0x%x", tx_addr.std_id, rx_addr.std_id);
-}
 
-const char tx_data_large[] =
-"========================================\n"
-    "|   ____  ___  ____       ____  ____   |\n"
-    "|  |_  _|/ __||    | ___ |_  _||  _ \\  |\n"
-    "|   _||_ \\__ \\| || | ___   ||  | ___/  |\n"
-    "|  |____||___/|____|       ||  |_|     |\n"
-    "========================================\n";
-
-#ifdef TEST_TARGET
-__NO_RETURN void
-can_send_test()
-{
-    int packet = 0;
-    while(1)
-    {
-        k_msleep(2000);
-
-        ret_code_t err_code = canbus_send(tx_data_large, sizeof(tx_data_large));
-        LOG_INF("Sent data #%d [%u]", packet, err_code);
-
-        packet++;
-    }
+    return RET_SUCCESS;
 }
-/* size of stack area used by each thread */
-#define STACKSIZE 1024
-/* scheduling priority used by each thread */
-#define PRIORITY 7
-K_THREAD_DEFINE(send_test, STACKSIZE, can_send_test, NULL, NULL, NULL,
-                PRIORITY, 0, 0);
-#endif
diff --git a/include/canbus.h → src/messaging/canbus.h b/include/canbus.h → src/messaging/canbus.h
@@ -10,7 +10,7 @@
 
 /// Send chunk of data over CAN bus
 /// ISO-TP protocol is used to ensure flow-control
-/// Non-blocking mode
+/// ⚠️ Blocking mode
 /// \param data
 /// \param len
 /// \return
@@ -19,7 +19,7 @@
 ret_code_t
 canbus_send(const char *data, size_t len);
 
-void
+ret_code_t
 canbus_init(void);
 
 #endif //ORB_FIRMWARE_APP_MAIN_SRC_CANBUS_H