diff --git a/app/elm4/boards/native_posix.conf b/app/elm4/boards/native_posix.conf index 1e285976..9953e856 100644 --- 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 index 9dab819c..9d591bc0 100644 --- 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 index e048bafa..e4fb1231 100644 --- 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 index 06e23480..45d20a6a 100644 --- a/app/mrbuggy3/prj.conf +++ b/app/mrbuggy3/prj.conf @@ -4,10 +4,13 @@ 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 @@ -15,7 +18,7 @@ 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 index 1e285976..9953e856 100644 --- 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 index b35ca0b2..97b8a923 100644 --- 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 index 50abfd78..f7707f8b 100644 --- a/include/synapse/zbus/common.h +++ b/include/synapse/zbus/common.h @@ -14,6 +14,14 @@ #include +#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 index f9650a2d..40ca9cc2 100644 --- 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 index 0d11d126..13e42ae2 100644 --- 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 index 5de71699..33102c56 100644 --- 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 index 7d52f791..a181c078 100644 --- 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 index 7be524d5..df79bd82 100644 --- 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 index a5bf6761..5d95ce4d 100644 --- 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 new file mode 100644 index 00000000..f4c35f3d --- /dev/null +++ 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 new file mode 100644 index 00000000..cbd9549f --- /dev/null +++ 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 b/lib/core/workqueues/CMakeLists.txt similarity index 71% rename from lib/synapse/uart/CMakeLists.txt rename to lib/core/workqueues/CMakeLists.txt index 201cae12..5da29a53 100644 --- a/lib/synapse/uart/CMakeLists.txt +++ b/lib/core/workqueues/CMakeLists.txt @@ -1,7 +1,8 @@ # 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() @@ -9,5 +10,3 @@ 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 new file mode 100644 index 00000000..317d3048 --- /dev/null +++ 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 new file mode 100644 index 00000000..f64c3d23 --- /dev/null +++ b/lib/core/workqueues/main.c @@ -0,0 +1,58 @@ +/* + * Copyright CogniPilot Foundation 2023 + * SPDX-License-Identifier: Apache-2.0 + */ +#include +#include + +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 index f36548b2..45abc297 100644 --- 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 index 39393b16..6404b1c9 100644 --- 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); diff --git a/lib/estimate/rover2d/main.c b/lib/estimate/rover2d/main.c index 0ab2f7af..ab34593f 100644 --- a/lib/estimate/rover2d/main.c +++ b/lib/estimate/rover2d/main.c @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -29,18 +30,18 @@ typedef struct ctx_ { synapse_msgs_WheelOdometry sub_wheel_odometry; synapse_msgs_Actuators sub_actuators; synapse_msgs_Odometry pub_odometry; - bool wheel_odometry_updated; - bool actuators_updated; + volatile bool actuators_updated; bool initialized; double x[3]; } ctx_t; +struct k_poll_signal signal; + // private initialization static ctx_t ctx = { .sub_wheel_odometry = synapse_msgs_WheelOdometry_init_default, .sub_actuators = synapse_msgs_Actuators_init_default, .pub_odometry = synapse_msgs_Odometry_init_default, - .wheel_odometry_updated = false, .actuators_updated = false, .initialized = false, .x = { 0 }, @@ -50,7 +51,7 @@ void listener_estimate_rover2d_callback(const struct zbus_channel* chan) { if (chan == &chan_out_wheel_odometry) { ctx.sub_wheel_odometry = *(synapse_msgs_WheelOdometry*)(chan->message); - ctx.wheel_odometry_updated = true; + k_poll_signal_raise(&signal, 0x1337); // LOG_DBG("wheel odometry updated"); } else if (chan == &chan_out_actuators) { ctx.sub_actuators = *(synapse_msgs_Actuators*)(chan->message); @@ -88,31 +89,42 @@ void handle_update(double* x1) } } -ZBUS_LISTENER_DEFINE(listener_estimate_rover2d, listener_estimate_rover2d_callback); +ZBUS_LISTENER_DEFINE_DISABLED(listener_estimate_rover2d, listener_estimate_rover2d_callback); static void estimate_rover2d_entry_point(void* p1, void* p2, void* p3) { // LOG_DBG("started"); // variables - double dt = 1.0 / 200.0; - double rotation = 0; + int32_t seq = 0; double rotation_last = 0; + k_poll_signal_init(&signal); + + struct k_poll_event events[1] = { + K_POLL_EVENT_INITIALIZER(K_POLL_TYPE_SIGNAL, + K_POLL_MODE_NOTIFY_ONLY, + &signal), + }; + + // enable the zbus listener + int ret = zbus_obs_set_enable(&listener_estimate_rover2d, true); + if (ret != 0) { + LOG_ERR("could not enable observer: %d", ret); + } // estimator state and sqrt covariance while (true) { - // sleep to set rate - k_usleep(dt * 1e6); - - // continue if no new data - if (!ctx.wheel_odometry_updated) + int ret = k_poll(events, 1, K_MSEC(1000)); + events[0].signal->signaled = 0; + events[0].state = K_POLL_STATE_NOT_READY; + if (ret != 0) { + LOG_ERR("not receiving wheel odometry"); + k_usleep(1000); continue; + } // get data - if (ctx.wheel_odometry_updated) { - rotation = ctx.sub_wheel_odometry.rotation; - // LOG_DBG("rotation: %10.4f", rotation); - } + double rotation = ctx.sub_wheel_odometry.rotation; // wait for valid steering angle and odometry to initialize if (!ctx.initialized) { @@ -127,7 +139,7 @@ static void estimate_rover2d_entry_point(void* p1, void* p2, void* p3) } /* predict:(x0[3],delta,u,l)->(x1[3]) */ - if (ctx.wheel_odometry_updated) { + { // LOG_DBG("predict"); #ifdef CONFIG_DREAM_SITL @@ -171,6 +183,17 @@ static void estimate_rover2d_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; @@ -184,16 +207,16 @@ static void estimate_rover2d_entry_point(void* p1, void* p2, void* p3) ctx.pub_odometry.pose.pose.orientation.y = 0; ctx.pub_odometry.pose.pose.orientation.z = sin(theta / 2); ctx.pub_odometry.pose.pose.orientation.w = cos(theta / 2); + zbus_chan_pub(&chan_out_odometry, &ctx.pub_odometry, K_NO_WAIT); } log_x(ctx.x); // set data as old now - ctx.wheel_odometry_updated = false; ctx.actuators_updated = false; } } -K_THREAD_DEFINE(estimate_rover2d_thread, MY_STACK_SIZE, estimate_rover2d_entry_point, NULL, NULL, NULL, MY_PRIORITY, 0, 0); +K_THREAD_DEFINE(estimate_rover2d, MY_STACK_SIZE, estimate_rover2d_entry_point, NULL, NULL, NULL, MY_PRIORITY, 0, 0); /* vi: ts=4 sw=4 et */ diff --git a/lib/sense/CMakeLists.txt b/lib/sense/CMakeLists.txt index 6d27189a..875f410c 100644 --- a/lib/sense/CMakeLists.txt +++ b/lib/sense/CMakeLists.txt @@ -1,8 +1,9 @@ # SPDX-License-Identifier: Apache-2.0 +add_subdirectory_ifdef(CONFIG_SENSE_BARO baro) add_subdirectory_ifdef(CONFIG_SENSE_IMU imu) add_subdirectory_ifdef(CONFIG_SENSE_MAG mag) -add_subdirectory_ifdef(CONFIG_SENSE_WHEEL_ODOMETRY wheel_odometry) add_subdirectory_ifdef(CONFIG_SENSE_POWER power) add_subdirectory_ifdef(CONFIG_SENSE_UBX_GNSS ubx_gnss) +add_subdirectory_ifdef(CONFIG_SENSE_WHEEL_ODOMETRY wheel_odometry) add_subdirectory_ifdef(CONFIG_SYNAPSE_VESC_CAN_STATUS vesc_can_status) diff --git a/lib/sense/Kconfig b/lib/sense/Kconfig index 9d835049..1a177eb8 100644 --- a/lib/sense/Kconfig +++ b/lib/sense/Kconfig @@ -3,6 +3,7 @@ menu "Sense" +rsource "baro/Kconfig" rsource "imu/Kconfig" rsource "mag/Kconfig" rsource "power/Kconfig" diff --git a/lib/sense/baro/CMakeLists.txt b/lib/sense/baro/CMakeLists.txt new file mode 100644 index 00000000..37ac421e --- /dev/null +++ b/lib/sense/baro/CMakeLists.txt @@ -0,0 +1,14 @@ +# Copyright (c) 2023, CogniPilot Foundation +# SPDX-License-Identifier: Apache-2.0 + +zephyr_library_named(baro) + + +# we need to be able to include generated header files +zephyr_include_directories() + +zephyr_library_sources( + main.c + ) + +add_dependencies(baro synapse_protobuf) diff --git a/lib/sense/baro/Kconfig b/lib/sense/baro/Kconfig new file mode 100644 index 00000000..7983913a --- /dev/null +++ b/lib/sense/baro/Kconfig @@ -0,0 +1,15 @@ +# Copyright (c) 2023, CogniPilot Foundation +# SPDX-License-Identifier: Apache-2.0 +menuconfig SENSE_BARO + bool "Enable Baro" + default n + help + This option enables the barometric altimeter driver interface + +if SENSE_BARO + +module = SENSE_BARO +module-str = sense_baro +source "subsys/logging/Kconfig.template.log_config" + +endif # SENSE_BARO diff --git a/lib/sense/baro/main.c b/lib/sense/baro/main.c new file mode 100644 index 00000000..e497c5ef --- /dev/null +++ b/lib/sense/baro/main.c @@ -0,0 +1,107 @@ +/* + * Copyright CogniPilot Foundation 2023 + * SPDX-License-Identifier: Apache-2.0 + */ +#include +#include +#include +#include + +#include + +LOG_MODULE_REGISTER(sense_baro, CONFIG_SENSE_BARO_LOG_LEVEL); + +#define MY_STACK_SIZE 4096 +#define MY_PRIORITY 6 + +extern struct k_work_q g_low_priority_work_q; +static const struct device* g_baro_dev[2]; +static int32_t g_seq = 0; + +static const struct device* sensor_check(const struct device* const dev) +{ + if (dev == NULL) { + /* No such node, or the node does not have status "okay". */ + LOG_ERR("no device found"); + return NULL; + } + + if (!device_is_ready(dev)) { + LOG_ERR("device %s is not ready, check the driver initialization logs for errors", + dev->name); + return NULL; + } + + LOG_INF("baro found device %s", dev->name); + return dev; +} + +void baro_work_handler(struct k_work* work) +{ + double baro_data_array[2][2] = {}; + for (int i = 0; i < 2; i++) { + // default all data to zero + struct sensor_value baro_press = {}; + struct sensor_value baro_temp = {}; + + // get accel if device present + if (g_baro_dev[i] != NULL) { + sensor_sample_fetch(g_baro_dev[i]); + sensor_channel_get(g_baro_dev[i], SENSOR_CHAN_PRESS, &baro_press); + sensor_channel_get(g_baro_dev[i], SENSOR_CHAN_AMBIENT_TEMP, &baro_temp); + LOG_DBG("baro %d: %d.%06d %d.%06d", i, + baro_press.val1, baro_press.val2, + baro_temp.val1, baro_temp.val2); + } + + baro_data_array[i][0] = baro_press.val1 + baro_press.val2 * 1e-6; + baro_data_array[i][1] = baro_temp.val1 + baro_temp.val2 * 1e-6; + } + + // select first baro for data for now: TODO implement voting + // TODO add barmetric formula equation + double press = baro_data_array[0][0]; + double temp = 15.0; // standard atmosphere temp in C + const float sea_press = 101.325; + double alt = ((pow((sea_press / press), 1 / 5.257) - 1.0) * (temp + 273.15)) / 0.0065; + // LOG_DBG("press %10.4f, temp: %10.4f, alt: %10.4f", press, temp, alt); + + // publish mag to zbus + synapse_msgs_Altimeter msg = synapse_msgs_Altimeter_init_default; + 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.vertical_position = alt; + msg.vertical_velocity = 0; + msg.vertical_reference = 0; + zbus_chan_pub(&chan_out_altimeter, &msg, K_NO_WAIT); +} + +K_WORK_DEFINE(baro_work, baro_work_handler); + +void baro_timer_handler(struct k_timer* dummy) +{ + k_work_submit_to_queue(&g_low_priority_work_q, &baro_work); +} + +K_TIMER_DEFINE(baro_timer, baro_timer_handler, NULL); + +int sense_baro_entry_point(void) +{ + g_baro_dev[0] = sensor_check(DEVICE_DT_GET(DT_ALIAS(baro0))); + g_baro_dev[1] = sensor_check(DEVICE_DT_GET(DT_ALIAS(baro1))); + k_timer_start(&baro_timer, K_MSEC(1000), K_MSEC(1000)); + return 0; +} + +K_THREAD_DEFINE(sense_baro, MY_STACK_SIZE, + sense_baro_entry_point, NULL, NULL, NULL, + MY_PRIORITY, 0, 0); + +// vi: ts=4 sw=4 et diff --git a/lib/sense/imu/main.c b/lib/sense/imu/main.c index b5ebe118..af18d4e2 100644 --- a/lib/sense/imu/main.c +++ b/lib/sense/imu/main.c @@ -9,8 +9,14 @@ LOG_MODULE_REGISTER(sense_imu, CONFIG_SENSE_IMU_LOG_LEVEL); -#define MY_STACK_SIZE 1280 -#define MY_PRIORITY 6 +#define THREAD_STACK_SIZE 2048 +#define THREAD_PRIORITY 6 + +extern struct k_work_q g_high_priority_work_q; + +static const struct device* g_accel_dev[3]; +static const struct device* g_gyro_dev[3]; +static int32_t g_seq = 0; static const struct device* sensor_check(const struct device* const dev) { @@ -30,92 +36,106 @@ static const struct device* sensor_check(const struct device* const dev) return dev; } -int sense_imu_entry_point(void) +void imu_work_handler(struct k_work* work) { - const struct device* const accel_dev[] = { - sensor_check(DEVICE_DT_GET(DT_ALIAS(accel0))), - sensor_check(DEVICE_DT_GET(DT_ALIAS(accel1))), - sensor_check(DEVICE_DT_GET(DT_ALIAS(accel2))) - }; - - const struct device* const gyro_dev[] = { - sensor_check(DEVICE_DT_GET(DT_ALIAS(gyro0))), - sensor_check(DEVICE_DT_GET(DT_ALIAS(gyro1))), - sensor_check(DEVICE_DT_GET(DT_ALIAS(gyro2))) - }; - double accel_data_array[3][3] = {}; double gyro_data_array[3][3] = {}; - while (1) { - - LOG_DBG(""); - for (int i = 0; i < 3; i++) { - // default all data to zero - struct sensor_value accel_value[3] = {}; - struct sensor_value gyro_value[3] = {}; - - // get accel if device present - if (accel_dev[i] != NULL) { - sensor_sample_fetch(accel_dev[i]); - sensor_channel_get(accel_dev[i], SENSOR_CHAN_ACCEL_XYZ, accel_value); - LOG_DBG("accel %d: %d.%06d %d.%06d %d.%06d", i, - accel_value[0].val1, accel_value[0].val2, - accel_value[1].val1, accel_value[1].val2, - accel_value[2].val1, accel_value[2].val2); - } - - // get gyro if device present - if (gyro_dev[i] != NULL) { - // don't resample if it is the same device as accel, want same timestamp - if (gyro_dev[i] != accel_dev[i]) { - sensor_sample_fetch(gyro_dev[i]); - } - sensor_channel_get(gyro_dev[i], SENSOR_CHAN_GYRO_XYZ, gyro_value); - LOG_DBG("gyro %d: %d.%06d %d.%06d %d.%06d", i, - gyro_value[0].val1, gyro_value[0].val2, - gyro_value[1].val1, gyro_value[1].val2, - gyro_value[2].val1, gyro_value[2].val2); - } + // LOG_DBG(""); + for (int i = 0; i < 3; i++) { + // default all data to zero + struct sensor_value accel_value[3] = {}; + struct sensor_value gyro_value[3] = {}; + + // get accel if device present + if (g_accel_dev[i] != NULL) { + sensor_sample_fetch(g_accel_dev[i]); + sensor_channel_get(g_accel_dev[i], SENSOR_CHAN_ACCEL_XYZ, accel_value); + LOG_DBG("accel %d: %d.%06d %d.%06d %d.%06d", i, + accel_value[0].val1, accel_value[0].val2, + accel_value[1].val1, accel_value[1].val2, + accel_value[2].val1, accel_value[2].val2); + } - for (int j = 0; j < 3; j++) { - accel_data_array[i][j] = accel_value[j].val1 + accel_value[j].val2 * 1e-6; - gyro_data_array[i][j] = gyro_value[j].val1 + gyro_value[j].val2 * 1e-6; + // get gyro if device present + if (g_gyro_dev[i] != NULL) { + // don't resample if it is the same device as accel, want same timestamp + if (g_gyro_dev[i] != g_accel_dev[i]) { + sensor_sample_fetch(g_gyro_dev[i]); } + sensor_channel_get(g_gyro_dev[i], SENSOR_CHAN_GYRO_XYZ, gyro_value); + LOG_DBG("gyro %d: %d.%06d %d.%06d %d.%06d", i, + gyro_value[0].val1, gyro_value[0].val2, + gyro_value[1].val1, gyro_value[1].val2, + gyro_value[2].val1, gyro_value[2].val2); } - // select first imu for data for now: TODO implement voting - double accel[3] = { - accel_data_array[0][0], - accel_data_array[0][1], - accel_data_array[0][2] - }; - - double gyro[3] = { - gyro_data_array[0][0], - gyro_data_array[0][1], - gyro_data_array[0][2] - }; - - // publish imu to zbus - synapse_msgs_Imu msg; - strncpy(msg.header.frame_id, "map", sizeof(msg.header.frame_id) - 1); - msg.angular_velocity.x = gyro[0]; - msg.angular_velocity.y = gyro[1]; - msg.angular_velocity.z = gyro[2]; - msg.linear_acceleration.x = accel[0]; - msg.linear_acceleration.y = accel[1]; - msg.linear_acceleration.z = accel[2]; - zbus_chan_pub(&chan_out_imu, &msg, K_NO_WAIT); - - // 200 Hz - k_sleep(K_MSEC(5)); + for (int j = 0; j < 3; j++) { + accel_data_array[i][j] = accel_value[j].val1 + accel_value[j].val2 * 1e-6; + gyro_data_array[i][j] = gyro_value[j].val1 + gyro_value[j].val2 * 1e-6; + } } + + // select first imu for data for now: TODO implement voting + double accel[3] = { + accel_data_array[0][0], + accel_data_array[0][1], + accel_data_array[0][2] + }; + + double gyro[3] = { + gyro_data_array[0][0], + gyro_data_array[0][1], + gyro_data_array[0][2] + }; + + // publish imu to zbus + synapse_msgs_Imu msg = synapse_msgs_Imu_init_default; + 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, "base_link", sizeof(msg.header.frame_id) - 1); + msg.has_angular_velocity = true; + msg.angular_velocity.x = gyro[0]; + msg.angular_velocity.y = gyro[1]; + msg.angular_velocity.z = gyro[2]; + msg.has_linear_acceleration = true; + msg.linear_acceleration.x = accel[0]; + msg.linear_acceleration.y = accel[1]; + msg.linear_acceleration.z = accel[2]; + zbus_chan_pub(&chan_out_imu, &msg, K_NO_WAIT); +} + +K_WORK_DEFINE(imu_work, imu_work_handler); + +void imu_timer_handler(struct k_timer* dummy) +{ + k_work_submit_to_queue(&g_high_priority_work_q, &imu_work); +} + +K_TIMER_DEFINE(imu_timer, imu_timer_handler, NULL); + +int sense_imu_entry_point(void) +{ + g_accel_dev[0] = sensor_check(DEVICE_DT_GET(DT_ALIAS(accel0))); + g_accel_dev[1] = sensor_check(DEVICE_DT_GET(DT_ALIAS(accel1))); + g_accel_dev[2] = sensor_check(DEVICE_DT_GET(DT_ALIAS(accel2))); + + g_gyro_dev[0] = sensor_check(DEVICE_DT_GET(DT_ALIAS(gyro0))); + g_gyro_dev[1] = sensor_check(DEVICE_DT_GET(DT_ALIAS(gyro1))); + g_gyro_dev[2] = sensor_check(DEVICE_DT_GET(DT_ALIAS(gyro2))); + + k_timer_start(&imu_timer, K_MSEC(5), K_MSEC(5)); return 0; } -K_THREAD_DEFINE(sense_imu_thread, MY_STACK_SIZE, +K_THREAD_DEFINE(sense_imu, THREAD_STACK_SIZE, sense_imu_entry_point, NULL, NULL, NULL, - MY_PRIORITY, 0, 0); + THREAD_PRIORITY, 0, 0); // vi: ts=4 sw=4 et diff --git a/lib/sense/mag/main.c b/lib/sense/mag/main.c index fa67f4cc..5aeda6c9 100644 --- a/lib/sense/mag/main.c +++ b/lib/sense/mag/main.c @@ -9,9 +9,13 @@ LOG_MODULE_REGISTER(sense_mag, CONFIG_SENSE_MAG_LOG_LEVEL); -#define MY_STACK_SIZE 740 +#define MY_STACK_SIZE 2048 #define MY_PRIORITY 6 +extern struct k_work_q g_high_priority_work_q; +static const struct device* g_mag_dev[2]; +static int32_t g_seq = 0; + static const struct device* sensor_check(const struct device* const dev) { if (dev == NULL) { @@ -30,59 +34,72 @@ static const struct device* sensor_check(const struct device* const dev) return dev; } -int sense_mag_entry_point(void) +void mag_work_handler(struct k_work* work) { - const struct device* const mag_dev[] = { - sensor_check(DEVICE_DT_GET(DT_ALIAS(mag0))), - sensor_check(DEVICE_DT_GET(DT_ALIAS(mag1))), - }; - + LOG_DBG(""); double mag_data_array[2][3] = {}; - - while (1) { - - LOG_DBG(""); - for (int i = 0; i < 2; i++) { - // default all data to zero - struct sensor_value mag_value[3] = {}; - - // get accel if device present - if (mag_dev[i] != NULL) { - sensor_sample_fetch(mag_dev[i]); - sensor_channel_get(mag_dev[i], SENSOR_CHAN_MAGN_XYZ, mag_value); - LOG_DBG("mag %d: %d.%06d %d.%06d %d.%06d", i, - mag_value[0].val1, mag_value[0].val2, - mag_value[1].val1, mag_value[1].val2, - mag_value[2].val1, mag_value[2].val2); - } - - for (int j = 0; j < 3; j++) { - mag_data_array[i][j] = mag_value[j].val1 + mag_value[j].val2 * 1e-6; - } + for (int i = 0; i < 2; i++) { + // default all data to zero + struct sensor_value mag_value[3] = {}; + + // get accel if device present + if (g_mag_dev[i] != NULL) { + sensor_sample_fetch(g_mag_dev[i]); + sensor_channel_get(g_mag_dev[i], SENSOR_CHAN_MAGN_XYZ, mag_value); + LOG_DBG("mag %d: %d.%06d %d.%06d %d.%06d", i, + mag_value[0].val1, mag_value[0].val2, + mag_value[1].val1, mag_value[1].val2, + mag_value[2].val1, mag_value[2].val2); } - // select first mag for data for now: TODO implement voting - double mag[3] = { - mag_data_array[0][0], - mag_data_array[0][1], - mag_data_array[0][2] - }; - - // publish mag to zbus - synapse_msgs_MagneticField msg; - strncpy(msg.header.frame_id, "map", sizeof(msg.header.frame_id) - 1); - msg.magnetic_field.x = mag[0]; - msg.magnetic_field.y = mag[1]; - msg.magnetic_field.z = mag[2]; - zbus_chan_pub(&chan_out_magnetic_field, &msg, K_NO_WAIT); - - // 50 Hz - k_sleep(K_MSEC(20)); + for (int j = 0; j < 3; j++) { + mag_data_array[i][j] = mag_value[j].val1 + mag_value[j].val2 * 1e-6; + } } + + // select first mag for data for now: TODO implement voting + double mag[3] = { + mag_data_array[0][0], + mag_data_array[0][1], + mag_data_array[0][2] + }; + + // publish mag to zbus + synapse_msgs_MagneticField msg = synapse_msgs_MagneticField_init_default; + 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.has_magnetic_field = true; + msg.magnetic_field.x = mag[0]; + msg.magnetic_field.y = mag[1]; + msg.magnetic_field.z = mag[2]; + zbus_chan_pub(&chan_out_magnetic_field, &msg, K_NO_WAIT); +} + +K_WORK_DEFINE(mag_work, mag_work_handler); + +void mag_timer_handler(struct k_timer* dummy) +{ + k_work_submit_to_queue(&g_high_priority_work_q, &mag_work); +} + +K_TIMER_DEFINE(mag_timer, mag_timer_handler, NULL); + +int sense_mag_entry_point(void) +{ + g_mag_dev[0] = sensor_check(DEVICE_DT_GET(DT_ALIAS(mag0))); + g_mag_dev[1] = sensor_check(DEVICE_DT_GET(DT_ALIAS(mag1))); + k_timer_start(&mag_timer, K_MSEC(20), K_MSEC(20)); return 0; } -K_THREAD_DEFINE(sense_mag_thread, MY_STACK_SIZE, +K_THREAD_DEFINE(sense_mag, MY_STACK_SIZE, sense_mag_entry_point, NULL, NULL, NULL, MY_PRIORITY, 0, 0); diff --git a/lib/sense/power/main.c b/lib/sense/power/main.c index 826d86de..3b1da125 100644 --- a/lib/sense/power/main.c +++ b/lib/sense/power/main.c @@ -14,48 +14,64 @@ LOG_MODULE_REGISTER(sense_power, CONFIG_SENSE_POWER_LOG_LEVEL); -#define MY_STACK_SIZE 560 +#define MY_STACK_SIZE 2048 #define MY_PRIORITY 6 -const struct device* const g_dev = DEVICE_DT_GET(DT_ALIAS(power0)); +extern struct k_work_q g_high_priority_work_q; +static const struct device* const g_dev = DEVICE_DT_GET(DT_ALIAS(power0)); +static int32_t g_seq = 0; -void publish_battery_data_zbus() +void power_work_handler(struct k_work* work) { - + int ret = sensor_sample_fetch(g_dev); + if (ret) { + LOG_ERR("Could not fetch sensor data"); + return; + } struct sensor_value voltage, current; sensor_channel_get(g_dev, SENSOR_CHAN_VOLTAGE, &voltage); sensor_channel_get(g_dev, SENSOR_CHAN_CURRENT, ¤t); - synapse_msgs_BatteryState msg_battery_state; + synapse_msgs_BatteryState msg = synapse_msgs_BatteryState_init_default; + + 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, "base_link", sizeof(msg.header.frame_id) - 1); - msg_battery_state.voltage = sensor_value_to_double(&voltage); - msg_battery_state.current = sensor_value_to_double(¤t); + msg.voltage = sensor_value_to_double(&voltage); + msg.current = sensor_value_to_double(¤t); - zbus_chan_pub(&chan_out_battery_state, &msg_battery_state, K_NO_WAIT); + zbus_chan_pub(&chan_out_battery_state, &msg, K_NO_WAIT); } -void sense_power_entry_point(void) +K_WORK_DEFINE(power_work, power_work_handler); + +void power_timer_handler(struct k_timer* dummy) { - int rc = 0; + k_work_submit_to_queue(&g_high_priority_work_q, &power_work); +} +K_TIMER_DEFINE(power_timer, power_timer_handler, NULL); + +int sense_power_entry_point(void) +{ + g_seq = 0; if (!device_is_ready(g_dev)) { LOG_ERR("Device %s is not ready", g_dev->name); } - while (true) { - k_sleep(K_MSEC(100)); - rc = sensor_sample_fetch(g_dev); - if (rc) { - LOG_ERR("Could not fetch sensor data"); - continue; - } - - publish_battery_data_zbus(); - } + k_timer_start(&power_timer, K_MSEC(100), K_MSEC(100)); + return 0; } -K_THREAD_DEFINE(sense_power_thread, MY_STACK_SIZE, +K_THREAD_DEFINE(sense_power, MY_STACK_SIZE, sense_power_entry_point, NULL, NULL, NULL, MY_PRIORITY, 0, 0); diff --git a/lib/sense/ubx_gnss/main.c b/lib/sense/ubx_gnss/main.c index 1bde8381..dcf06077 100644 --- a/lib/sense/ubx_gnss/main.c +++ b/lib/sense/ubx_gnss/main.c @@ -27,6 +27,7 @@ LOG_MODULE_REGISTER(ubx_gnss, CONFIG_UBX_GNSS_LOG_LEVEL); static int32_t gMeasurementPeriodMs = 100; static bool running = false; static bool isAlive = false; +static int32_t g_seq = 0; void publish_gnss_data_zbus(uDeviceHandle_t devHandle, int32_t errorCode, @@ -35,16 +36,26 @@ void publish_gnss_data_zbus(uDeviceHandle_t devHandle, isAlive = true; if (errorCode == 0) { - synapse_msgs_NavSatFix nav_sat_fix; - - nav_sat_fix.latitude = pLocation->latitudeX1e7 / 1e7; - nav_sat_fix.longitude = pLocation->longitudeX1e7 / 1e7; - nav_sat_fix.altitude = pLocation->altitudeMillimetres / 1e3; + synapse_msgs_NavSatFix msg = synapse_msgs_NavSatFix_init_default; + + msg.has_header = true; + strncpy(msg.header.frame_id, "map", sizeof(msg.header.frame_id) - 1); + msg.header.has_stamp = true; + msg.latitude = pLocation->latitudeX1e7 / 1e7; + msg.longitude = pLocation->longitudeX1e7 / 1e7; + msg.altitude = pLocation->altitudeMillimetres / 1e3; + 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; // TODO Covariance - zbus_chan_pub(&chan_out_nav_sat_fix, &nav_sat_fix, K_NO_WAIT); - LOG_DBG("lat %f long %f\n", nav_sat_fix.latitude, nav_sat_fix.longitude); + zbus_chan_pub(&chan_out_nav_sat_fix, &msg, K_NO_WAIT); + LOG_DBG("lat %f long %f\n", msg.latitude, msg.longitude); } else if (errorCode == U_ERROR_COMMON_TIMEOUT) { // LOG_ERR("Tiemout error"); } else { @@ -56,6 +67,7 @@ void publish_gnss_data_zbus(uDeviceHandle_t devHandle, void sense_ubx_gnss_entry_point() { int32_t errorCode; + g_seq = 0; // Remove the line below if you want the log printouts from ubxlib uPortLogOff(); @@ -115,6 +127,6 @@ void sense_ubx_gnss_entry_point() } } -K_THREAD_DEFINE(ubx_gnss_thread, MY_STACK_SIZE, +K_THREAD_DEFINE(ubx_gnss, MY_STACK_SIZE, sense_ubx_gnss_entry_point, NULL, NULL, NULL, MY_PRIORITY, 0, 0); diff --git a/lib/sense/vesc_can_status/main.c b/lib/sense/vesc_can_status/main.c index 0bb98155..772fa431 100644 --- a/lib/sense/vesc_can_status/main.c +++ b/lib/sense/vesc_can_status/main.c @@ -35,7 +35,7 @@ void vesc_can_entry_point(const struct shell* sh) } } -K_THREAD_DEFINE(can_vesc_thread, MY_STACK_SIZE, +K_THREAD_DEFINE(can_vesc, MY_STACK_SIZE, vesc_can_entry_point, NULL, NULL, NULL, MY_PRIORITY, 0, 0); diff --git a/lib/sense/wheel_odometry/main.c b/lib/sense/wheel_odometry/main.c index e922c5f4..0248f62b 100644 --- a/lib/sense/wheel_odometry/main.c +++ b/lib/sense/wheel_odometry/main.c @@ -9,9 +9,14 @@ LOG_MODULE_REGISTER(sense_wheel_odometry, CONFIG_SENSE_WHEEL_ODOMETRY_LOG_LEVEL); -#define MY_STACK_SIZE 520 +#define MY_STACK_SIZE 2048 #define MY_PRIORITY 6 +extern struct k_work_q g_high_priority_work_q; +static const struct device* g_dev[1]; +static int32_t g_seq = 0; +static int g_n_sensors = 1; + static const struct device* sensor_check(const struct device* const dev) { if (dev == NULL) { @@ -30,50 +35,63 @@ static const struct device* sensor_check(const struct device* const dev) return dev; } -int sense_wheel_odometry_entry_point(void) +void wheel_odometry_work_handler(struct k_work* work) { - const struct device* const dev[] = { - sensor_check(DEVICE_DT_GET(DT_ALIAS(wheel_odometry0))) - }; + LOG_DBG(""); + double data_array[1] = {}; + for (int i = 0; i < g_n_sensors; i++) { + // default all data to zero + struct sensor_value value = {}; + + // get accel if device present + if (g_dev[i] != NULL) { + sensor_sample_fetch(g_dev[i]); + sensor_channel_get(g_dev[i], SENSOR_CHAN_ROTATION, &value); + LOG_DBG("rotation %d: %d.%06d", i, value.val1, value.val2); + } - int n_sensors = 1; + for (int j = 0; j < g_n_sensors; j++) { + data_array[i] = value.val1 + value.val2 * 1e-6; + } + } - double data_array[1] = {}; + // select first wheel encoder for data for now: TODO implement voting + double rotation = data_array[0]; - while (1) { + // publish mag to zbus + synapse_msgs_WheelOdometry msg = synapse_msgs_WheelOdometry_init_default; - LOG_DBG(""); - for (int i = 0; i < n_sensors; i++) { - // default all data to zero - struct sensor_value value = {}; + 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, "base_link", sizeof(msg.header.frame_id) - 1); - // get accel if device present - if (dev[i] != NULL) { - sensor_sample_fetch(dev[i]); - sensor_channel_get(dev[i], SENSOR_CHAN_ROTATION, &value); - LOG_DBG("rotation %d: %d.%06d", i, value.val1, value.val2); - } + msg.rotation = -rotation; // account for negative rotation of encoder + zbus_chan_pub(&chan_out_wheel_odometry, &msg, K_NO_WAIT); +} - for (int j = 0; j < n_sensors; j++) { - data_array[i] = value.val1 + value.val2 * 1e-6; - } - } +K_WORK_DEFINE(wheel_odometry_work, wheel_odometry_work_handler); - // select first wheel encoder for data for now: TODO implement voting - double rotation = data_array[0]; +void wheel_odometry_timer_handler(struct k_timer* dummy) +{ + k_work_submit_to_queue(&g_high_priority_work_q, &wheel_odometry_work); +} - // publish mag to zbus - synapse_msgs_WheelOdometry msg; - msg.rotation = -rotation; // account for negative rotation of encoder - zbus_chan_pub(&chan_out_wheel_odometry, &msg, K_NO_WAIT); +K_TIMER_DEFINE(wheel_odometry_timer, wheel_odometry_timer_handler, NULL); - // 200 Hz - k_sleep(K_MSEC(5)); - } +int sense_wheel_odometry_entry_point(void) +{ + g_dev[0] = sensor_check(DEVICE_DT_GET(DT_ALIAS(wheel_odometry0))); + k_timer_start(&wheel_odometry_timer, K_MSEC(10), K_MSEC(10)); return 0; } -K_THREAD_DEFINE(sense_wheel_odometry_thread, MY_STACK_SIZE, +K_THREAD_DEFINE(sense_wheel_odometry, MY_STACK_SIZE, sense_wheel_odometry_entry_point, NULL, NULL, NULL, MY_PRIORITY, 0, 0); diff --git a/lib/synapse/CMakeLists.txt b/lib/synapse/CMakeLists.txt index 171015a6..89876e5f 100644 --- a/lib/synapse/CMakeLists.txt +++ b/lib/synapse/CMakeLists.txt @@ -1,6 +1,6 @@ # SPDX-License-Identifier: Apache-2.0 +add_subdirectory_ifdef(CONFIG_SYNAPSE_ZBUS zbus) add_subdirectory_ifdef(CONFIG_SYNAPSE_ETHERNET ethernet) -add_subdirectory_ifdef(CONFIG_SYNAPSE_UART uart) +add_subdirectory_ifdef(CONFIG_SYNAPSE_TOPIC topic) add_subdirectory_ifdef(CONFIG_SYNAPSE_VESC_CAN vesc_can) -add_subdirectory_ifdef(CONFIG_SYNAPSE_ZBUS zbus) diff --git a/lib/synapse/Kconfig b/lib/synapse/Kconfig index 7751dad9..e2d5c2b9 100644 --- a/lib/synapse/Kconfig +++ b/lib/synapse/Kconfig @@ -4,7 +4,7 @@ menu "Synapse" rsource "ethernet/Kconfig" -rsource "uart/Kconfig" +rsource "topic/Kconfig" rsource "vesc_can/Kconfig" rsource "zbus/Kconfig" diff --git a/lib/synapse/topic/CMakeLists.txt b/lib/synapse/topic/CMakeLists.txt new file mode 100644 index 00000000..00b2a2c2 --- /dev/null +++ b/lib/synapse/topic/CMakeLists.txt @@ -0,0 +1,13 @@ +# Copyright (c) 2023, CogniPilot Foundation +# SPDX-License-Identifier: Apache-2.0 + +zephyr_library_named(synapse_topic) + +# we need to be able to include generated header files +zephyr_include_directories() + +zephyr_library_sources( + main.c + ) + +add_dependencies(synapse_topic synapse_zbus) diff --git a/lib/synapse/topic/Kconfig b/lib/synapse/topic/Kconfig new file mode 100644 index 00000000..4d1cd812 --- /dev/null +++ b/lib/synapse/topic/Kconfig @@ -0,0 +1,15 @@ +# Copyright (c) 2023, CogniPilot Foundation +# SPDX-License-Identifier: Apache-2.0 + +config SYNAPSE_TOPIC + bool "Enable synapse topic shell" + help + This option enables the synapse topic shell + +if SYNAPSE_TOPIC + +module = SYNAPSE_TOPIC +module-str = synapse_topic +source "subsys/logging/Kconfig.template.log_config" + +endif # SYNAPSE_TOPIC diff --git a/lib/synapse/topic/main.c b/lib/synapse/topic/main.c new file mode 100644 index 00000000..11f3059d --- /dev/null +++ b/lib/synapse/topic/main.c @@ -0,0 +1,424 @@ +/* + * Copyright (c) 2022 Rodrigo Peixoto + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#include +#include +#include + +#include + +LOG_MODULE_DECLARE(synapse_topic, LOG_LEVEL_INF); +static const struct shell* g_sh = NULL; +static const struct zbus_channel* g_chan_echo = NULL; +static int g_chan_msg_count = 0; +typedef enum ListenerMode { LISTENER_MODE_NONE, + LISTENER_MODE_HZ, + LISTENER_MODE_ECHO } listener_mode_t; +static listener_mode_t g_listener_mode = LISTENER_MODE_NONE; + +void listener_synapse_topic_callback(const struct zbus_channel* chan); +ZBUS_LISTENER_DEFINE(listener_synapse_topic, listener_synapse_topic_callback); + +void print_Time(synapse_msgs_Time* msg) +{ + shell_print(g_sh, "stamp: %lld.%09d", msg->sec, msg->nanosec); +} + +void print_Vector3(synapse_msgs_Vector3* msg) +{ + shell_print(g_sh, "x: %10.4f y: %10.4f z: %10.4f", msg->x, msg->y, msg->z); +} + +void print_Point(synapse_msgs_Point* msg) +{ + shell_print(g_sh, "x: %10.4f y: %10.4f z: %10.4f", msg->x, msg->y, msg->z); +} + +void print_Quaternion(synapse_msgs_Quaternion* msg) +{ + shell_print(g_sh, "w: %10.4f x: %10.4f y: %10.4f z: %10.4f", msg->w, msg->x, msg->y, msg->z); +} + +void print_Twist(synapse_msgs_Twist* msg) +{ + if (msg->has_angular) { + shell_print(g_sh, "angular"); + print_Vector3(&msg->angular); + } + if (msg->has_linear) { + shell_print(g_sh, "linear"); + print_Vector3(&msg->linear); + } +} + +void print_TwistWithCovariance(synapse_msgs_TwistWithCovariance* msg) +{ + if (msg->has_twist) { + print_Twist(&msg->twist); + } + + shell_print(g_sh, "covariance"); + for (int i = 0; i < msg->covariance_count; i++) { + shell_print(g_sh, "%10.4f", msg->covariance[i]); + } +} + +void print_Pose(synapse_msgs_Pose* msg) +{ + shell_print(g_sh, "position"); + if (msg->has_position) { + print_Point(&msg->position); + } + + shell_print(g_sh, "orientation"); + if (msg->has_orientation) { + print_Quaternion(&msg->orientation); + } +} + +void print_PoseWithCovariance(synapse_msgs_PoseWithCovariance* msg) +{ + if (msg->has_pose) { + print_Pose(&msg->pose); + } + + shell_print(g_sh, "covariance"); + for (int i = 0; i < msg->covariance_count; i++) { + shell_print(g_sh, "%10.4f", msg->covariance[i]); + } +} + +void print_Header(synapse_msgs_Header* msg) +{ + if (msg->has_stamp) { + print_Time(&msg->stamp); + } + shell_print(g_sh, "frame: %s", msg->frame_id); + shell_print(g_sh, "seq: %d", msg->seq); +} + +void print_Altimeter(synapse_msgs_Altimeter* msg) +{ + shell_print(g_sh, "alt: %10.4f m, vel: %10.4f m/s, ref alt: %10.4f m", + msg->vertical_position, msg->vertical_velocity, msg->vertical_reference); +} + +void print_BatteryState(synapse_msgs_BatteryState* msg) +{ + if (msg->has_header) { + print_Header(&msg->header); + } + shell_print(g_sh, "voltage: %10.4f V, current: %10.4f A", + msg->voltage, msg->current); +} + +void print_NavSatFix(synapse_msgs_NavSatFix* msg) +{ + if (msg->has_header) { + print_Header(&msg->header); + } + shell_print(g_sh, "lat: %10.7f deg", msg->latitude); + shell_print(g_sh, "lon: %10.7f deg", msg->longitude); + shell_print(g_sh, "alt: %10.7f m", msg->altitude); +} + +void print_Actuators(synapse_msgs_Actuators* msg) +{ + if (msg->has_header) { + print_Header(&msg->header); + } + + shell_print(g_sh, "position [m]"); + for (int i = 0; i < msg->position_count; i++) { + shell_print(g_sh, "%10.4f", msg->position[i]); + } + + shell_print(g_sh, "velocity [m/s]"); + for (int i = 0; i < msg->velocity_count; i++) { + shell_print(g_sh, "%10.4f", msg->velocity[i]); + } + + shell_print(g_sh, "normalized"); + for (int i = 0; i < msg->normalized_count; i++) { + shell_print(g_sh, "%10.4f", msg->normalized[i]); + } +} + +void print_Imu(synapse_msgs_Imu* msg) +{ + if (msg->has_header) { + print_Header(&msg->header); + } + + if (msg->has_angular_velocity) { + shell_print(g_sh, "angular velocity [rad/s]"); + print_Vector3(&msg->angular_velocity); + + shell_print(g_sh, "covariance"); + for (int i = 0; i < msg->angular_velocity_covariance_count; i++) { + shell_print(g_sh, "%10.4f", msg->angular_velocity_covariance[i]); + } + } + + if (msg->has_linear_acceleration) { + shell_print(g_sh, "linear acceleration [m/s^2]"); + print_Vector3(&msg->linear_acceleration); + + shell_print(g_sh, "covariance"); + for (int i = 0; i < msg->linear_acceleration_covariance_count; i++) { + shell_print(g_sh, "%10.4f", msg->linear_acceleration_covariance[i]); + } + } + + if (msg->has_orientation) { + shell_print(g_sh, "orientation"); + print_Quaternion(&msg->orientation); + } +} + +void print_Odometry(synapse_msgs_Odometry* msg) +{ + if (msg->has_header) { + print_Header(&msg->header); + } + + if (msg->has_pose) { + print_PoseWithCovariance(&msg->pose); + } + + if (msg->has_twist) { + print_TwistWithCovariance(&msg->twist); + } + shell_print(g_sh, "child frame: %s", msg->child_frame_id); +} + +void print_WheelOdometry(synapse_msgs_WheelOdometry* msg) +{ + if (msg->has_header) { + print_Header(&msg->header); + } + + shell_print(g_sh, "rotation: %10.4f", msg->rotation); +} + +void print_Joy(synapse_msgs_Joy* msg) +{ + shell_print(g_sh, "axes"); + for (int i = 0; i < msg->axes_count; i++) { + shell_print(g_sh, "%10.4f", msg->axes[i]); + } + + shell_print(g_sh, "buttons"); + for (int i = 0; i < msg->buttons_count; i++) { + shell_print(g_sh, "%10d", msg->buttons[i]); + } +} + +void print_BezierCurve(synapse_msgs_BezierCurve* msg) +{ + shell_print(g_sh, "x"); + for (int i = 0; i < msg->x_count; i++) { + shell_print(g_sh, "%10.4f", msg->x[i]); + } + shell_print(g_sh, "y"); + for (int i = 0; i < msg->y_count; i++) { + shell_print(g_sh, "%10.4f", msg->y[i]); + } + shell_print(g_sh, "z"); + for (int i = 0; i < msg->z_count; i++) { + shell_print(g_sh, "%10.4f", msg->z[i]); + } + shell_print(g_sh, "yaw"); + for (int i = 0; i < msg->yaw_count; i++) { + shell_print(g_sh, "%10.4f", msg->yaw[i]); + } + shell_print(g_sh, "time stop: %lld", msg->time_stop); +} + +void print_BezierTrajectory(synapse_msgs_BezierTrajectory* msg) +{ + if (msg->has_header) { + print_Header(&msg->header); + } + + shell_print(g_sh, "time start: %lld", msg->time_start); + + for (int i = 0; i < msg->curves_count; i++) { + shell_print(g_sh, "curve: %d", i); + print_BezierCurve(&msg->curves[i]); + } +} + +void print_MagneticField(synapse_msgs_MagneticField* msg) +{ + if (msg->has_header) { + print_Header(&msg->header); + } + + if (msg->has_magnetic_field) { + print_Vector3(&msg->magnetic_field); + } + + shell_print(g_sh, "covariance"); + for (int i = 0; i < msg->magnetic_field_covariance_count; i++) { + shell_print(g_sh, "%10.4f", msg->magnetic_field_covariance[i]); + } +} + +void listener_synapse_topic_callback(const struct zbus_channel* chan) +{ + // filter out channels we are not echoing + if (chan != g_chan_echo) { + return; + } + + if (g_listener_mode == LISTENER_MODE_HZ) { + g_chan_msg_count += 1; + } else if (g_listener_mode == LISTENER_MODE_ECHO) { + if (chan == &chan_out_nav_sat_fix) { + synapse_msgs_NavSatFix* msg = (synapse_msgs_NavSatFix*)(chan->message); + print_NavSatFix(msg); + } else if (chan == &chan_in_actuators || chan == &chan_out_actuators) { + synapse_msgs_Actuators* msg = (synapse_msgs_Actuators*)(chan->message); + print_Actuators(msg); + } else if (chan == &chan_in_clock_offset) { + synapse_msgs_Time* msg = (synapse_msgs_Time*)(chan->message); + print_Time(msg); + } else if (chan == &chan_out_imu) { + synapse_msgs_Imu* msg = (synapse_msgs_Imu*)(chan->message); + print_Imu(msg); + } else if (chan == &chan_in_odometry || chan == &chan_out_odometry) { + synapse_msgs_Odometry* msg = (synapse_msgs_Odometry*)(chan->message); + print_Odometry(msg); + } else if (chan == &chan_out_altimeter) { + synapse_msgs_Altimeter* msg = (synapse_msgs_Altimeter*)(chan->message); + print_Altimeter(msg); + } else if (chan == &chan_out_battery_state) { + synapse_msgs_BatteryState* msg = (synapse_msgs_BatteryState*)(chan->message); + print_BatteryState(msg); + } else if (chan == &chan_out_magnetic_field) { + synapse_msgs_MagneticField* msg = (synapse_msgs_MagneticField*)(chan->message); + print_MagneticField(msg); + } else if (chan == &chan_out_wheel_odometry) { + synapse_msgs_WheelOdometry* msg = (synapse_msgs_WheelOdometry*)(chan->message); + print_WheelOdometry(msg); + } else if (chan == &chan_in_joy) { + synapse_msgs_Joy* msg = (synapse_msgs_Joy*)(chan->message); + print_Joy(msg); + } else if (chan == &chan_in_cmd_vel) { + synapse_msgs_Twist msg = *(synapse_msgs_Twist*)(chan->message); + print_Twist(&msg); + } else if (chan == &chan_in_bezier_trajectory) { + synapse_msgs_BezierTrajectory* msg = (synapse_msgs_BezierTrajectory*)(chan->message); + print_BezierTrajectory(msg); + } else { + shell_print(g_sh, "%s printing not implemented", zbus_chan_name(chan)); + } + + // disable listener + int ret = zbus_obs_set_enable(&listener_synapse_topic, false); + if (ret != 0) { + shell_print(g_sh, "could not disable observer: %d", ret); + } + } +} + +static bool print_channel_name_iterator(const struct zbus_channel* chan) +{ + shell_print(g_sh, "%s", zbus_chan_name(chan) + 5); + return true; +} + +static int topic_list(const struct shell* sh, + size_t argc, char** argv, void* data) +{ + g_sh = sh; + zbus_iterate_over_channels(print_channel_name_iterator); + return 0; +} + +static int topic_echo(const struct shell* sh, + size_t argc, char** argv, void* data) +{ + g_sh = sh; + g_chan_echo = data; + g_listener_mode = LISTENER_MODE_ECHO; + int ret = zbus_obs_set_enable(&listener_synapse_topic, true); + if (ret != 0) { + shell_print(g_sh, "could not enable observer: %d", ret); + } + return 0; +} + +static int topic_info(const struct shell* sh, + size_t argc, char** argv, void* data) +{ + const struct zbus_channel* chan = data; + shell_print(sh, "channel %s:", zbus_chan_name(chan)); + shell_print(sh, "message size: %d", zbus_chan_msg_size(chan)); + shell_print(sh, " observers:"); + for (const struct zbus_observer* const* obs = chan->observers; *obs != NULL; ++obs) { + shell_print(sh, " - %s", zbus_obs_name(*obs)); + } + + return 0; +} + +static int topic_hz(const struct shell* sh, + size_t argc, char** argv, void* data) +{ + g_sh = sh; + g_chan_echo = data; + g_chan_msg_count = 0; + g_listener_mode = LISTENER_MODE_HZ; + int ret = zbus_obs_set_enable(&listener_synapse_topic, true); + if (ret != 0) { + shell_print(g_sh, "could not enable observer: %d", ret); + } + k_msleep(2000); + shell_print(g_sh, "Hz: %10.2f", g_chan_msg_count / 2.0f); + return 0; +} + +#define TOPIC_DICTIONARY() \ + (in_actuators, &chan_in_actuators, "in_actuators"), \ + (in_bezier_trajectory, &chan_in_bezier_trajectory, "in_bezier_trajectory"), \ + (in_clock_offset, &chan_in_clock_offset, "in_clock_offset"), \ + (in_cmd_vel, &chan_in_cmd_vel, "in_cmd_vel"), \ + (in_joy, &chan_in_joy, "in_joy"), \ + (in_nav_sat_fix, &chan_in_nav_sat_fix, "in_nav_sat_fix"), \ + (in_odometry, &chan_in_odometry, "in_odometry"), \ + (out_actuators, &chan_out_actuators, "out_actuators"), \ + (out_altimeter, &chan_out_altimeter, "out_altimeter"), \ + (out_battery_state, &chan_out_battery_state, "out_battery_state"), \ + (out_imu, &chan_out_imu, "out_imu"), \ + (out_magnetic_field, &chan_out_magnetic_field, "out_magnetic_field"), \ + (out_nav_sat_fix, &chan_out_nav_sat_fix, "out_nav_sat_fix"), \ + (out_odometry, &chan_out_odometry, "out_odometry"), \ + (out_wheel_odometry, &chan_out_wheel_odometry, "out_wheel_odometry") + +/* Creating subcommands (level 2 command) dict for command "topic echo". */ +SHELL_SUBCMD_DICT_SET_CREATE(sub_topic_echo, topic_echo, TOPIC_DICTIONARY()); + +/* Creating subcommands (level 2 command) dict for command "topic info". */ +SHELL_SUBCMD_DICT_SET_CREATE(sub_topic_info, topic_info, TOPIC_DICTIONARY()); + +/* Creating subcommands (level 2 command) dict for command "topic hz". */ +SHELL_SUBCMD_DICT_SET_CREATE(sub_topic_hz, topic_hz, TOPIC_DICTIONARY()); + +/* Creating subcommands (level 1 command) array for command "topic". */ +SHELL_STATIC_SUBCMD_SET_CREATE(sub_topic, + SHELL_CMD(list, NULL, "list topics", topic_list), + SHELL_CMD(info, &sub_topic_info, "show topic info", NULL), + SHELL_CMD(echo, &sub_topic_echo, "echo topic data once", NULL), + SHELL_CMD(hz, &sub_topic_hz, "topic pub rate", NULL), + SHELL_SUBCMD_SET_END); + +/* Creating root (level 0) command "topic" */ +SHELL_CMD_REGISTER(topic, &sub_topic, "topic command", NULL); + +/* vi: ts=4 sw=4 et: */ diff --git a/lib/synapse/uart/Kconfig b/lib/synapse/uart/Kconfig deleted file mode 100644 index 16b81d2d..00000000 --- a/lib/synapse/uart/Kconfig +++ /dev/null @@ -1,15 +0,0 @@ -# Copyright (c) 2023, CogniPilot Foundation -# SPDX-License-Identifier: Apache-2.0 - -menuconfig SYNAPSE_UART - bool "UART" - help - This option enables the synapse uart interface - -if SYNAPSE_UART - -module = SYNAPSE_UART -module-str = synapse_uart -source "subsys/logging/Kconfig.template.log_config" - -endif # SYNAPSE_UART diff --git a/lib/synapse/uart/main.c b/lib/synapse/uart/main.c deleted file mode 100644 index 34d347e9..00000000 --- a/lib/synapse/uart/main.c +++ /dev/null @@ -1,137 +0,0 @@ -/* - * Copyright CogniPilot Foundation 2023 - * SPDX-License-Identifier: Apache-2.0 - */ -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "synapse/zbus/common.h" - -LOG_MODULE_REGISTER(synapse_uart, CONFIG_SYNAPSE_UART_LOG_LEVEL); - -#define MY_STACK_SIZE 200 -#define MY_PRIORITY 5 - -#define RX_BUF_SIZE 100 - -static const struct device* const uart_dev = DEVICE_DT_GET(DT_ALIAS(telem1)); - -static void write_uart(TinyFrame* tf, const uint8_t* buf, uint32_t len) -{ - for (int i = 0; i < len; i++) { - uart_poll_out(uart_dev, buf[i]); - } -} - -static TinyFrame g_tf = { - .write = write_uart, - .peer_bit = TF_MASTER -}; - -static TF_Result genericListener(TinyFrame* tf, TF_Msg* msg) -{ - dumpFrameInfo(msg); - return TF_STAY; -} - -/* - * Read characters from UART until line end is detected. Afterwards push the - * data to the message queue. - */ -#if !defined(CONFIG_ARCH_POSIX) -void serial_cb(const struct device* dev, void* user_data) -{ - uint8_t c; - - if (!uart_irq_update(uart_dev)) { - return; - } - - if (!uart_irq_rx_ready(uart_dev)) { - return; - } - - /* read until FIFO empty */ - while (uart_fifo_read(uart_dev, &c, 1) == 1) { - TF_AcceptChar(&g_tf, c); - } -} -#endif - -// ROS -> cerebri -TOPIC_LISTENER(in_actuators, synapse_msgs_Actuators) -TOPIC_LISTENER(in_bezier_trajectory, synapse_msgs_BezierTrajectory) -TOPIC_LISTENER(in_cmd_vel, synapse_msgs_Twist) -TOPIC_LISTENER(in_joy, synapse_msgs_Joy) -TOPIC_LISTENER(in_odometry, synapse_msgs_Odometry) - -void listener_synapse_uart_callback(const struct zbus_channel* chan) -{ - if (chan == NULL) { // start of if else statements for channel type - } - // cerebri -> ROS - TOPIC_PUBLISHER(out_actuators, synapse_msgs_Actuators, SYNAPSE_OUT_ACTUATORS_TOPIC) - TOPIC_PUBLISHER(out_odometry, synapse_msgs_Odometry, SYNAPSE_OUT_ODOMETRY_TOPIC) -} - -ZBUS_LISTENER_DEFINE(listener_synapse_uart, listener_synapse_uart_callback); - -static void uart_entry_point(void) -{ - // ros -> cerebri - TF_AddGenericListener(&g_tf, genericListener); - TF_AddTypeListener(&g_tf, SYNAPSE_IN_ACTUATORS_TOPIC, in_actuators_Listener); - TF_AddTypeListener(&g_tf, SYNAPSE_IN_BEZIER_TRAJECTORY_TOPIC, in_bezier_trajectory_Listener); - TF_AddTypeListener(&g_tf, SYNAPSE_IN_CMD_VEL_TOPIC, in_cmd_vel_Listener); - TF_AddTypeListener(&g_tf, SYNAPSE_IN_JOY_TOPIC, in_joy_Listener); - TF_AddTypeListener(&g_tf, SYNAPSE_IN_ODOMETRY_TOPIC, in_odometry_Listener); - -#if defined(CONFIG_ARCH_POSIX) - while (true) { - uint8_t c; - int count = 0; - while (uart_poll_in(uart_dev, &c) == 0) { - TF_AcceptChar(&g_tf, c); - count++; - } - TF_Tick(&g_tf); - } -#else // CONFIG_ARCH_POSIX - if (!device_is_ready(uart_dev)) { - LOG_ERR("UART device not found"); - return; - } - - /* configure interrupt and callback to receive data */ - int ret = uart_irq_callback_user_data_set(uart_dev, serial_cb, NULL); - - if (ret < 0) { - if (ret == -ENOTSUP) { - LOG_ERR("interrupt-driven UART API support not enabled"); - } else if (ret == -ENOSYS) { - LOG_ERR("UART device does not support interrupt-driven API"); - } else { - LOG_ERR("Error setting UART callback: %d", ret); - } - return; - } - uart_irq_rx_enable(uart_dev); - - while (true) { - k_msleep(1000); - TF_Tick(&g_tf); - } -#endif // CONFIG_ARCH_POSIX -} - -K_THREAD_DEFINE(synapse_uart, MY_STACK_SIZE, uart_entry_point, - NULL, NULL, NULL, MY_PRIORITY, 0, 0); - -/* vi: ts=4 sw=4 et */ diff --git a/lib/synapse/zbus/CMakeLists.txt b/lib/synapse/zbus/CMakeLists.txt index c2d59407..9a6a3c82 100644 --- a/lib/synapse/zbus/CMakeLists.txt +++ b/lib/synapse/zbus/CMakeLists.txt @@ -7,7 +7,6 @@ zephyr_library_named(synapse_zbus) zephyr_include_directories() zephyr_library_sources( - zbus_topic_list.c channels.c ) diff --git a/lib/synapse/zbus/Kconfig b/lib/synapse/zbus/Kconfig index 5b6c8573..144a7f1f 100644 --- a/lib/synapse/zbus/Kconfig +++ b/lib/synapse/zbus/Kconfig @@ -2,10 +2,14 @@ # SPDX-License-Identifier: Apache-2.0 config SYNAPSE_ZBUS - bool "ZBUS" + bool "Enables zbus channels" help - This option enables the synapse zbus interface + This option enables the synapse zbus channels if SYNAPSE_ZBUS +module = SYNAPSE_ZBUS +module-str = synapse_zbus +source "subsys/logging/Kconfig.template.log_config" + endif # SYNAPSE_ZBUS diff --git a/lib/synapse/zbus/channels.c b/lib/synapse/zbus/channels.c index 40e89c76..f5753345 100644 --- a/lib/synapse/zbus/channels.c +++ b/lib/synapse/zbus/channels.c @@ -7,7 +7,11 @@ ZBUS_CHAN_DEFINE(chan_in_actuators, // Name synapse_msgs_Actuators, // Message type NULL, // Validator NULL, // User Data - ZBUS_OBSERVERS(), // observers + ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif + ), // observers ZBUS_MSG_INIT(0) // Initial value {0} ); @@ -16,6 +20,9 @@ ZBUS_CHAN_DEFINE(chan_in_bezier_trajectory, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #if defined(CONFIG_CONTROL_ACKERMANN) listener_control_ackermann, #elif defined(CONFIG_CONTROL_DIFFDRIVE) @@ -30,6 +37,9 @@ ZBUS_CHAN_DEFINE(chan_in_clock_offset, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #if defined(CONFIG_CONTROL_ACKERMANN) listener_control_ackermann, #elif defined(CONFIG_CONTROL_DIFFDRIVE) @@ -44,6 +54,9 @@ ZBUS_CHAN_DEFINE(chan_in_cmd_vel, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #if defined(CONFIG_CONTROL_ACKERMANN) listener_control_ackermann, #elif defined(CONFIG_CONTROL_DIFFDRIVE) @@ -58,6 +71,9 @@ ZBUS_CHAN_DEFINE(chan_in_joy, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #if defined(CONFIG_CONTROL_ACKERMANN) listener_control_ackermann, #elif defined(CONFIG_CONTROL_DIFFDRIVE) @@ -72,11 +88,11 @@ ZBUS_CHAN_DEFINE(chan_in_nav_sat_fix, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #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} @@ -87,6 +103,9 @@ ZBUS_CHAN_DEFINE(chan_in_odometry, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #if defined(CONFIG_CONTROL_ACKERMANN) listener_control_ackermann, #elif defined(CONFIG_CONTROL_DIFFDRIVE) @@ -105,6 +124,9 @@ ZBUS_CHAN_DEFINE(chan_out_actuators, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #if defined(CONFIG_ACTUATE_PWM) listener_actuator_pwm, #endif @@ -117,9 +139,6 @@ ZBUS_CHAN_DEFINE(chan_out_actuators, // Name #if defined(CONFIG_SYNAPSE_ETHERNET) listener_synapse_ethernet, #endif -#if defined(CONFIG_SYNAPSE_UART) - listener_synapse_uart, -#endif #if defined(CONFIG_ESTIMATE_ROVER2D) listener_estimate_rover2d, #endif @@ -132,6 +151,9 @@ ZBUS_CHAN_DEFINE(chan_out_altimeter, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #if defined(CONFIG_ESTIMATE_ATTITUDE) listener_estimate_attitude, #endif @@ -144,6 +166,9 @@ ZBUS_CHAN_DEFINE(chan_out_battery_state, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #if defined(CONFIG_CONTROL_ACKERMANN) listener_control_ackermann, #elif defined(CONFIG_CONTROL_DIFFDRIVE) @@ -158,6 +183,9 @@ ZBUS_CHAN_DEFINE(chan_out_imu, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #if defined(CONFIG_ESTIMATE_ATTITUDE) listener_estimate_attitude, #endif @@ -170,6 +198,9 @@ ZBUS_CHAN_DEFINE(chan_out_magnetic_field, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #if defined(CONFIG_ESTIMATE_ATTITUDE) listener_estimate_attitude, #endif @@ -182,6 +213,9 @@ ZBUS_CHAN_DEFINE(chan_out_nav_sat_fix, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #if defined(CONFIG_ESTIMATE_ATTITUDE) listener_estimate_attitude, #endif @@ -194,11 +228,11 @@ ZBUS_CHAN_DEFINE(chan_out_odometry, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #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} @@ -209,6 +243,9 @@ ZBUS_CHAN_DEFINE(chan_out_wheel_odometry, // Name NULL, // Validator NULL, // User Data ZBUS_OBSERVERS( +#if defined(CONFIG_SYNAPSE_TOPIC) + listener_synapse_topic, +#endif #if defined(CONFIG_ESTIMATE_ROVER2D) listener_estimate_rover2d, #endif diff --git a/lib/synapse/zbus/zbus_topic_list.c b/lib/synapse/zbus/zbus_topic_list.c deleted file mode 100644 index 207cca77..00000000 --- a/lib/synapse/zbus/zbus_topic_list.c +++ /dev/null @@ -1,64 +0,0 @@ -/* - * Copyright (c) 2022 Rodrigo Peixoto - * SPDX-License-Identifier: Apache-2.0 - */ - -#include - -#include -#include -#include -#include - -#define MY_STACK_SIZE 500 -#define MY_PRIORITY 5 - -LOG_MODULE_REGISTER(zbus_topic_list, LOG_LEVEL_INF); -static int count = 0; - -#if defined(CONFIG_ZBUS_STRUCTS_ITERABLE_ACCESS) -static bool print_channel_data_iterator(const struct zbus_channel* chan) -{ - LOG_INF("%d - Channel %s:", count, zbus_chan_name(chan)); - LOG_INF(" Message size: %d", zbus_chan_msg_size(chan)); - ++count; - LOG_INF(" Observers:"); - for (const struct zbus_observer* const* obs = chan->observers; *obs != NULL; ++obs) { - LOG_INF(" - %s", (*obs)->name); - } - - return true; -} - -static bool print_observer_data_iterator(const struct zbus_observer* obs) -{ - LOG_INF("%d - %s %s", count, obs->queue ? "Subscriber" : "Listener", zbus_obs_name(obs)); - - ++count; - - return true; -} -#endif // CONFIG_ZBUS_STRUCTS_ITERABLE_ACCESS - -static int zbus_topic_list(const struct shell* shell, - size_t argc, char** argv, void* data) -{ - count = 0; - -#if defined(CONFIG_ZBUS_STRUCTS_ITERABLE_ACCESS) - LOG_INF("Channel list:"); - zbus_iterate_over_channels(print_channel_data_iterator); - - count = 0; - - LOG_INF("Observers list:"); - zbus_iterate_over_observers(print_observer_data_iterator); -#endif // CONFIG_ZBUS_STRUCTS_ITERABLE_ACCESS - return 0; -} - -/* Creating subcommands (level 1 command) array for command "demo". */ -SHELL_STATIC_SUBCMD_SET_CREATE(sub_zbus, SHELL_CMD(topic, NULL, "List topics.", zbus_topic_list), - SHELL_SUBCMD_SET_END); -/* Creating root (level 0) command "demo" */ -SHELL_CMD_REGISTER(zbus, &sub_zbus, "zbus command", NULL);