Implements zbus topic hz/info/echo, improve sched.

This schedules a listener once to echo the topic. Autocomplete works on the zephyr shell for topics. Also adds topic info and hz commmands. While checking topic hz, found scheduling issues and resolved using work queues. Switched estimator and control to polling. Remove synapse uart node. Closes #93, Closes #96, Closes #97, Closes #98 Closes #99, Closes #100 Co-authored-by: Benjamin Perseghetti <[email protected]> Signed-off-by: James Goppert <[email protected]>
CogniPilot · Aug 20, 2023 · 64327a6 · 64327a6
1 parent bee6f1a
commit 64327a6
Show file tree

Hide file tree

Showing 43 changed files with 1,107 additions and 469 deletions.
diff --git a/app/elm4/boards/native_posix.conf b/app/elm4/boards/native_posix.conf
@@ -7,7 +7,6 @@ CONFIG_UART_NATIVE_POSIX_PORT_1_ENABLE=n
 
 CONFIG_DREAM_SITL=y
 CONFIG_SYNAPSE_ETHERNET=y
-CONFIG_SYNAPSE_UART=n
 
 CONFIG_NEWLIB_LIBC=n
 CONFIG_EXTERNAL_LIBC=y

diff --git a/app/elm4/prj.conf b/app/elm4/prj.conf
@@ -14,7 +14,6 @@ CONFIG_ZBUS_CHANNEL_NAME=y
 CONFIG_ZBUS_OBSERVER_NAME=y
 
 CONFIG_SYNAPSE_ZBUS=y
-CONFIG_SYNAPSE_UART=y
 CONFIG_SYNAPSE_ETHERNET=y
 CONFIG_SYNAPSE_TINYFRAME=y
 CONFIG_SYNAPSE_PROTOBUF=y

diff --git a/app/mrbuggy3/boards/native_posix.conf b/app/mrbuggy3/boards/native_posix.conf
@@ -1,13 +1,14 @@
 CONFIG_ETH_NATIVE_POSIX=y
 CONFIG_TEST_RANDOM_GENERATOR=y
-CONFIG_SYS_CLOCK_TICKS_PER_SEC=1000
+CONFIG_SYS_CLOCK_TICKS_PER_SEC=100000
 CONFIG_NATIVE_POSIX_SLOWDOWN_TO_REAL_TIME=n
 
+CONFIG_DREAM_SITL_LOG_LEVEL_DBG=y
+
 CONFIG_UART_NATIVE_POSIX_PORT_1_ENABLE=n
 
 CONFIG_DREAM_SITL=y
 CONFIG_SYNAPSE_ETHERNET=y
-CONFIG_SYNAPSE_UART=n
 
 # turn off driver interfaces, handled by dream
 CONFIG_SENSE_IMU=n

diff --git a/app/mrbuggy3/prj.conf b/app/mrbuggy3/prj.conf
@@ -4,18 +4,21 @@ CONFIG_SERIAL=y
 CONFIG_CONSOLE=y
 CONFIG_NANOPB=y
 CONFIG_LOG=y
-CONFIG_LOG_MODE_MINIMAL=y
+CONFIG_LOG_MODE_MINIMAL=n
 CONFIG_BOOT_BANNER=y
 CONFIG_MAIN_THREAD_PRIORITY=5
 
+CONFIG_SYSTEM_WORKQUEUE_PRIORITY=-2
+CONFIG_SYS_CLOCK_TICKS_PER_SEC=100000
+
 CONFIG_ZBUS=y
 CONFIG_ZBUS_LOG_LEVEL_INF=y
 CONFIG_ZBUS_CHANNEL_NAME=y
 CONFIG_ZBUS_OBSERVER_NAME=y
 CONFIG_ZBUS_STRUCTS_ITERABLE_ACCESS=y
 
 CONFIG_SYNAPSE_ZBUS=y
-CONFIG_SYNAPSE_UART=y
+CONFIG_SYNAPSE_TOPIC=y
 CONFIG_SYNAPSE_ETHERNET=y
 CONFIG_SYNAPSE_TINYFRAME=y
 CONFIG_SYNAPSE_PROTOBUF=y
@@ -35,6 +38,9 @@ CONFIG_SENSE_IMU_LOG_LEVEL_DBG=n
 CONFIG_SENSE_MAG=y
 CONFIG_SENSE_MAG_LOG_LEVEL_DBG=n
 
+CONFIG_SENSE_BARO=n
+CONFIG_SENSE_BARO_LOG_LEVEL_DBG=n
+
 CONFIG_SENSE_POWER=y
 CONFIG_SENSE_POWER_LOG_LEVEL_DBG=n
 

diff --git a/app/rddrone/boards/native_posix.conf b/app/rddrone/boards/native_posix.conf
@@ -7,7 +7,6 @@ CONFIG_UART_NATIVE_POSIX_PORT_1_ENABLE=n
 
 CONFIG_DREAM_SITL=y
 CONFIG_SYNAPSE_ETHERNET=y
-CONFIG_SYNAPSE_UART=n
 
 CONFIG_NEWLIB_LIBC=n
 CONFIG_EXTERNAL_LIBC=y

diff --git a/app/rddrone/prj.conf b/app/rddrone/prj.conf
@@ -15,7 +15,6 @@ CONFIG_ZBUS_OBSERVER_NAME=y
 CONFIG_ZBUS_STRUCTS_ITERABLE_ACCESS=y
 
 CONFIG_SYNAPSE_ZBUS=y
-CONFIG_SYNAPSE_UART=y
 CONFIG_SYNAPSE_ETHERNET=y
 CONFIG_SYNAPSE_TINYFRAME=y
 CONFIG_SYNAPSE_PROTOBUF=y

diff --git a/include/synapse/zbus/common.h b/include/synapse/zbus/common.h
@@ -14,6 +14,14 @@
 
 #include <synapse/zbus/channels.h>
 
+#define ZBUS_LISTENER_DEFINE_DISABLED(_name, _cb)           \
+    _ZBUS_STRUCT_DECLARE(zbus_observer,                     \
+        _name)                                              \
+        = { ZBUS_OBSERVER_NAME_INIT(_name) /* Name field */ \
+                  .enabled                                  \
+              = false,                                      \
+              .queue = NULL, .callback = (_cb) }
+
 #define TOPIC_LISTENER(CHANNEL, CLASS)                                           \
     static TF_Result CHANNEL##_Listener(TinyFrame* tf, TF_Msg* frame)            \
     {                                                                            \

diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt
@@ -2,6 +2,7 @@
 
 add_subdirectory(actuate)
 add_subdirectory(control)
+add_subdirectory(core)
 add_subdirectory(dream)
 add_subdirectory(estimate)
 add_subdirectory(sense)

diff --git a/lib/Kconfig b/lib/Kconfig
@@ -5,6 +5,7 @@ menu "Libraries"
 
 rsource "actuate/Kconfig"
 rsource "control/Kconfig"
+rsource "core/Kconfig"
 rsource "dream/Kconfig"
 rsource "estimate/Kconfig"
 rsource "sense/Kconfig"

diff --git a/lib/actuate/pwm/main.c b/lib/actuate/pwm/main.c
@@ -101,7 +101,7 @@ void actuator_pwm_entry_point()
     }
 }
 
-K_THREAD_DEFINE(actuator_pwm_thread, MY_STACK_SIZE,
+K_THREAD_DEFINE(actuator_pwm, MY_STACK_SIZE,
     actuator_pwm_entry_point, NULL, NULL, NULL,
     MY_PRIORITY, 0, 0);
 

diff --git a/lib/actuate/vesc_can/main.c b/lib/actuate/vesc_can/main.c
@@ -158,7 +158,7 @@ void actuate_vesc_can_entry_point()
     }
 }
 
-K_THREAD_DEFINE(actuate_vesc_can_thread, MY_STACK_SIZE,
+K_THREAD_DEFINE(actuate_vesc_can, MY_STACK_SIZE,
     actuate_vesc_can_entry_point, NULL, NULL, NULL,
     MY_PRIORITY, 0, 0);
 

diff --git a/lib/control/ackermann/main.c b/lib/control/ackermann/main.c
@@ -17,7 +17,7 @@
 
 LOG_MODULE_REGISTER(control_ackermann, CONFIG_CONTROL_ACKERMANN_LOG_LEVEL);
 
-#define MY_STACK_SIZE 1024
+#define MY_STACK_SIZE 3072
 #define MY_PRIORITY 4
 
 enum control_mode_t {
@@ -27,9 +27,10 @@ enum control_mode_t {
     MODE_CMD_VEL = 3,
 };
 typedef enum control_mode_t control_mode_t;
-char* g_mode_name[4] = { "init", "manual", "auto", "cmd_vel" };
+static char* g_mode_name[4] = { "init", "manual", "auto", "cmd_vel" };
+static int32_t g_seq = 0;
 
-control_mode_t g_mode = { MODE_INIT };
+control_mode_t g_mode = MODE_INIT;
 bool g_armed = false;
 static synapse_msgs_Odometry g_pose = synapse_msgs_Odometry_init_default;
 static synapse_msgs_Twist g_cmd_vel = synapse_msgs_Twist_init_default;
@@ -96,7 +97,7 @@ void mixer()
 {
 
     // given cmd_vel, compute actuators
-    synapse_msgs_Actuators actuators = synapse_msgs_Actuators_init_default;
+    synapse_msgs_Actuators msg = synapse_msgs_Actuators_init_default;
 
     double turn_angle = 0;
     double omega_fwd = 0;
@@ -128,15 +129,26 @@ void mixer()
         omega_fwd = 0;
         turn_angle = 0;
     }
-    actuators.position_count = 1;
-    actuators.velocity_count = 1;
-    actuators.normalized_count = 2;
-    actuators.position[0] = turn_angle;
-    actuators.velocity[0] = omega_fwd;
+
+    msg.has_header = true;
+    int64_t uptime_ticks = k_uptime_ticks();
+    int64_t sec = uptime_ticks / CONFIG_SYS_CLOCK_TICKS_PER_SEC;
+    int32_t nanosec = (uptime_ticks - sec * CONFIG_SYS_CLOCK_TICKS_PER_SEC) * 1e9 / CONFIG_SYS_CLOCK_TICKS_PER_SEC;
+    msg.header.seq = g_seq++;
+    msg.header.has_stamp = true;
+    msg.header.stamp.sec = sec;
+    msg.header.stamp.nanosec = nanosec;
+    strncpy(msg.header.frame_id, "map", sizeof(msg.header.frame_id) - 1);
+
+    msg.position_count = 1;
+    msg.velocity_count = 1;
+    msg.normalized_count = 2;
+    msg.position[0] = turn_angle;
+    msg.velocity[0] = omega_fwd;
 #ifdef CONFIG_BUGGY3_MOTOR_ENB_REQUIRED
-    actuators.normalized[0] = -1;
+    msg.normalized[0] = -1;
 #endif
-    zbus_chan_pub(&chan_out_actuators, &actuators, K_NO_WAIT);
+    zbus_chan_pub(&chan_out_actuators, &msg, K_NO_WAIT);
 }
 
 void stop()

diff --git a/lib/control/diffdrive/main.c b/lib/control/diffdrive/main.c
@@ -141,7 +141,7 @@ void diffdrive_entry_point(void* p1, void* p2, void* p3)
     }
 }
 
-K_THREAD_DEFINE(diffdrive_thread, MY_STACK_SIZE,
+K_THREAD_DEFINE(diffdrive, MY_STACK_SIZE,
     diffdrive_entry_point, NULL, NULL, NULL,
     MY_PRIORITY, 0, 0);
 

diff --git a/lib/core/CMakeLists.txt b/lib/core/CMakeLists.txt
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: Apache-2.0
+
+add_subdirectory_ifdef(CONFIG_CORE_WORKQUEUES workqueues)
diff --git a/lib/core/Kconfig b/lib/core/Kconfig
@@ -0,0 +1,8 @@
+# Copyright (c) 2023 CogniPilot Foundation
+# SPDX-License-Identifier: Apache-2.0
+
+menu "Core"
+
+rsource "workqueues/Kconfig"
+
+endmenu
diff --git a/lib/synapse/uart/CMakeLists.txt → lib/core/workqueues/CMakeLists.txt b/lib/synapse/uart/CMakeLists.txt → lib/core/workqueues/CMakeLists.txt
@@ -1,13 +1,12 @@
 # Copyright (c) 2023, CogniPilot Foundation
 # SPDX-License-Identifier: Apache-2.0
 
-zephyr_library_named(synapse_uart)
+zephyr_library_named(core_workqueues)
+
 
 # we need to be able to include generated header files
 zephyr_include_directories()
 
 zephyr_library_sources(
   main.c
   )
-
-add_dependencies(synapse_uart synapse_protobuf)
diff --git a/lib/core/workqueues/Kconfig b/lib/core/workqueues/Kconfig
@@ -0,0 +1,15 @@
+# Copyright (c) 2023, CogniPilot Foundation
+# SPDX-License-Identifier: Apache-2.0
+menuconfig CORE_WORKQUEUES
+  bool "Enable core work queues"
+  default y
+  help
+    This option enables the system work queues
+
+if CORE_WORKQUEUES
+
+module = CORE_WORKQUEUES
+module-str = core_workqueues
+source "subsys/logging/Kconfig.template.log_config"
+
+endif # CORE_WORKQUEUES
diff --git a/lib/core/workqueues/main.c b/lib/core/workqueues/main.c
@@ -0,0 +1,58 @@
+/*
+ * Copyright CogniPilot Foundation 2023
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#include <zephyr/kernel.h>
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(core_workqueues, CONFIG_CORE_WORKQUEUES_LOG_LEVEL);
+
+#define THREAD_STACK_SIZE 2048
+#define THREAD_PRIORITY 6
+
+#define LOW_PRIORITY_STACK_SIZE 2048
+#define LOW_PRIORITY_PRIORITY 0
+
+#define HIGH_PRIORITY_STACK_SIZE 2048
+#define HIGH_PRIORITY_PRIORITY -1
+
+K_THREAD_STACK_DEFINE(high_priority_stack_area, HIGH_PRIORITY_STACK_SIZE);
+K_THREAD_STACK_DEFINE(low_priority_stack_area, LOW_PRIORITY_STACK_SIZE);
+
+struct k_work_q g_high_priority_work_q, g_low_priority_work_q;
+
+int core_workqueues_entry_point(void)
+{
+    // high priority
+    k_work_queue_init(&g_high_priority_work_q);
+    struct k_work_queue_config high_priority_cfg = {
+        .name = "high_priority_q",
+        .no_yield = true
+    };
+    k_work_queue_start(
+        &g_high_priority_work_q,
+        high_priority_stack_area,
+        K_THREAD_STACK_SIZEOF(high_priority_stack_area),
+        HIGH_PRIORITY_PRIORITY,
+        &high_priority_cfg);
+
+    // high priority
+    k_work_queue_init(&g_low_priority_work_q);
+    struct k_work_queue_config low_priority_cfg = {
+        .name = "low_priority_q",
+        .no_yield = false
+    };
+    k_work_queue_start(
+        &g_low_priority_work_q,
+        low_priority_stack_area,
+        K_THREAD_STACK_SIZEOF(low_priority_stack_area),
+        LOW_PRIORITY_PRIORITY,
+        &low_priority_cfg);
+    return 0;
+}
+
+K_THREAD_DEFINE(core_workqueues, THREAD_STACK_SIZE,
+    core_workqueues_entry_point, NULL, NULL, NULL,
+    THREAD_PRIORITY, 0, 0);
+
+// vi: ts=4 sw=4 et
diff --git a/lib/dream/sitl/zephyr_main.c b/lib/dream/sitl/zephyr_main.c
@@ -109,14 +109,13 @@ static void zephyr_sim_entry_point(void)
         int32_t delta_nsec = g_sim_clock.sim.nanosec - ts_board.tv_nsec;
         int64_t wait_msec = delta_sec * 1e3 + delta_nsec * 1e-6;
 
-        LOG_DBG("sim: sec %lld nsec %d\n",
-            g_sim_clock.sim.sec, g_sim_clock.sim.nanosec);
-        LOG_DBG("board: sec %lld nsec %ld\n",
-            ts_board.tv_sec, ts_board.tv_nsec);
-        LOG_DBG("wait: msec %lld\n", wait_msec);
-
         // sleep to match clocks
         if (wait_msec > 0) {
+            LOG_DBG("sim: sec %lld nsec %d\n",
+                g_sim_clock.sim.sec, g_sim_clock.sim.nanosec);
+            LOG_DBG("board: sec %ld nsec %ld\n",
+                ts_board.tv_sec, ts_board.tv_nsec);
+            LOG_DBG("wait: msec %lld\n", wait_msec);
             k_msleep(wait_msec);
         } else {
             struct timespec request, remaining;
@@ -128,7 +127,7 @@ static void zephyr_sim_entry_point(void)
 }
 
 // zephyr threads
-K_THREAD_DEFINE(zephyr_sim_thread, MY_STACK_SIZE, zephyr_sim_entry_point,
+K_THREAD_DEFINE(zephyr_sim, MY_STACK_SIZE, zephyr_sim_entry_point,
     NULL, NULL, NULL, MY_PRIORITY, 0, 0);
 
 // vi: ts=4 sw=4 et
diff --git a/lib/estimate/attitude/main.c b/lib/estimate/attitude/main.c
@@ -103,6 +103,7 @@ static void estimate_attitude_entry_point(void* p1, void* p2, void* p3)
     // variables
     double dt = 1.0 / 1.0;
     bool initialized = false;
+    int32_t seq = 0;
 
     // parameters
     const double decl = 0;
@@ -292,6 +293,16 @@ static void estimate_attitude_entry_point(void* p1, void* p2, void* p3)
             strncpy(
                 ctx.pub_odometry.child_frame_id,
                 child_frame_id, sizeof(ctx.pub_odometry.child_frame_id) - 1);
+
+            ctx.pub_odometry.has_header = true;
+            int64_t uptime_ticks = k_uptime_ticks();
+            int64_t sec = uptime_ticks / CONFIG_SYS_CLOCK_TICKS_PER_SEC;
+            int32_t nanosec = (uptime_ticks - sec * CONFIG_SYS_CLOCK_TICKS_PER_SEC) * 1e9 / CONFIG_SYS_CLOCK_TICKS_PER_SEC;
+            ctx.pub_odometry.header.seq = seq++;
+            ctx.pub_odometry.header.has_stamp = true;
+            ctx.pub_odometry.header.stamp.sec = sec;
+            ctx.pub_odometry.header.stamp.nanosec = nanosec;
+
             ctx.pub_odometry.has_pose = true;
             ctx.pub_odometry.pose.has_pose = true;
             ctx.pub_odometry.pose.pose.has_orientation = true;
@@ -314,7 +325,7 @@ static void estimate_attitude_entry_point(void* p1, void* p2, void* p3)
     }
 }
 
-K_THREAD_DEFINE(estimate_attitude_thread, MY_STACK_SIZE,
+K_THREAD_DEFINE(estimate_attitude, MY_STACK_SIZE,
     estimate_attitude_entry_point, NULL, NULL, NULL,
     MY_PRIORITY, 0, 0);