From 6d2cac588f7a11cc1ec2c337e29fc00432d18fcd Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Thu, 28 Jan 2021 17:43:24 +0100 Subject: [PATCH 01/18] baseline rbs demo --- examples/uros_rbs/Kconfig | 17 +++++ examples/uros_rbs/Make.defs | 3 + examples/uros_rbs/Makefile | 25 +++++++ examples/uros_rbs/app.c | 143 ++++++++++++++++++++++++++++++++++++ 4 files changed, 188 insertions(+) create mode 100644 examples/uros_rbs/Kconfig create mode 100644 examples/uros_rbs/Make.defs create mode 100644 examples/uros_rbs/Makefile create mode 100644 examples/uros_rbs/app.c diff --git a/examples/uros_rbs/Kconfig b/examples/uros_rbs/Kconfig new file mode 100644 index 00000000..5566c7a8 --- /dev/null +++ b/examples/uros_rbs/Kconfig @@ -0,0 +1,17 @@ +config UROS_RBS_EXAMPLE + bool "micro-ROS RBS Demo" + default n + depends on UROS + ---help--- + micro-ROS RBS demo app + +if UROS_RBS_EXAMPLE + +config UROS_RBS_EXAMPLE_PROGNAME + string "Program name" + default "uros_rbs" + depends on BUILD_KERNEL + ---help--- + Reservation based scheduling demo. + +endif diff --git a/examples/uros_rbs/Make.defs b/examples/uros_rbs/Make.defs new file mode 100644 index 00000000..0414e3ce --- /dev/null +++ b/examples/uros_rbs/Make.defs @@ -0,0 +1,3 @@ +ifeq ($(CONFIG_UROS_RBS_EXAMPLE),y) + CONFIGURED_APPS += examples/uros_rbs +endif \ No newline at end of file diff --git a/examples/uros_rbs/Makefile b/examples/uros_rbs/Makefile new file mode 100644 index 00000000..f629cf58 --- /dev/null +++ b/examples/uros_rbs/Makefile @@ -0,0 +1,25 @@ +-include $(TOPDIR)/Make.defs + +#Set the scheduler priority for the app. +CONFIG_UROS_RBS_EXAMPLE_PRIORITY ?= SCHED_PRIORITY_DEFAULT +#Set the stack size to the app. The minimum stack size on NuttX for a micro-ROS App is 65000 bytes +CONFIG_UROS_RBS_EXAMPLE_STACKSIZE ?= 65000 + +#This is the name of the app on the NSH console +APPNAME = uros_rbs +PRIORITY = $(CONFIG_UROS_RBS_EXAMPLE_PRIORITY) +STACKSIZE = $(CONFIG_UROS_RBS_EXAMPLE_STACKSIZE) + +#Add the source files. +ASRCS = +CSRCS = +MAINSRC = app.c + +CONFIG_UROS_RBS_EXAMPLE_PROGNAME ?= uros_rbs$(EXEEXT) +PROGNAME = $(CONFIG_UROS_RBS_EXAMPLE_PROGNAME) +UROS_RBS_INCLUDES = $(shell find $(APPDIR)/$(CONFIG_UROS_DIR)/install -type d -name include) +CFLAGS += ${shell $(INCDIR) $(INCDIROPT) "$(CC)" "$(UROS_RBS_INCLUDES)"} -std=c99 + +MODULE = CONFIG_UROS_RBS_EXAMPLE + +include $(APPDIR)/Application.mk \ No newline at end of file diff --git a/examples/uros_rbs/app.c b/examples/uros_rbs/app.c new file mode 100644 index 00000000..b87a0f95 --- /dev/null +++ b/examples/uros_rbs/app.c @@ -0,0 +1,143 @@ +#include +#include +#include +#include + +#include + +#include +#include +#include + +#define STRING_BUFFER_LEN 100 + +#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}} +#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}} + +rcl_publisher_t ping_publisher; +rcl_publisher_t pong_publisher; +rcl_subscription_t ping_subscriber; +rcl_subscription_t pong_subscriber; + +std_msgs__msg__Header incoming_ping; +std_msgs__msg__Header outcoming_ping; +std_msgs__msg__Header incoming_pong; + +int device_id; +int seq_no; +int pong_count; + +void ping_timer_callback(rcl_timer_t * timer, int64_t last_call_time) +{ + (void) last_call_time; + + if (timer != NULL) { + + seq_no = rand(); + sprintf(outcoming_ping.frame_id.data, "%d_%d", seq_no, device_id); + outcoming_ping.frame_id.size = strlen(outcoming_ping.frame_id.data); + + // Fill the message timestamp + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + outcoming_ping.stamp.sec = ts.tv_sec; + outcoming_ping.stamp.nanosec = ts.tv_nsec; + + // Reset the pong count and publish the ping message + pong_count = 0; + rcl_publish(&ping_publisher, (const void*)&outcoming_ping, NULL); + printf("Ping send seq %s\n", outcoming_ping.frame_id.data); + } +} + +void ping_subscription_callback(const void * msgin) +{ + const std_msgs__msg__Header * msg = (const std_msgs__msg__Header *)msgin; + + // Dont pong my own pings + if(strcmp(outcoming_ping.frame_id.data, msg->frame_id.data) != 0){ + printf("Ping received with seq %s. Answering.\n", msg->frame_id.data); + rcl_publish(&pong_publisher, (const void*)msg, NULL); + } +} + + +void pong_subscription_callback(const void * msgin) +{ + const std_msgs__msg__Header * msg = (const std_msgs__msg__Header *)msgin; + + if(strcmp(outcoming_ping.frame_id.data, msg->frame_id.data) == 0) { + pong_count++; + printf("Pong for seq %s (%d)\n", msg->frame_id.data, pong_count); + } +} + + +#if defined(BUILD_MODULE) +int main(int argc, char *argv[]) +#else +int uros_rbs_main(int argc, char* argv[]) +#endif +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + rclc_support_t support; + + // create init_options + RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); + + // create node + rcl_node_t node = rcl_get_zero_initialized_node(); + RCCHECK(rclc_node_init_default(&node, "pingpong_node", "", &support)); + + // Create a reliable ping publisher + RCCHECK(rclc_publisher_init_default(&ping_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping")); + + // Create a best effort pong publisher + RCCHECK(rclc_publisher_init_best_effort(&pong_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong")); + + // Create a best effort ping subscriber + RCCHECK(rclc_subscription_init_best_effort(&ping_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping")); + + // Create a best effort pong subscriber + RCCHECK(rclc_subscription_init_best_effort(&pong_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong")); + + + // Create a 3 seconds ping timer timer, + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(2000), ping_timer_callback)); + + + // Create executor + rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); + RCCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator)); + + unsigned int rcl_wait_timeout = 1000; // in ms + RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout))); + RCCHECK(rclc_executor_add_timer(&executor, &timer)); + RCCHECK(rclc_executor_add_subscription(&executor, &ping_subscriber, &incoming_ping, &ping_subscription_callback, ON_NEW_DATA)); + RCCHECK(rclc_executor_add_subscription(&executor, &pong_subscriber, &incoming_pong, &pong_subscription_callback, ON_NEW_DATA)); + + // Create and allocate the pingpong messages + + char outcoming_ping_buffer[STRING_BUFFER_LEN]; + outcoming_ping.frame_id.data = outcoming_ping_buffer; + outcoming_ping.frame_id.capacity = STRING_BUFFER_LEN; + + char incoming_ping_buffer[STRING_BUFFER_LEN]; + incoming_ping.frame_id.data = incoming_ping_buffer; + incoming_ping.frame_id.capacity = STRING_BUFFER_LEN; + + char incoming_pong_buffer[STRING_BUFFER_LEN]; + incoming_pong.frame_id.data = incoming_pong_buffer; + incoming_pong.frame_id.capacity = STRING_BUFFER_LEN; + + device_id = rand(); + + rclc_executor_spin(&executor); + + RCCHECK(rcl_publisher_fini(&ping_publisher, &node)); + RCCHECK(rcl_publisher_fini(&pong_publisher, &node)); + RCCHECK(rcl_subscription_fini(&ping_subscriber, &node)); + RCCHECK(rcl_subscription_fini(&pong_subscriber, &node)); + RCCHECK(rcl_node_fini(&node)); +} \ No newline at end of file From 5a42931219aee2b06a79fc063c530b504e3d6b2e Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Thu, 28 Jan 2021 18:06:44 +0100 Subject: [PATCH 02/18] changed application source code file, clashes with app.c from pingpong example. The app.o will be replaced if the same is used. --- examples/uros_rbs/Makefile | 2 +- examples/uros_rbs/{app.c => main_rbs.c} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename examples/uros_rbs/{app.c => main_rbs.c} (100%) diff --git a/examples/uros_rbs/Makefile b/examples/uros_rbs/Makefile index f629cf58..dd3f9f69 100644 --- a/examples/uros_rbs/Makefile +++ b/examples/uros_rbs/Makefile @@ -13,7 +13,7 @@ STACKSIZE = $(CONFIG_UROS_RBS_EXAMPLE_STACKSIZE) #Add the source files. ASRCS = CSRCS = -MAINSRC = app.c +MAINSRC = main_rbs.c CONFIG_UROS_RBS_EXAMPLE_PROGNAME ?= uros_rbs$(EXEEXT) PROGNAME = $(CONFIG_UROS_RBS_EXAMPLE_PROGNAME) diff --git a/examples/uros_rbs/app.c b/examples/uros_rbs/main_rbs.c similarity index 100% rename from examples/uros_rbs/app.c rename to examples/uros_rbs/main_rbs.c From 7fa046e066697239de924382d419085ef07fbee9 Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Thu, 28 Jan 2021 18:17:24 +0100 Subject: [PATCH 03/18] function shaddowed the onces in uros_pingpong - made them static, watch out for global variables, which are also defined multiple times - this version compiles and runs on the Olimex board. test passed. --- examples/uros_rbs/main_rbs.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index b87a0f95..fdd05faa 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -27,6 +27,7 @@ int device_id; int seq_no; int pong_count; +static void ping_timer_callback(rcl_timer_t * timer, int64_t last_call_time) { (void) last_call_time; @@ -50,6 +51,7 @@ void ping_timer_callback(rcl_timer_t * timer, int64_t last_call_time) } } +static void ping_subscription_callback(const void * msgin) { const std_msgs__msg__Header * msg = (const std_msgs__msg__Header *)msgin; @@ -61,7 +63,7 @@ void ping_subscription_callback(const void * msgin) } } - +static void pong_subscription_callback(const void * msgin) { const std_msgs__msg__Header * msg = (const std_msgs__msg__Header *)msgin; From afae8ab0141cf5d52be99785defc599864a73f39 Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Mon, 1 Feb 2021 20:04:15 +0100 Subject: [PATCH 04/18] baseline pongnode --- examples/uros_rbs/main_rbs.c | 211 +++++++++++++++++------------------ 1 file changed, 102 insertions(+), 109 deletions(-) diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index fdd05faa..034bdd4d 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -1,145 +1,138 @@ +// Copyright (c) 2020 Robert Bosch GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#include +#include +#include +#include + +#include + #include #include #include #include +#include -#include - -#include -#include -#include #define STRING_BUFFER_LEN 100 #define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}} #define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}} +#define RCUNUSED(fn) { rcl_ret_t temp_rc __attribute__((unused)); temp_rc = fn; } -rcl_publisher_t ping_publisher; -rcl_publisher_t pong_publisher; -rcl_subscription_t ping_subscriber; -rcl_subscription_t pong_subscriber; -std_msgs__msg__Header incoming_ping; -std_msgs__msg__Header outcoming_ping; -std_msgs__msg__Header incoming_pong; +rcl_subscription_t high_ping_subscription_; +std_msgs__msg__Int32 high_ping_msg_; +rcl_publisher_t high_pong_publisher_; +rcl_subscription_t low_ping_subscription_; +std_msgs__msg__Int32 low_ping_msg_; +rcl_publisher_t low_pong_publisher_; -int device_id; -int seq_no; -int pong_count; -static -void ping_timer_callback(rcl_timer_t * timer, int64_t last_call_time) +void burn_cpu_cycles(long duration) { - (void) last_call_time; - - if (timer != NULL) { - - seq_no = rand(); - sprintf(outcoming_ping.frame_id.data, "%d_%d", seq_no, device_id); - outcoming_ping.frame_id.size = strlen(outcoming_ping.frame_id.data); - - // Fill the message timestamp - struct timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); - outcoming_ping.stamp.sec = ts.tv_sec; - outcoming_ping.stamp.nanosec = ts.tv_nsec; - - // Reset the pong count and publish the ping message - pong_count = 0; - rcl_publish(&ping_publisher, (const void*)&outcoming_ping, NULL); - printf("Ping send seq %s\n", outcoming_ping.frame_id.data); - } + if (duration > 0) { + clockid_t clockId; + clockId = CLOCK_REALTIME; + // clock_getcpuclockid(pthread_self(), &clockId); + // NuttX does not implement this function + // -lrt is not available , I did not find this function in + // NuttX subfolders + // adding EXTRA_LIBS to the Makefile did not resolve the error. + struct timespec startTimeP; + clock_gettime(clockId, &startTimeP); + int x = 0; + bool doAgain = true; + while (doAgain) { + while (x != rand() && x % 10 != 0) { + x++; + } + struct timespec currentTimeP; + clock_gettime(clockId, ¤tTimeP); + long currentDuration = (currentTimeP.tv_sec - startTimeP.tv_sec) * 1000000000 + (currentTimeP.tv_nsec - startTimeP.tv_nsec); + doAgain = (currentDuration < duration); + } + } } -static -void ping_subscription_callback(const void * msgin) -{ - const std_msgs__msg__Header * msg = (const std_msgs__msg__Header *)msgin; - // Dont pong my own pings - if(strcmp(outcoming_ping.frame_id.data, msg->frame_id.data) != 0){ - printf("Ping received with seq %s. Answering.\n", msg->frame_id.data); - rcl_publish(&pong_publisher, (const void*)msg, NULL); - } +void high_ping_received(const void * pong_msg) +{ + burn_cpu_cycles(100000000); // TODO: Get this value from parameter 'high_busyloop'. + printf("high ping received.\n"); + RCUNUSED(rcl_publish(&high_pong_publisher_, pong_msg, NULL)); } -static -void pong_subscription_callback(const void * msgin) -{ - const std_msgs__msg__Header * msg = (const std_msgs__msg__Header *)msgin; - if(strcmp(outcoming_ping.frame_id.data, msg->frame_id.data) == 0) { - pong_count++; - printf("Pong for seq %s (%d)\n", msg->frame_id.data, pong_count); - } +void low_ping_received(const void * pong_msg) +{ + burn_cpu_cycles(100000000); // TODO: Get this value from parameter 'low_busyloop'. + printf("low ping received.\n"); + RCUNUSED(rcl_publish(&low_pong_publisher_, pong_msg, NULL)); } - #if defined(BUILD_MODULE) int main(int argc, char *argv[]) #else int uros_rbs_main(int argc, char* argv[]) #endif { - rcl_allocator_t allocator = rcl_get_default_allocator(); - rclc_support_t support; - - // create init_options - RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); - - // create node - rcl_node_t node = rcl_get_zero_initialized_node(); - RCCHECK(rclc_node_init_default(&node, "pingpong_node", "", &support)); - - // Create a reliable ping publisher - RCCHECK(rclc_publisher_init_default(&ping_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping")); - - // Create a best effort pong publisher - RCCHECK(rclc_publisher_init_best_effort(&pong_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong")); - - // Create a best effort ping subscriber - RCCHECK(rclc_subscription_init_best_effort(&ping_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/ping")); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rclc_support_t support; + printf("Welcome to RBS Demo!\n"); + RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); - // Create a best effort pong subscriber - RCCHECK(rclc_subscription_init_best_effort(&pong_subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Header), "/microROS/pong")); + rcl_node_t node = rcl_get_zero_initialized_node(); + RCCHECK(rclc_node_init_default(&node, "pong_rclc_node", "", &support)); + RCCHECK(rclc_publisher_init_best_effort(&high_pong_publisher_, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "high_pong")); + RCCHECK(rclc_subscription_init_best_effort(&high_ping_subscription_, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "high_ping")); - // Create a 3 seconds ping timer timer, - rcl_timer_t timer = rcl_get_zero_initialized_timer(); - RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(2000), ping_timer_callback)); + RCCHECK(rclc_publisher_init_best_effort(&low_pong_publisher_, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "low_pong")); + RCCHECK(rclc_subscription_init_best_effort(&low_ping_subscription_, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "low_ping")); + rclc_executor_t high_executor = rclc_executor_get_zero_initialized_executor(); + RCCHECK(rclc_executor_init(&high_executor, &support.context, 1, &allocator)); + RCCHECK(rclc_executor_add_subscription(&high_executor, &high_ping_subscription_, &high_ping_msg_, &high_ping_received, ON_NEW_DATA)); + RCCHECK(rclc_executor_add_subscription(&high_executor, &low_ping_subscription_, &low_ping_msg_, &low_ping_received, ON_NEW_DATA)); + // rclc_executor_t low_executor = rclc_executor_get_zero_initialized_executor(); + // RCCHECK(rclc_executor_init(&low_executor, &support.context, 1, &allocator)); + // RCCHECK(rclc_executor_add_subscription(&low_executor, &low_ping_subscription_, &low_ping_msg_, &low_ping_received, ON_NEW_DATA)); - // Create executor - rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); - RCCHECK(rclc_executor_init(&executor, &support.context, 3, &allocator)); + rclc_executor_spin(&high_executor); + // TODO: And in second thread call: rclc_executor_spin(&low_executor); - unsigned int rcl_wait_timeout = 1000; // in ms - RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(rcl_wait_timeout))); - RCCHECK(rclc_executor_add_timer(&executor, &timer)); - RCCHECK(rclc_executor_add_subscription(&executor, &ping_subscriber, &incoming_ping, &ping_subscription_callback, ON_NEW_DATA)); - RCCHECK(rclc_executor_add_subscription(&executor, &pong_subscriber, &incoming_pong, &pong_subscription_callback, ON_NEW_DATA)); + RCCHECK(rcl_subscription_fini(&high_ping_subscription_, &node)); + RCCHECK(rcl_publisher_fini(&high_pong_publisher_, &node)); + RCCHECK(rcl_subscription_fini(&low_ping_subscription_, &node)); + RCCHECK(rcl_publisher_fini(&low_pong_publisher_, &node)); - // Create and allocate the pingpong messages - - char outcoming_ping_buffer[STRING_BUFFER_LEN]; - outcoming_ping.frame_id.data = outcoming_ping_buffer; - outcoming_ping.frame_id.capacity = STRING_BUFFER_LEN; - - char incoming_ping_buffer[STRING_BUFFER_LEN]; - incoming_ping.frame_id.data = incoming_ping_buffer; - incoming_ping.frame_id.capacity = STRING_BUFFER_LEN; - - char incoming_pong_buffer[STRING_BUFFER_LEN]; - incoming_pong.frame_id.data = incoming_pong_buffer; - incoming_pong.frame_id.capacity = STRING_BUFFER_LEN; - - device_id = rand(); - - rclc_executor_spin(&executor); - - RCCHECK(rcl_publisher_fini(&ping_publisher, &node)); - RCCHECK(rcl_publisher_fini(&pong_publisher, &node)); - RCCHECK(rcl_subscription_fini(&ping_subscriber, &node)); - RCCHECK(rcl_subscription_fini(&pong_subscriber, &node)); - RCCHECK(rcl_node_fini(&node)); -} \ No newline at end of file + RCCHECK(rcl_node_fini(&node)); +} +/* +arm-none-eabi-ld --entry=__start -nostartfiles -nodefaultlibs -g -T/home/jst3si/olimex_ws/firmware/NuttX/configs/olimex-stm32-e407/scripts/ld.script -L"/home/jst3si/olimex_ws/firmware/NuttX/staging" -L"/home/jst3si/olimex_ws/firmware/NuttX/arch/arm/src/board" -L "/usr/lib/gcc/arm-none-eabi/6.3.1/../../../arm-none-eabi/lib/thumb/v7e-m/fpv4-sp/hard" \ + -o "/home/jst3si/olimex_ws/firmware/NuttX/nuttx" \ + --start-group -lsched -ldrivers -lconfigs -lc -lmm -larch -lxx -lapps -lnet -lfs -lbinfmt -lxx -lboard -lsupc++ "/usr/lib/gcc/arm-none-eabi/6.3.1/thumb/v7e-m/fpv4-sp/hard/libgcc.a" --end-group +/home/jst3si/olimex_ws/firmware/NuttX/staging/libapps.a(main_rbs.o): In function `burn_cpu_cycles': +/home/jst3si/olimex_ws/firmware/apps/examples/uros_rbs/main_rbs.c:49: undefined reference to `clock_getcpuclockid' +Makefile:184: recipe for target 'nuttx' failed +make[1]: *** [nuttx] Error 1 +make[1]: Verzeichnis „/home/jst3si/olimex_ws/firmware/NuttX/arch/arm/src“ wird verlassen +tools/Makefile.unix:422: recipe for target 'pass2' failed +make: *** [pass2] Error 2 + + +*/ From 3332d5b4f8a7b17bc3312cce05eca0d26d9cfb52 Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Tue, 2 Feb 2021 15:23:53 +0100 Subject: [PATCH 05/18] updated RCCHECK macro to print error message and to reset error message text. --- examples/uros_rbs/main_rbs.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index 034bdd4d..c7add7fd 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -29,7 +29,9 @@ #define STRING_BUFFER_LEN 100 -#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}} +#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){\ + printf("Failed status on line %d: %d. Message: %s, Aborting.\n",__LINE__,(int)temp_rc, rcl_get_error_string().str);\ + rcutils_reset_error(); return 1;}} #define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}} #define RCUNUSED(fn) { rcl_ret_t temp_rc __attribute__((unused)); temp_rc = fn; } From 47b7e43e6b6aedcdff099654f27991fb9e0604b0 Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Tue, 2 Feb 2021 15:24:53 +0100 Subject: [PATCH 06/18] corrected number of handles. added fini method for executor. --- examples/uros_rbs/main_rbs.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index c7add7fd..2a042ca1 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -107,7 +107,7 @@ int uros_rbs_main(int argc, char* argv[]) RCCHECK(rclc_subscription_init_best_effort(&low_ping_subscription_, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "low_ping")); rclc_executor_t high_executor = rclc_executor_get_zero_initialized_executor(); - RCCHECK(rclc_executor_init(&high_executor, &support.context, 1, &allocator)); + RCCHECK(rclc_executor_init(&high_executor, &support.context, 2, &allocator)); RCCHECK(rclc_executor_add_subscription(&high_executor, &high_ping_subscription_, &high_ping_msg_, &high_ping_received, ON_NEW_DATA)); RCCHECK(rclc_executor_add_subscription(&high_executor, &low_ping_subscription_, &low_ping_msg_, &low_ping_received, ON_NEW_DATA)); // rclc_executor_t low_executor = rclc_executor_get_zero_initialized_executor(); @@ -117,6 +117,7 @@ int uros_rbs_main(int argc, char* argv[]) rclc_executor_spin(&high_executor); // TODO: And in second thread call: rclc_executor_spin(&low_executor); + RCCHECK(rclc_executor_fini(&high_executor)); RCCHECK(rcl_subscription_fini(&high_ping_subscription_, &node)); RCCHECK(rcl_publisher_fini(&high_pong_publisher_, &node)); RCCHECK(rcl_subscription_fini(&low_ping_subscription_, &node)); From c1244a1f3f7fa1c2678533cdaf391e15dc8175e7 Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Tue, 2 Feb 2021 18:02:19 +0100 Subject: [PATCH 07/18] updated demo with scheduling priority. --- examples/uros_rbs/main_rbs.c | 34 +++++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index 2a042ca1..400f24a4 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -73,7 +73,7 @@ void burn_cpu_cycles(long duration) void high_ping_received(const void * pong_msg) { - burn_cpu_cycles(100000000); // TODO: Get this value from parameter 'high_busyloop'. + // burn_cpu_cycles(100000000); // TODO: Get this value from parameter 'high_busyloop'. printf("high ping received.\n"); RCUNUSED(rcl_publish(&high_pong_publisher_, pong_msg, NULL)); } @@ -81,7 +81,7 @@ void high_ping_received(const void * pong_msg) void low_ping_received(const void * pong_msg) { - burn_cpu_cycles(100000000); // TODO: Get this value from parameter 'low_busyloop'. + // burn_cpu_cycles(100000000); // TODO: Get this value from parameter 'low_busyloop'. printf("low ping received.\n"); RCUNUSED(rcl_publish(&low_pong_publisher_, pong_msg, NULL)); } @@ -108,15 +108,24 @@ int uros_rbs_main(int argc, char* argv[]) rclc_executor_t high_executor = rclc_executor_get_zero_initialized_executor(); RCCHECK(rclc_executor_init(&high_executor, &support.context, 2, &allocator)); - RCCHECK(rclc_executor_add_subscription(&high_executor, &high_ping_subscription_, &high_ping_msg_, &high_ping_received, ON_NEW_DATA)); - RCCHECK(rclc_executor_add_subscription(&high_executor, &low_ping_subscription_, &low_ping_msg_, &low_ping_received, ON_NEW_DATA)); - // rclc_executor_t low_executor = rclc_executor_get_zero_initialized_executor(); - // RCCHECK(rclc_executor_init(&low_executor, &support.context, 1, &allocator)); - // RCCHECK(rclc_executor_add_subscription(&low_executor, &low_ping_subscription_, &low_ping_msg_, &low_ping_received, ON_NEW_DATA)); - - rclc_executor_spin(&high_executor); - // TODO: And in second thread call: rclc_executor_spin(&low_executor); - + + rclc_executor_sched_param_t sparam_high, sparam_low; + sparam_high.priority = sched_get_priority_max(SCHED_FIFO); + sparam_low.priority = sched_get_priority_min(SCHED_FIFO); + printf("max prio %d min prio %d\n", SCHED_PRIORITY_MAX, SCHED_PRIORITY_MIN); + printf("high prio %d low prio %d\n", sparam_high.priority, sparam_low.priority); + + sparam_high.priority = 20; + sparam_low.priority = 10; + printf("high prio %d low prio %d\n", sparam_high.priority, sparam_low.priority); + + printf("subscription high ping \n"); + RCCHECK(rclc_executor_add_subscription_sched(&high_executor, &high_ping_subscription_, &high_ping_msg_, &high_ping_received, ON_NEW_DATA, &sparam_high)); + printf("subscription low ping \n"); + RCCHECK(rclc_executor_add_subscription_sched(&high_executor, &low_ping_subscription_, &low_ping_msg_, &low_ping_received, ON_NEW_DATA, &sparam_low)); + printf("start executor \n"); + RCCHECK(rclc_executor_start_multi_threading_for_nuttx(&high_executor)); + printf("clean up \n"); RCCHECK(rclc_executor_fini(&high_executor)); RCCHECK(rcl_subscription_fini(&high_ping_subscription_, &node)); RCCHECK(rcl_publisher_fini(&high_pong_publisher_, &node)); @@ -126,6 +135,9 @@ int uros_rbs_main(int argc, char* argv[]) RCCHECK(rcl_node_fini(&node)); } /* +Error message missing library rt for clock_getcpuclockid(pthread_self(), &clockId); +but this function is not implemented in NuttX! (01-02-2021) + arm-none-eabi-ld --entry=__start -nostartfiles -nodefaultlibs -g -T/home/jst3si/olimex_ws/firmware/NuttX/configs/olimex-stm32-e407/scripts/ld.script -L"/home/jst3si/olimex_ws/firmware/NuttX/staging" -L"/home/jst3si/olimex_ws/firmware/NuttX/arch/arm/src/board" -L "/usr/lib/gcc/arm-none-eabi/6.3.1/../../../arm-none-eabi/lib/thumb/v7e-m/fpv4-sp/hard" \ -o "/home/jst3si/olimex_ws/firmware/NuttX/nuttx" \ --start-group -lsched -ldrivers -lconfigs -lc -lmm -larch -lxx -lapps -lnet -lfs -lbinfmt -lxx -lboard -lsupc++ "/usr/lib/gcc/arm-none-eabi/6.3.1/thumb/v7e-m/fpv4-sp/hard/libgcc.a" --end-group From 0746a008311494f82e7e1b2abae999f843b9400c Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Tue, 2 Feb 2021 22:22:47 +0100 Subject: [PATCH 08/18] added simple pthread example --- examples/uros_rbs/main_rbs.c | 104 +++++++++++++++++++++++++++++++++-- 1 file changed, 98 insertions(+), 6 deletions(-) diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index 400f24a4..1bced508 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -86,6 +86,18 @@ void low_ping_received(const void * pong_msg) RCUNUSED(rcl_publish(&low_pong_publisher_, pong_msg, NULL)); } +void * thread_run(void * arg) +{ + int * value = (int *) arg; + printf("thread_run 1 %d\n",(*value)); +} + +void * thread_run2(void * arg) +{ + int * value = (int *) arg; + printf("thread_run 2 %d\n",(*value)); +} + #if defined(BUILD_MODULE) int main(int argc, char *argv[]) #else @@ -95,6 +107,85 @@ int uros_rbs_main(int argc, char* argv[]) rcl_allocator_t allocator = rcl_get_default_allocator(); rclc_support_t support; printf("Welcome to RBS Demo!\n"); +/* + struct sched_param param, param2; + pthread_attr_t attr, attr2; + pthread_t worker_thread, worker_thread2; + int ret; + + // Set the executor thread priority + param.sched_priority = SCHED_PRIORITY_DEFAULT; + ret = sched_setparam(0, ¶m); + if (ret < 0) + { + printf("uros_rbs: sched_setparam failed: %d\n" , ret); + return 1; + } + (void)pthread_attr_init(&attr); + (void)pthread_attr_setschedparam(&attr, ¶m); + // (void)pthread_attr_setstacksize(&attr, CONFIG_UROS_RBS_EXAMPLE_STACKSIZE); + int value = param.sched_priority; + ret = pthread_create(&worker_thread, &attr, &thread_run, &value); + if (ret != 0) + { + printf("uros_rbs: pthread_create failed: %d\n", ret); + return 1; + } + // second worker thread + param2.sched_priority = SCHED_PRIORITY_DEFAULT + 10; + (void)pthread_attr_init(&attr2); + (void)pthread_attr_setschedparam(&attr2, ¶m2); + // (void)pthread_attr_setstacksize(&attr, CONFIG_UROS_RBS_EXAMPLE_STACKSIZE); + int value2 = param2.sched_priority; + ret = pthread_create(&worker_thread2, &attr, thread_run2, &value2); + if (ret != 0) + { + printf("uros_rbs: pthread_create worker_thread2 failed: %d\n", ret); + return 1; + } + + + int i=0; + while(i<3){ + printf("sleeping\n"); + sleep(1); + i++; + } + + + ret = pthread_join(worker_thread, NULL); + if (ret != 0) + { + fprintf(stderr, "uros_rbs: pthread_join failed\n"); + } + else + { + printf("uros_rbs: worker_thread returned.\n"); + } + + ret = pthread_join(worker_thread2, NULL); + if (ret != 0) + { + fprintf(stderr, "uros_rbs: pthread_join worker_thread2 failed\n"); + } + else + { + printf("uros_rbs: worker_thread2 returned.\n"); + } +*/ + + struct sched_param exe_param; + int ret; + + // Set the executor thread priority + exe_param.sched_priority = 130; + ret = sched_setparam(0, &exe_param); + if (ret < 0) + { + printf("uros_rbs: sched_setparam failed: %d\n" , ret); + return 1; + } + RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); rcl_node_t node = rcl_get_zero_initialized_node(); @@ -110,13 +201,11 @@ int uros_rbs_main(int argc, char* argv[]) RCCHECK(rclc_executor_init(&high_executor, &support.context, 2, &allocator)); rclc_executor_sched_param_t sparam_high, sparam_low; - sparam_high.priority = sched_get_priority_max(SCHED_FIFO); - sparam_low.priority = sched_get_priority_min(SCHED_FIFO); + //sparam_high.priority = sched_get_priority_max(SCHED_FIFO); + //sparam_low.priority = sched_get_priority_min(SCHED_FIFO); printf("max prio %d min prio %d\n", SCHED_PRIORITY_MAX, SCHED_PRIORITY_MIN); - printf("high prio %d low prio %d\n", sparam_high.priority, sparam_low.priority); - - sparam_high.priority = 20; - sparam_low.priority = 10; + sparam_high.priority = 120; + sparam_low.priority = 100; printf("high prio %d low prio %d\n", sparam_high.priority, sparam_low.priority); printf("subscription high ping \n"); @@ -125,6 +214,7 @@ int uros_rbs_main(int argc, char* argv[]) RCCHECK(rclc_executor_add_subscription_sched(&high_executor, &low_ping_subscription_, &low_ping_msg_, &low_ping_received, ON_NEW_DATA, &sparam_low)); printf("start executor \n"); RCCHECK(rclc_executor_start_multi_threading_for_nuttx(&high_executor)); + printf("clean up \n"); RCCHECK(rclc_executor_fini(&high_executor)); RCCHECK(rcl_subscription_fini(&high_ping_subscription_, &node)); @@ -133,6 +223,8 @@ int uros_rbs_main(int argc, char* argv[]) RCCHECK(rcl_publisher_fini(&low_pong_publisher_, &node)); RCCHECK(rcl_node_fini(&node)); + + return 0; } /* Error message missing library rt for clock_getcpuclockid(pthread_self(), &clockId); From 923ad8a5b1d944e406020086274c813ea308584e Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Wed, 3 Feb 2021 23:08:10 +0100 Subject: [PATCH 09/18] increased STACKSIZE to 68000 --- examples/uros_rbs/Makefile | 2 +- examples/uros_rbs/main_rbs.c | 15 +++++++++------ 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/examples/uros_rbs/Makefile b/examples/uros_rbs/Makefile index dd3f9f69..19f0752e 100644 --- a/examples/uros_rbs/Makefile +++ b/examples/uros_rbs/Makefile @@ -3,7 +3,7 @@ #Set the scheduler priority for the app. CONFIG_UROS_RBS_EXAMPLE_PRIORITY ?= SCHED_PRIORITY_DEFAULT #Set the stack size to the app. The minimum stack size on NuttX for a micro-ROS App is 65000 bytes -CONFIG_UROS_RBS_EXAMPLE_STACKSIZE ?= 65000 +CONFIG_UROS_RBS_EXAMPLE_STACKSIZE ?= 68000 #This is the name of the app on the NSH console APPNAME = uros_rbs diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index 1bced508..a103cb8e 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -86,6 +86,7 @@ void low_ping_received(const void * pong_msg) RCUNUSED(rcl_publish(&low_pong_publisher_, pong_msg, NULL)); } +/* void * thread_run(void * arg) { int * value = (int *) arg; @@ -97,7 +98,7 @@ void * thread_run2(void * arg) int * value = (int *) arg; printf("thread_run 2 %d\n",(*value)); } - +*/ #if defined(BUILD_MODULE) int main(int argc, char *argv[]) #else @@ -178,13 +179,15 @@ int uros_rbs_main(int argc, char* argv[]) int ret; // Set the executor thread priority - exe_param.sched_priority = 130; + /* + exe_param.sched_priority = 10; ret = sched_setparam(0, &exe_param); if (ret < 0) { printf("uros_rbs: sched_setparam failed: %d\n" , ret); return 1; } + */ RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); @@ -200,13 +203,13 @@ int uros_rbs_main(int argc, char* argv[]) rclc_executor_t high_executor = rclc_executor_get_zero_initialized_executor(); RCCHECK(rclc_executor_init(&high_executor, &support.context, 2, &allocator)); - rclc_executor_sched_param_t sparam_high, sparam_low; + struct sched_param sparam_high, sparam_low; //sparam_high.priority = sched_get_priority_max(SCHED_FIFO); //sparam_low.priority = sched_get_priority_min(SCHED_FIFO); printf("max prio %d min prio %d\n", SCHED_PRIORITY_MAX, SCHED_PRIORITY_MIN); - sparam_high.priority = 120; - sparam_low.priority = 100; - printf("high prio %d low prio %d\n", sparam_high.priority, sparam_low.priority); + sparam_high.sched_priority = 20; + sparam_low.sched_priority = 10; + printf("high prio %d low prio %d\n", sparam_high.sched_priority, sparam_low.sched_priority); printf("subscription high ping \n"); RCCHECK(rclc_executor_add_subscription_sched(&high_executor, &high_ping_subscription_, &high_ping_msg_, &high_ping_received, ON_NEW_DATA, &sparam_high)); From 46e524be5a0b1a175d922857a8a67525326148ca Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Thu, 4 Feb 2021 17:40:11 +0100 Subject: [PATCH 10/18] experimenting with STACKSIZE 68992 => output from nsh shell: nsh>uros_rbs => nsh:uros_rbs: command not found, added scheduling parameter for sporadic server, enabled CONFIG_SCHED_SPORADIC kernel variable. --- examples/uros_rbs/Makefile | 2 +- examples/uros_rbs/main_rbs.c | 28 ++++++++++++++++++++++++++-- 2 files changed, 27 insertions(+), 3 deletions(-) diff --git a/examples/uros_rbs/Makefile b/examples/uros_rbs/Makefile index 19f0752e..c4df261f 100644 --- a/examples/uros_rbs/Makefile +++ b/examples/uros_rbs/Makefile @@ -3,7 +3,7 @@ #Set the scheduler priority for the app. CONFIG_UROS_RBS_EXAMPLE_PRIORITY ?= SCHED_PRIORITY_DEFAULT #Set the stack size to the app. The minimum stack size on NuttX for a micro-ROS App is 65000 bytes -CONFIG_UROS_RBS_EXAMPLE_STACKSIZE ?= 68000 +CONFIG_UROS_RBS_EXAMPLE_STACKSIZE ?= 68992 #This is the name of the app on the NSH console APPNAME = uros_rbs diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index a103cb8e..719deb39 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -207,8 +207,32 @@ int uros_rbs_main(int argc, char* argv[]) //sparam_high.priority = sched_get_priority_max(SCHED_FIFO); //sparam_low.priority = sched_get_priority_min(SCHED_FIFO); printf("max prio %d min prio %d\n", SCHED_PRIORITY_MAX, SCHED_PRIORITY_MIN); - sparam_high.sched_priority = 20; - sparam_low.sched_priority = 10; + + bool sporadic_scheduling = false; + + sparam_high.sched_priority = 30; + + if(sporadic_scheduling){ + sparam_high.sched_priority = 30; + sparam_high.sched_ss_low_priority = 3; + sparam_high.sched_ss_repl_period.tv_sec = 5; + sparam_high.sched_ss_repl_period.tv_nsec = 0; + sparam_high.sched_ss_init_budget.tv_sec = 2; + sparam_high.sched_ss_init_budget.tv_nsec = 0; + sparam_high.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; + } + + sparam_low.sched_priority = 20; + if(sporadic_scheduling){ + sparam_low.sched_priority = 20; + sparam_low.sched_ss_low_priority = 2; + sparam_low.sched_ss_repl_period.tv_sec = 5; + sparam_low.sched_ss_repl_period.tv_nsec = 0; + sparam_low.sched_ss_init_budget.tv_sec = 1; + sparam_low.sched_ss_init_budget.tv_nsec = 0; + sparam_low.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; + } + printf("high prio %d low prio %d\n", sparam_high.sched_priority, sparam_low.sched_priority); printf("subscription high ping \n"); From 72f0e3a1d0a8eee2163faa44acef207e8b0cafc4 Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Thu, 4 Feb 2021 18:33:15 +0100 Subject: [PATCH 11/18] set STACKSIZE to 68625, enabled sporadic scheduling parameters. => this configuration works with ping-pong example. --- examples/uros_rbs/Makefile | 14 ++++++++++++-- examples/uros_rbs/main_rbs.c | 2 +- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/examples/uros_rbs/Makefile b/examples/uros_rbs/Makefile index c4df261f..c586386f 100644 --- a/examples/uros_rbs/Makefile +++ b/examples/uros_rbs/Makefile @@ -3,8 +3,18 @@ #Set the scheduler priority for the app. CONFIG_UROS_RBS_EXAMPLE_PRIORITY ?= SCHED_PRIORITY_DEFAULT #Set the stack size to the app. The minimum stack size on NuttX for a micro-ROS App is 65000 bytes -CONFIG_UROS_RBS_EXAMPLE_STACKSIZE ?= 68992 - +CONFIG_UROS_RBS_EXAMPLE_STACKSIZE ?= 68625 +# with CONFIG_SCHED_SPORADIC: +# 65000 => uros_rbs hangs after some while +# 68480 => uros_rbs hangs after some while +# 68600 => uros_rbs hangs after some while (after sending messages) +# 68625 => works! +# 68650 => nsh:uros_rbs: command not found +# 68698 => nsh:uros_rbs: command not found +# 68700 => nsh:uros_rbs: command not found +# 68800 => nsh:uros_rbs: command not found +# 69000 => nsh:uros_rbs: command not found +# #This is the name of the app on the NSH console APPNAME = uros_rbs PRIORITY = $(CONFIG_UROS_RBS_EXAMPLE_PRIORITY) diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index 719deb39..71dab0e6 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -208,7 +208,7 @@ int uros_rbs_main(int argc, char* argv[]) //sparam_low.priority = sched_get_priority_min(SCHED_FIFO); printf("max prio %d min prio %d\n", SCHED_PRIORITY_MAX, SCHED_PRIORITY_MIN); - bool sporadic_scheduling = false; + bool sporadic_scheduling = true; sparam_high.sched_priority = 30; From 8f2e0f6bfccc0d249deb730a36f17da1cb49b006 Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Mon, 8 Feb 2021 10:26:40 +0100 Subject: [PATCH 12/18] added colcon-meta file to configure microxrcedds. --- examples/uros_rbs/Makefile | 3 +- examples/uros_rbs/main_rbs.c | 179 +++++++++++++++++-------- examples/uros_rbs/uros_rbs-colcon.meta | 16 +++ 3 files changed, 138 insertions(+), 60 deletions(-) create mode 100644 examples/uros_rbs/uros_rbs-colcon.meta diff --git a/examples/uros_rbs/Makefile b/examples/uros_rbs/Makefile index c586386f..432632d9 100644 --- a/examples/uros_rbs/Makefile +++ b/examples/uros_rbs/Makefile @@ -3,14 +3,13 @@ #Set the scheduler priority for the app. CONFIG_UROS_RBS_EXAMPLE_PRIORITY ?= SCHED_PRIORITY_DEFAULT #Set the stack size to the app. The minimum stack size on NuttX for a micro-ROS App is 65000 bytes -CONFIG_UROS_RBS_EXAMPLE_STACKSIZE ?= 68625 +CONFIG_UROS_RBS_EXAMPLE_STACKSIZE ?= 68640 # with CONFIG_SCHED_SPORADIC: # 65000 => uros_rbs hangs after some while # 68480 => uros_rbs hangs after some while # 68600 => uros_rbs hangs after some while (after sending messages) # 68625 => works! # 68650 => nsh:uros_rbs: command not found -# 68698 => nsh:uros_rbs: command not found # 68700 => nsh:uros_rbs: command not found # 68800 => nsh:uros_rbs: command not found # 69000 => nsh:uros_rbs: command not found diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index 71dab0e6..3f182988 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -44,7 +44,7 @@ std_msgs__msg__Int32 low_ping_msg_; rcl_publisher_t low_pong_publisher_; -void burn_cpu_cycles(long duration) +void burn_cpu_cycles_high(long duration) { if (duration > 0) { clockid_t clockId; @@ -70,10 +70,37 @@ void burn_cpu_cycles(long duration) } } +void burn_cpu_cycles_low(long duration) +{ + if (duration > 0) { + clockid_t clockId; + clockId = CLOCK_REALTIME; + // clock_getcpuclockid(pthread_self(), &clockId); + // NuttX does not implement this function + // -lrt is not available , I did not find this function in + // NuttX subfolders + // adding EXTRA_LIBS to the Makefile did not resolve the error. + struct timespec startTimeP; + clock_gettime(clockId, &startTimeP); + int x = 0; + bool doAgain = true; + while (doAgain) { + while (x != rand() && x % 10 != 0) { + x++; + } + struct timespec currentTimeP; + clock_gettime(clockId, ¤tTimeP); + long currentDuration = (currentTimeP.tv_sec - startTimeP.tv_sec) * 1000000000 + (currentTimeP.tv_nsec - startTimeP.tv_nsec); + doAgain = (currentDuration < duration); + } + } +} + + void high_ping_received(const void * pong_msg) { - // burn_cpu_cycles(100000000); // TODO: Get this value from parameter 'high_busyloop'. + burn_cpu_cycles_high(10000000); // 10ms TODO: Get this value from parameter 'high_busyloop'. printf("high ping received.\n"); RCUNUSED(rcl_publish(&high_pong_publisher_, pong_msg, NULL)); } @@ -81,7 +108,7 @@ void high_ping_received(const void * pong_msg) void low_ping_received(const void * pong_msg) { - // burn_cpu_cycles(100000000); // TODO: Get this value from parameter 'low_busyloop'. + burn_cpu_cycles_low(10000000); // 10ms TODO: Get this value from parameter 'low_busyloop'. printf("low ping received.\n"); RCUNUSED(rcl_publish(&low_pong_publisher_, pong_msg, NULL)); } @@ -91,12 +118,58 @@ void * thread_run(void * arg) { int * value = (int *) arg; printf("thread_run 1 %d\n",(*value)); + int duration = 100000000; // 100ms + while(1) { + clockid_t clockId; + clockId = CLOCK_REALTIME; + // clock_getcpuclockid(pthread_self(), &clockId); + // NuttX does not implement this function + // -lrt is not available , I did not find this function in + // NuttX subfolders + // adding EXTRA_LIBS to the Makefile did not resolve the error. + struct timespec startTimeP; + clock_gettime(clockId, &startTimeP); + int x = 0; + bool doAgain = true; + while (doAgain) { + while (x != rand() && x % 10 != 0) { + x++; + } + struct timespec currentTimeP; + clock_gettime(clockId, ¤tTimeP); + long currentDuration = (currentTimeP.tv_sec - startTimeP.tv_sec) * 1000000000 + (currentTimeP.tv_nsec - startTimeP.tv_nsec); + doAgain = (currentDuration < duration); + } + } } +*/ +/* void * thread_run2(void * arg) { - int * value = (int *) arg; - printf("thread_run 2 %d\n",(*value)); + // int * value = (int *) arg; + int ret; + struct sched_param p; + ret = sched_setparam(0, &p); + if (ret < 0) + { + printf("uros_rbs: sched_getparam failed: %d\n" , ret); + return 1; + } + printf("busy-loop prio: %d\n",p.sched_priority); + unsigned int milliseconds = 100; + while(1) + { + volatile unsigned int i; + volatile unsigned int j; + + for (i = 0; i < milliseconds; i++) + { + for (j = 0; j < CONFIG_BOARD_LOOPSPERMSEC; j++) + { + } + } + } } */ #if defined(BUILD_MODULE) @@ -107,31 +180,28 @@ int uros_rbs_main(int argc, char* argv[]) { rcl_allocator_t allocator = rcl_get_default_allocator(); rclc_support_t support; + int ret; printf("Welcome to RBS Demo!\n"); + /* + // busy thread struct sched_param param, param2; pthread_attr_t attr, attr2; pthread_t worker_thread, worker_thread2; - int ret; - // Set the executor thread priority - param.sched_priority = SCHED_PRIORITY_DEFAULT; - ret = sched_setparam(0, ¶m); - if (ret < 0) - { - printf("uros_rbs: sched_setparam failed: %d\n" , ret); - return 1; - } + (void)pthread_attr_init(&attr); (void)pthread_attr_setschedparam(&attr, ¶m); - // (void)pthread_attr_setstacksize(&attr, CONFIG_UROS_RBS_EXAMPLE_STACKSIZE); + param.sched_priority = 10; int value = param.sched_priority; - ret = pthread_create(&worker_thread, &attr, &thread_run, &value); + ret = pthread_create(&worker_thread, &attr, &thread_run2, &value); if (ret != 0) - { - printf("uros_rbs: pthread_create failed: %d\n", ret); - return 1; - } + { + printf("uros_rbs: pthread_create failed: %d\n", ret); + return 1; + } +*/ + /* // second worker thread param2.sched_priority = SCHED_PRIORITY_DEFAULT + 10; (void)pthread_attr_init(&attr2); @@ -175,19 +245,18 @@ int uros_rbs_main(int argc, char* argv[]) } */ - struct sched_param exe_param; - int ret; + // Set the executor thread priority - /* - exe_param.sched_priority = 10; + struct sched_param exe_param; + exe_param.sched_priority = 100; ret = sched_setparam(0, &exe_param); if (ret < 0) { printf("uros_rbs: sched_setparam failed: %d\n" , ret); return 1; } - */ + RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); @@ -200,50 +269,44 @@ int uros_rbs_main(int argc, char* argv[]) RCCHECK(rclc_publisher_init_best_effort(&low_pong_publisher_, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "low_pong")); RCCHECK(rclc_subscription_init_best_effort(&low_ping_subscription_, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "low_ping")); - rclc_executor_t high_executor = rclc_executor_get_zero_initialized_executor(); - RCCHECK(rclc_executor_init(&high_executor, &support.context, 2, &allocator)); + rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); + RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator)); struct sched_param sparam_high, sparam_low; //sparam_high.priority = sched_get_priority_max(SCHED_FIFO); //sparam_low.priority = sched_get_priority_min(SCHED_FIFO); printf("max prio %d min prio %d\n", SCHED_PRIORITY_MAX, SCHED_PRIORITY_MIN); - bool sporadic_scheduling = true; - - sparam_high.sched_priority = 30; - - if(sporadic_scheduling){ - sparam_high.sched_priority = 30; - sparam_high.sched_ss_low_priority = 3; - sparam_high.sched_ss_repl_period.tv_sec = 5; - sparam_high.sched_ss_repl_period.tv_nsec = 0; - sparam_high.sched_ss_init_budget.tv_sec = 2; - sparam_high.sched_ss_init_budget.tv_nsec = 0; - sparam_high.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; - } - - sparam_low.sched_priority = 20; - if(sporadic_scheduling){ - sparam_low.sched_priority = 20; - sparam_low.sched_ss_low_priority = 2; - sparam_low.sched_ss_repl_period.tv_sec = 5; - sparam_low.sched_ss_repl_period.tv_nsec = 0; - sparam_low.sched_ss_init_budget.tv_sec = 1; - sparam_low.sched_ss_init_budget.tv_nsec = 0; - sparam_low.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; - } - + sparam_high.sched_priority = 60; + sparam_high.sched_ss_low_priority = 3; + sparam_high.sched_ss_repl_period.tv_sec = 0; + sparam_high.sched_ss_repl_period.tv_nsec = 10000000; // 10ms + sparam_high.sched_ss_init_budget.tv_sec = 1; + sparam_high.sched_ss_init_budget.tv_nsec = 6000000; // 6ms + sparam_high.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; + + sparam_low.sched_priority = 50; + sparam_low.sched_ss_low_priority = 2; + sparam_low.sched_ss_repl_period.tv_sec = 0; + sparam_low.sched_ss_repl_period.tv_nsec = 10000000; // 10ms + sparam_low.sched_ss_init_budget.tv_sec = 1; + sparam_low.sched_ss_init_budget.tv_nsec = 4000000; // 4ms + sparam_low.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; printf("high prio %d low prio %d\n", sparam_high.sched_priority, sparam_low.sched_priority); printf("subscription high ping \n"); - RCCHECK(rclc_executor_add_subscription_sched(&high_executor, &high_ping_subscription_, &high_ping_msg_, &high_ping_received, ON_NEW_DATA, &sparam_high)); - printf("subscription low ping \n"); - RCCHECK(rclc_executor_add_subscription_sched(&high_executor, &low_ping_subscription_, &low_ping_msg_, &low_ping_received, ON_NEW_DATA, &sparam_low)); - printf("start executor \n"); - RCCHECK(rclc_executor_start_multi_threading_for_nuttx(&high_executor)); + RCCHECK(rclc_executor_add_subscription_sched(&executor, &high_ping_subscription_, &high_ping_msg_, &high_ping_received, ON_NEW_DATA, &sparam_high)); + + printf("subscription low ping \n"); + RCCHECK(rclc_executor_add_subscription_sched(&executor, &low_ping_subscription_, &low_ping_msg_, &low_ping_received, ON_NEW_DATA, &sparam_low)); + + // int sched_policy = SCHED_SPORADIC; + int sched_policy = SCHED_FIFO; + + RCCHECK(rclc_executor_start_multi_threading_for_nuttx(&executor, sched_policy)); printf("clean up \n"); - RCCHECK(rclc_executor_fini(&high_executor)); + RCCHECK(rclc_executor_fini(&executor)); RCCHECK(rcl_subscription_fini(&high_ping_subscription_, &node)); RCCHECK(rcl_publisher_fini(&high_pong_publisher_, &node)); RCCHECK(rcl_subscription_fini(&low_ping_subscription_, &node)); diff --git a/examples/uros_rbs/uros_rbs-colcon.meta b/examples/uros_rbs/uros_rbs-colcon.meta new file mode 100644 index 00000000..fa543fd2 --- /dev/null +++ b/examples/uros_rbs/uros_rbs-colcon.meta @@ -0,0 +1,16 @@ +{ + "names": { + "rmw_microxrcedds": { + "cmake-args": [ + "-DRMW_UXRCE_TRANSPORT=custom_serial", + "-DRMW_UXRCE_MAX_NODES=1", + "-DRMW_UXRCE_MAX_PUBLISHERS=2", + "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=2", + "-DRMW_UXRCE_MAX_SERVICES=0", + "-DRMW_UXRCE_MAX_CLIENTS=0", + "-DRMW_UXRCE_MAX_HISTORY=2", + "-DRMW_UXRCE_XML_BUFFER_LENGTH=400" + ] + } + } +} From 283f8e16f76e3eff8740904bf908913e53545d2f Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Mon, 8 Feb 2021 13:36:48 +0100 Subject: [PATCH 13/18] test program for sporadic scheduling --- examples/uros_rbs/main_rbs.c | 187 ++++++++++++++++++++++++----------- 1 file changed, 129 insertions(+), 58 deletions(-) diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index 3f182988..fa33c4e6 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -30,9 +31,10 @@ #define STRING_BUFFER_LEN 100 #define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){\ - printf("Failed status on line %d: %d. Message: %s, Aborting.\n",__LINE__,(int)temp_rc, rcl_get_error_string().str);\ - rcutils_reset_error(); return 1;}} -#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}} + printf("Failed status on line %d: %d: %s. Aborting.\n",__LINE__,(int)temp_rc, rcl_get_error_string().str);\ + rcutils_reset_error(); return 1;}} +#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d:\ + %s. Continuing.\n",__LINE__,(int)temp_rc, rcl_get_error_string().str);rcutils_reset_error();}} #define RCUNUSED(fn) { rcl_ret_t temp_rc __attribute__((unused)); temp_rc = fn; } @@ -43,7 +45,7 @@ rcl_subscription_t low_ping_subscription_; std_msgs__msg__Int32 low_ping_msg_; rcl_publisher_t low_pong_publisher_; - +/* void burn_cpu_cycles_high(long duration) { if (duration > 0) { @@ -112,7 +114,7 @@ void low_ping_received(const void * pong_msg) printf("low ping received.\n"); RCUNUSED(rcl_publish(&low_pong_publisher_, pong_msg, NULL)); } - +*/ /* void * thread_run(void * arg) { @@ -143,109 +145,178 @@ void * thread_run(void * arg) } } */ +unsigned int thread_1_cnt =0, thread_2_cnt = 0; -/* -void * thread_run2(void * arg) +static +void * worker_thread_run(void * arg) { - // int * value = (int *) arg; int ret; + int * thread_id = (int *) arg; struct sched_param p; - ret = sched_setparam(0, &p); - if (ret < 0) + +#ifdef CONFIG_SCHED_SPORADIC + printf ("CONFIG_SCHED_SPORADIC defined.\n"); +#endif + + ret = sched_getparam(pthread_self(), &p); + if (ret != 0) { - printf("uros_rbs: sched_getparam failed: %d\n" , ret); - return 1; + printf("worker thread %d: sched_getparam failed: %d\n" , (*thread_id), ret); + return; } - printf("busy-loop prio: %d\n",p.sched_priority); - unsigned int milliseconds = 100; + printf("running worker-thread %d: prio %d budget %d ns period %d ns\n",(*thread_id), p.sched_priority, + p.sched_ss_init_budget.tv_nsec,p.sched_ss_repl_period.tv_nsec); + + printf("running worker-thread %d: prio %d budget %lf ms period %lf ms\n",(*thread_id), p.sched_priority, + (double) p.sched_ss_init_budget.tv_nsec/(double)1000000,(double)p.sched_ss_repl_period.tv_nsec/(double)1000000); + + unsigned int milliseconds = 1000; + sleep(1); while(1) { volatile unsigned int i; volatile unsigned int j; - + printf("thread %d\n", (*thread_id)); for (i = 0; i < milliseconds; i++) { + for (j = 0; j < CONFIG_BOARD_LOOPSPERMSEC; j++) { } + if ((*thread_id)== 1) + { + thread_1_cnt++; // count progress in milliseconds + } + if ((*thread_id)== 2) + { + thread_2_cnt++; // count progress in milliseconds + } } } } -*/ -#if defined(BUILD_MODULE) -int main(int argc, char *argv[]) -#else -int uros_rbs_main(int argc, char* argv[]) -#endif + +void test_sporadic_scheduling(int thr1_prio_high, int thr1_prio_low, int thr1_budget_ns, int thr1_period_ns, + int thr2_prio_high, int thr2_prio_low, int thr2_budget_ns, int thr2_period_ns +) { - rcl_allocator_t allocator = rcl_get_default_allocator(); - rclc_support_t support; + struct sched_param sparam_high, sparam_low; int ret; - printf("Welcome to RBS Demo!\n"); -/* - // busy thread - struct sched_param param, param2; - pthread_attr_t attr, attr2; - pthread_t worker_thread, worker_thread2; + sparam_high.sched_priority = thr1_prio_high; + sparam_high.sched_ss_low_priority = thr1_prio_low; + sparam_high.sched_ss_repl_period.tv_sec = 0; + sparam_high.sched_ss_repl_period.tv_nsec = thr1_period_ns; + sparam_high.sched_ss_init_budget.tv_sec = 0; + sparam_high.sched_ss_init_budget.tv_nsec = thr1_budget_ns; + sparam_high.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; + + sparam_low.sched_priority = thr2_prio_high; + sparam_low.sched_ss_low_priority = thr2_prio_low; + sparam_low.sched_ss_repl_period.tv_sec = 0; + sparam_low.sched_ss_repl_period.tv_nsec = thr2_period_ns; + sparam_low.sched_ss_init_budget.tv_sec = 0; + sparam_low.sched_ss_init_budget.tv_nsec = thr2_budget_ns; + sparam_low.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; + printf("thread 1: high prio %d low prio %d budget %d ms period %d ms \n", + sparam_high.sched_priority, sparam_high.sched_ss_low_priority, + sparam_high.sched_ss_init_budget.tv_nsec/1000000, sparam_high.sched_ss_repl_period.tv_nsec / 1000000); + printf("thread 2: high prio %d low prio %d budget %d ms period %d ms \n", + sparam_low.sched_priority, sparam_low.sched_ss_low_priority, + sparam_low.sched_ss_init_budget.tv_nsec/1000000, sparam_low.sched_ss_repl_period.tv_nsec / 1000000); + + // first worker thread + pthread_attr_t attr_high, attr_low; + pthread_t worker_thread_high, worker_thread_low; + int id_high=1, id_low=2; + + (void)pthread_attr_init(&attr_high); + (void)pthread_attr_setschedparam(&attr_high, &sparam_high); + ret = pthread_attr_setschedpolicy(&attr_high, SCHED_SPORADIC); + if (ret != OK) + { + printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", + ret); + } + printf("param low_prio %d budget %d period %d\n", + attr_high.low_priority, + attr_high.budget.tv_nsec, + attr_high.repl_period.tv_nsec); - (void)pthread_attr_init(&attr); - (void)pthread_attr_setschedparam(&attr, ¶m); - param.sched_priority = 10; - int value = param.sched_priority; - ret = pthread_create(&worker_thread, &attr, &thread_run2, &value); + ret = pthread_create(&worker_thread_high, &attr_high, &worker_thread_run, &id_high); if (ret != 0) { - printf("uros_rbs: pthread_create failed: %d\n", ret); + printf("uros_rbs: pthread_create worker_thread_high failed: %d\n", ret); return 1; - } -*/ - /* + } else { + printf("uros_rbs: started worker_thread_high\n"); + } + // second worker thread - param2.sched_priority = SCHED_PRIORITY_DEFAULT + 10; - (void)pthread_attr_init(&attr2); - (void)pthread_attr_setschedparam(&attr2, ¶m2); - // (void)pthread_attr_setstacksize(&attr, CONFIG_UROS_RBS_EXAMPLE_STACKSIZE); - int value2 = param2.sched_priority; - ret = pthread_create(&worker_thread2, &attr, thread_run2, &value2); + (void)pthread_attr_init(&attr_low); + (void)pthread_attr_setschedparam(&attr_low, &sparam_low); + ret = pthread_attr_setschedpolicy(&attr_low, SCHED_SPORADIC); + if (ret != OK) + { + printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", + ret); + } + ret = pthread_create(&worker_thread_low, &attr_low, &worker_thread_run, &id_low); if (ret != 0) { - printf("uros_rbs: pthread_create worker_thread2 failed: %d\n", ret); + printf("uros_rbs: pthread_create worker_thread_low failed: %d\n", ret); return 1; + } else { + printf("uros_rbs: started worker_thread_low\n"); } - - int i=0; - while(i<3){ + for(unsigned int i=0;i<10;i++) + { printf("sleeping\n"); sleep(1); - i++; } - - ret = pthread_join(worker_thread, NULL); + ret = pthread_cancel(worker_thread_high); if (ret != 0) { - fprintf(stderr, "uros_rbs: pthread_join failed\n"); + fprintf(stderr, "uros_rbs: pthread_cancel worker_thread_high failed\n"); } else { - printf("uros_rbs: worker_thread returned.\n"); + printf("uros_rbs: worker_thread_high returned.\n"); } - ret = pthread_join(worker_thread2, NULL); + ret = pthread_cancel(worker_thread_low); if (ret != 0) { - fprintf(stderr, "uros_rbs: pthread_join worker_thread2 failed\n"); + fprintf(stderr, "uros_rbs: pthread_cancel worker_thread_low failed\n"); } else { - printf("uros_rbs: worker_thread2 returned.\n"); + printf("uros_rbs: worker_thread_low returned.\n"); } -*/ + + printf("progress high %d ms progress low %d ms\n", thread_1_cnt, thread_2_cnt); +} +#if defined(BUILD_MODULE) +int main(int argc, char *argv[]) +#else +int uros_rbs_main(int argc, char* argv[]) +#endif +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + rclc_support_t support; + int ret; + printf("Welcome to RBS Demo!\n"); + + test_sporadic_scheduling(50, 5, 7000000, 10000000, + 40, 4, 3000000, 10000000); + + + +/* // Set the executor thread priority struct sched_param exe_param; @@ -313,7 +384,7 @@ int uros_rbs_main(int argc, char* argv[]) RCCHECK(rcl_publisher_fini(&low_pong_publisher_, &node)); RCCHECK(rcl_node_fini(&node)); - + */ return 0; } /* From 6f692d9bfcb0e3d1971e2b2a6448d1084d5a81ea Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Tue, 9 Feb 2021 08:47:04 +0100 Subject: [PATCH 14/18] sporadic scheduling example from sporadic.c --- examples/uros_rbs/main_rbs.c | 546 +++++++++++++++++++++++++++++++++-- 1 file changed, 525 insertions(+), 21 deletions(-) diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index fa33c4e6..42e44da0 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -12,22 +12,481 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include -#include #include #include +#include #include +#include +/* #include #include #include #include #include +*/ + +/* copied from sporadic.c*/ +/**************************************************************************** + * Included Files + ****************************************************************************/ +/* +#include + +#include +#include +#include +#include +#include +#include + +#include "ostest.h" +*/ +#ifdef CONFIG_SCHED_SPORADIC + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* It is actually a better test without schedule locking because that + * forces the scheduler into an uninteresting fallback mode. + */ + +#undef sched_lock +#undef sched_unlock +#define sched_lock() +#define sched_unlock() + +#ifndef MIN +# define MIN(a,b) (((a) < (b)) ? (a) : (b)) +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static sem_t g_sporadic_sem; +static time_t g_start_time; + +static unsigned int sporadic_1_ms_cnt = 0; +static unsigned int sporadic_2_ms_cnt = 0; +static unsigned int fifo_ms_cnt = 0; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static void my_mdelay(unsigned int milliseconds) +{ + volatile unsigned int i; + volatile unsigned int j; + + for (i = 0; i < milliseconds; i++) + { + for (j = 0; j < CONFIG_BOARD_LOOPSPERMSEC; j++) + { + } + } +} + +static void *nuisance_func(void *parameter) +{ + /* Synchronized start */ + + while (sem_wait(&g_sporadic_sem) < 0); + + /* Sleep until we are cancelled */ + + for (;;) + { + /* Sleep gracefully for awhile */ + + usleep(500*1000); + + /* Then hog some CPU time */ + + my_mdelay(100); + } + + return NULL; +} + +static void *fifo_func(void *parameter) +{ + struct sched_param param; + time_t last; + time_t now; + int ret; + + while (sem_wait(&g_sporadic_sem) < 0); + + last = g_start_time; + + for (;;) + { + do + { + my_mdelay(1); + fifo_ms_cnt++; + sched_lock(); /* Just to exercise more logic */ + ret = sched_getparam(0, ¶m); + if (ret < 0) + { + printf("ERROR: sched_getparam failed\n"); + return NULL; + } + + now = time(NULL); + sched_unlock(); + } + while (now == last); + + sched_lock(); /* Just to exercise more logic */ + printf("%4lu FIFO: %d\n", + (unsigned long)(now-g_start_time), param.sched_priority); + last = now; + sched_unlock(); + } +} + +static void *sporadic_func(void *parameter) +{ + struct sched_param param; + time_t last; + time_t now; + int prio = 0; + int ret; + unsigned int * counter = (unsigned int *) parameter; + while (sem_wait(&g_sporadic_sem) < 0); + + last = g_start_time; + + for (;;) + { + do + { + my_mdelay(1); + (sporadic_1_ms_cnt)++; + sched_lock(); /* Just to exercise more logic */ + ret = sched_getparam(0, ¶m); + if (ret < 0) + { + printf("ERROR: sched_getparam failed\n"); + return NULL; + } + + now = time(NULL); + sched_unlock(); + } + while (now == last && prio == param.sched_priority); + + sched_lock(); /* Just to exercise more logic */ + printf("%4lu SPORADIC 1: %d->%d\n", + (unsigned long)(now-g_start_time), prio, param.sched_priority); + prio = param.sched_priority; + last = now; + sched_unlock(); + } +} + +static void *sporadic_func2(void *parameter) +{ + struct sched_param param; + time_t last; + time_t now; + int prio = 0; + int ret; + unsigned int * counter = (unsigned int *) parameter; + while (sem_wait(&g_sporadic_sem) < 0); + + last = g_start_time; + + for (;;) + { + do + { + //my_mdelay(1); + //(sporadic_2_ms_cnt)++; + sched_lock(); /* Just to exercise more logic */ + ret = sched_getparam(0, ¶m); + if (ret < 0) + { + printf("ERROR: sched_getparam failed\n"); + return NULL; + } + + now = time(NULL); + sched_unlock(); + } + while (now == last && prio == param.sched_priority); + + sched_lock(); /* Just to exercise more logic */ + printf("%4lu SPORADIC 2: %d->%d\n", + (unsigned long)(now-g_start_time), prio, param.sched_priority); + prio = param.sched_priority; + last = now; + sched_unlock(); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void sporadic_test(void) +{ + pthread_t nuisance_thread = (pthread_t)0; + pthread_t sporadic_thread = (pthread_t)0; + pthread_t sporadic_thread2 = (pthread_t)0; + pthread_t fifo_thread = (pthread_t)0; +#ifdef SDCC + pthread_addr_t result; +#endif + FAR void *result; + struct sched_param myparam; + struct sched_param sparam; + int prio_min; + int prio_max; + int prio_low; + int prio_mid; + int prio_high; + pthread_attr_t attr; + int ret; + +#if CONFIG_SCHED_SPORADIC_MAXREPL < 5 + printf("sporadic_test: CONFIG_SCHED_SPORADIC_MAXREPL is small: %d\n", + CONFIG_SCHED_SPORADIC_MAXREPL); + printf(" -- There will some errors in the replenishment interval\n"); +#endif + + printf("sporadic_test: Initializing semaphore to 0\n"); + sem_init(&g_sporadic_sem, 0, 0); + + sporadic_1_ms_cnt = 0; + sporadic_2_ms_cnt = 0; + fifo_ms_cnt = 0; + + prio_min = sched_get_priority_min(SCHED_FIFO); + prio_max = sched_get_priority_max(SCHED_FIFO); + + /* + prio_low = prio_min + ((prio_max - prio_min) >> 2); + prio_mid = (prio_min + prio_max) >> 1; + prio_high = prio_max - ((prio_max - prio_min) >> 2); + */ + prio_low = 20; + prio_mid = 120; + prio_high = 180; + /* Temporarily set our priority to prio_high + 2 */ + + ret = sched_getparam(0, &myparam); + if (ret != OK) + { + printf("sporadic_test: ERROR: sched_getparam failed, ret=%d\n", ret); + } + + sparam.sched_priority = prio_high + 2; + ret = sched_setparam(0, &sparam); + if (ret != OK) + { + printf("sporadic_test: ERROR: sched_setparam failed, ret=%d\n", ret); + } + + ret = pthread_attr_init(&attr); + if (ret != OK) + { + printf("sporadic_test: ERROR: pthread_attr_init failed, ret=%d\n", + ret); + } + + /* This semaphore will prevent anything from running until we are ready */ + + sched_lock(); + sem_init(&g_sporadic_sem, 0, 0); + + /* Start a FIFO thread at the highest priority (prio_max + 1) */ + + printf("sporadic_test: Starting FIFO thread at priority %d\n", prio_mid); + + ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO); + if (ret != OK) + { + printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", + ret); + } + + sparam.sched_priority = prio_high + 1; + ret = pthread_attr_setschedparam(&attr, &sparam); + if (ret != OK) + { + printf("sporadic_test: ERROR: pthread_attr_setschedparam failed, ret=%d\n", + ret); + } +/* + ret = pthread_create(&nuisance_thread, &attr, nuisance_func, NULL); + if (ret != 0) + { + printf("sporadic_test: ERROR: FIFO thread creation failed: %d\n", + ret); + } +*/ + /* Start a FIFO thread at the middle priority */ +/* + sparam.sched_priority = prio_mid; + ret = pthread_attr_setschedparam(&attr, &sparam); + if (ret != OK) + { + printf("sporadic_test: ERROR: pthread_attr_setschedparam failed, ret=%d\n", + ret); + } + + ret = pthread_create(&fifo_thread, &attr, fifo_func, NULL); + if (ret != 0) + { + printf("sporadic_test: ERROR: FIFO thread creation failed: %d\n", + ret); + } +*/ + /* Start a sporadic thread, with the following parameters: */ + + printf("sporadic_test: Starting sporadic thread at priority %d\n", + prio_high, prio_low); + + ret = pthread_attr_setschedpolicy(&attr, SCHED_SPORADIC); + if (ret != OK) + { + printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", + ret); + } + + sparam.sched_priority = prio_high; + sparam.sched_ss_low_priority = prio_low; + sparam.sched_ss_repl_period.tv_sec = 0; + sparam.sched_ss_repl_period.tv_nsec = 10000000; + sparam.sched_ss_init_budget.tv_sec = 0; + sparam.sched_ss_init_budget.tv_nsec = 2000000; + sparam.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; + + ret = pthread_attr_setschedparam(&attr, &sparam); + if (ret != OK) + { + printf("sporadic_test: ERROR: pthread_attr_setsched param failed, ret=%d\n", + ret); + } + + ret = pthread_create(&sporadic_thread, &attr, sporadic_func, &sporadic_1_ms_cnt); + if (ret != 0) + { + printf("sporadic_test: ERROR: sporadic thread creation failed: %d\n", + ret); + } + + printf("sporadic_test: Starting sporadic thread 2 at priority %d\n", + prio_high-1, prio_low-1); + + ret = pthread_attr_setschedpolicy(&attr, SCHED_SPORADIC); + if (ret != OK) + { + printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", + ret); + } + struct sched_param sparam2; + sparam2.sched_priority = prio_high-1; + sparam2.sched_ss_low_priority = prio_low-1; + sparam2.sched_ss_repl_period.tv_sec = 0; + sparam2.sched_ss_repl_period.tv_nsec = 10000000; + sparam2.sched_ss_init_budget.tv_sec = 0; + sparam2.sched_ss_init_budget.tv_nsec = 1000000; + sparam2.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; + + ret = pthread_attr_setschedparam(&attr, &sparam2); + if (ret != OK) + { + printf("sporadic_test: ERROR: pthread_attr_setsched param failed, ret=%d\n", + ret); + } + + ret = pthread_create(&sporadic_thread2, &attr, sporadic_func2, (pthread_addr_t)&sporadic_2_ms_cnt); + if (ret != 0) + { + printf("sporadic_test: ERROR: sporadic thread creation failed: %d\n", + ret); + } + + printf("sporadic_test: Starting FIFO thread at priority %d\n", prio_mid); + ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO); + if (ret != OK) + { + printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", + ret); + } + struct sched_param sparam3; + sparam3.sched_priority = prio_mid; + ret = pthread_attr_setschedparam(&attr, &sparam3); + if (ret != OK) + { + printf("sporadic_test: ERROR: pthread_attr_setschedparam failed, ret=%d\n", + ret); + } + + ret = pthread_create(&fifo_thread, &attr, fifo_func, NULL); + if (ret != 0) + { + printf("sporadic_test: ERROR: FIFO thread creation failed: %d\n", + ret); + } + g_start_time = time(NULL); + sem_post(&g_sporadic_sem); + sem_post(&g_sporadic_sem); + sem_post(&g_sporadic_sem); +/* + // Wait a while then kill the FIFO thread + + sleep(15); + ret = pthread_cancel(fifo_thread); + pthread_join(fifo_thread, &result); + + /// Wait a bit longer then kill the nuisance thread + + sleep(10); + ret = pthread_cancel(nuisance_thread); + pthread_join(nuisance_thread, &result); + + // Wait a bit longer then kill the sporadic thread +*/ + sleep(10); + + ret = pthread_cancel(sporadic_thread); + //pthread_join(sporadic_thread, &result); + + ret = pthread_cancel(sporadic_thread2); + //pthread_join(sporadic_thread, &result); + + ret = pthread_cancel(fifo_thread); + //pthread_join(fifo_thread, &result); + sched_unlock(); + + printf("sporadic_test: sporadic 1 %d ms sporadic 2 %d FIFO %d ms\n",sporadic_1_ms_cnt, sporadic_2_ms_cnt, fifo_ms_cnt); + printf("sporadic_test: Done\n"); + sem_destroy(&g_sporadic_sem); + + ret = sched_setparam(0, &myparam); + if (ret != OK) + { + printf("sporadic_test: ERROR: sched_setparam failed, ret=%d\n", ret); + } +} + +#endif /* CONFIG_SCHED_SPORADIC */ + +/* #define STRING_BUFFER_LEN 100 #define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){\ @@ -44,7 +503,7 @@ rcl_publisher_t high_pong_publisher_; rcl_subscription_t low_ping_subscription_; std_msgs__msg__Int32 low_ping_msg_; rcl_publisher_t low_pong_publisher_; - +*/ /* void burn_cpu_cycles_high(long duration) { @@ -145,6 +604,9 @@ void * thread_run(void * arg) } } */ + +/* +static unsigned int thread_1_cnt =0, thread_2_cnt = 0; static @@ -152,26 +614,37 @@ void * worker_thread_run(void * arg) { int ret; int * thread_id = (int *) arg; + int sched_policy = 0; struct sched_param p; #ifdef CONFIG_SCHED_SPORADIC printf ("CONFIG_SCHED_SPORADIC defined.\n"); #endif - ret = sched_getparam(pthread_self(), &p); + ret = nxsched_getparam(pthread_self(), &p); + if (ret != 0) + { + printf("worker thread %d: nxsched_getparam failed: %d\n" , (*thread_id), ret); + return; + } + printf("running worker-thread %d -1-: prio %d budget %d ns period %d ns\n",(*thread_id), p.sched_priority, + p.sched_ss_init_budget.tv_nsec,p.sched_ss_repl_period.tv_nsec); + + + ret = pthread_getschedparam(pthread_self(), &sched_policy, &p); if (ret != 0) { printf("worker thread %d: sched_getparam failed: %d\n" , (*thread_id), ret); return; } - printf("running worker-thread %d: prio %d budget %d ns period %d ns\n",(*thread_id), p.sched_priority, + printf("running worker-thread %d:-2- policy %d prio %d budget %d ns period %d ns\n",(*thread_id), sched_policy, p.sched_priority, p.sched_ss_init_budget.tv_nsec,p.sched_ss_repl_period.tv_nsec); - printf("running worker-thread %d: prio %d budget %lf ms period %lf ms\n",(*thread_id), p.sched_priority, - (double) p.sched_ss_init_budget.tv_nsec/(double)1000000,(double)p.sched_ss_repl_period.tv_nsec/(double)1000000); + // printf("running worker-thread %d: prio %d budget %lf ms period %lf ms\n",(*thread_id), p.sched_priority, + // (double) p.sched_ss_init_budget.tv_nsec/(double)1000000,(double)p.sched_ss_repl_period.tv_nsec/(double)1000000); unsigned int milliseconds = 1000; - sleep(1); + // sleep(1); while(1) { volatile unsigned int i; @@ -202,6 +675,9 @@ void test_sporadic_scheduling(int thr1_prio_high, int thr1_prio_low, int thr1_bu struct sched_param sparam_high, sparam_low; int ret; + thread_1_cnt = 0; + thread_2_cnt = 0; + sparam_high.sched_priority = thr1_prio_high; sparam_high.sched_ss_low_priority = thr1_prio_low; sparam_high.sched_ss_repl_period.tv_sec = 0; @@ -231,14 +707,20 @@ void test_sporadic_scheduling(int thr1_prio_high, int thr1_prio_low, int thr1_bu int id_high=1, id_low=2; (void)pthread_attr_init(&attr_high); - (void)pthread_attr_setschedparam(&attr_high, &sparam_high); + #ifdef _POSIX_SPORADIC_SERVER + printf("var _POSIX_SPORADIC_SERVER defined\n"); + #endif + printf("SCHED_SPORADIC = %d\n",SCHED_SPORADIC); ret = pthread_attr_setschedpolicy(&attr_high, SCHED_SPORADIC); if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", - ret); - } - printf("param low_prio %d budget %d period %d\n", + { + printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", + ret); + } + (void)pthread_attr_setschedparam(&attr_high, &sparam_high); + + printf("param policy %d low_prio %d budget %d period %d\n", + attr_high.policy, attr_high.low_priority, attr_high.budget.tv_nsec, attr_high.repl_period.tv_nsec); @@ -270,9 +752,31 @@ void test_sporadic_scheduling(int thr1_prio_high, int thr1_prio_low, int thr1_bu printf("uros_rbs: started worker_thread_low\n"); } + +// debugging parameters: + struct sched_param p; + + ret = nxsched_getparam(worker_thread_high, &p); + if (ret != 0) + { + printf("worker thread %d: nxsched_getparam failed: %d\n" , worker_thread_high, ret); + return; + } + printf("running worker-thread-high: pid %d prio %d budget %d ns period %d ns\n",worker_thread_high, p.sched_priority, + p.sched_ss_init_budget.tv_nsec, p.sched_ss_repl_period.tv_nsec); + + ret = nxsched_getparam(worker_thread_low, &p); + if (ret != 0) + { + printf("worker thread %d: nxsched_getparam failed: %d\n" , worker_thread_low, ret); + return; + } + printf("running worker-thread-high: pid %d prio %d budget %d ns period %d ns\n",worker_thread_low, p.sched_priority, + p.sched_ss_init_budget.tv_nsec, p.sched_ss_repl_period.tv_nsec); + for(unsigned int i=0;i<10;i++) { - printf("sleeping\n"); + // printf("main sleeping\n"); sleep(1); } @@ -298,7 +802,7 @@ void test_sporadic_scheduling(int thr1_prio_high, int thr1_prio_low, int thr1_bu printf("progress high %d ms progress low %d ms\n", thread_1_cnt, thread_2_cnt); } - +*/ #if defined(BUILD_MODULE) int main(int argc, char *argv[]) @@ -306,14 +810,14 @@ int main(int argc, char *argv[]) int uros_rbs_main(int argc, char* argv[]) #endif { - rcl_allocator_t allocator = rcl_get_default_allocator(); - rclc_support_t support; + // rcl_allocator_t allocator = rcl_get_default_allocator(); + // rclc_support_t support; int ret; - printf("Welcome to RBS Demo!\n"); - test_sporadic_scheduling(50, 5, 7000000, 10000000, - 40, 4, 3000000, 10000000); - + sporadic_test(); + // test_sporadic_scheduling(40, 4, 4000000, 10000000, + // 50, 5, 2000000, 10000000); + /* From c0542b32fa8273462db48aff585adbc9549a6fc0 Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Tue, 9 Feb 2021 16:49:15 +0100 Subject: [PATCH 15/18] singele sporadic thread --- examples/uros_rbs/main_rbs.c | 101 ++++++++++++++++++++--------------- 1 file changed, 57 insertions(+), 44 deletions(-) diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index 42e44da0..a75ecb85 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -23,13 +23,13 @@ #include #include -/* + #include #include #include #include #include -*/ + /* copied from sporadic.c*/ /**************************************************************************** @@ -131,8 +131,10 @@ static void *fifo_func(void *parameter) { do { - my_mdelay(1); - fifo_ms_cnt++; + my_mdelay(100); + usleep(100000); + //fifo_ms_cnt += 4; + fifo_ms_cnt +=1; sched_lock(); /* Just to exercise more logic */ ret = sched_getparam(0, ¶m); if (ret < 0) @@ -147,8 +149,8 @@ static void *fifo_func(void *parameter) while (now == last); sched_lock(); /* Just to exercise more logic */ - printf("%4lu FIFO: %d\n", - (unsigned long)(now-g_start_time), param.sched_priority); + //printf("%4lu FIFO: %d\n", + // (unsigned long)(now-g_start_time), param.sched_priority); last = now; sched_unlock(); } @@ -186,8 +188,8 @@ static void *sporadic_func(void *parameter) while (now == last && prio == param.sched_priority); sched_lock(); /* Just to exercise more logic */ - printf("%4lu SPORADIC 1: %d->%d\n", - (unsigned long)(now-g_start_time), prio, param.sched_priority); + // printf("%4lu SPORADIC 1: %d->%d\n", + // (unsigned long)(now-g_start_time), prio, param.sched_priority); prio = param.sched_priority; last = now; sched_unlock(); @@ -210,8 +212,8 @@ static void *sporadic_func2(void *parameter) { do { - //my_mdelay(1); - //(sporadic_2_ms_cnt)++; + my_mdelay(1); + (sporadic_2_ms_cnt)++; sched_lock(); /* Just to exercise more logic */ ret = sched_getparam(0, ¶m); if (ret < 0) @@ -226,8 +228,8 @@ static void *sporadic_func2(void *parameter) while (now == last && prio == param.sched_priority); sched_lock(); /* Just to exercise more logic */ - printf("%4lu SPORADIC 2: %d->%d\n", - (unsigned long)(now-g_start_time), prio, param.sched_priority); + // printf("%4lu SPORADIC 2: %d->%d\n", + // (unsigned long)(now-g_start_time), prio, param.sched_priority); prio = param.sched_priority; last = now; sched_unlock(); @@ -255,7 +257,7 @@ void sporadic_test(void) int prio_low; int prio_mid; int prio_high; - pthread_attr_t attr; + pthread_attr_t attr, attr_spor, attr_spor2, attr_fifo; int ret; #if CONFIG_SCHED_SPORADIC_MAXREPL < 5 @@ -274,15 +276,15 @@ void sporadic_test(void) prio_min = sched_get_priority_min(SCHED_FIFO); prio_max = sched_get_priority_max(SCHED_FIFO); - /* - prio_low = prio_min + ((prio_max - prio_min) >> 2); - prio_mid = (prio_min + prio_max) >> 1; - prio_high = prio_max - ((prio_max - prio_min) >> 2); - */ + + // prio_low = prio_min + ((prio_max - prio_min) >> 2); + // prio_mid = (prio_min + prio_max) >> 1; + // prio_high = prio_max - ((prio_max - prio_min) >> 2); + prio_low = 20; prio_mid = 120; prio_high = 180; - /* Temporarily set our priority to prio_high + 2 */ + // Temporarily set our priority to prio_high + 2 ret = sched_getparam(0, &myparam); if (ret != OK) @@ -304,12 +306,12 @@ void sporadic_test(void) ret); } - /* This semaphore will prevent anything from running until we are ready */ + // This semaphore will prevent anything from running until we are ready sched_lock(); sem_init(&g_sporadic_sem, 0, 0); - /* Start a FIFO thread at the highest priority (prio_max + 1) */ + // Start a FIFO thread at the highest priority (prio_max + 1) printf("sporadic_test: Starting FIFO thread at priority %d\n", prio_mid); @@ -356,8 +358,13 @@ void sporadic_test(void) printf("sporadic_test: Starting sporadic thread at priority %d\n", prio_high, prio_low); - - ret = pthread_attr_setschedpolicy(&attr, SCHED_SPORADIC); + ret = pthread_attr_init(&attr_spor); + if (ret != OK) + { + printf("sporadic_test: ERROR: pthread_attr_init failed, ret=%d\n", + ret); + } + ret = pthread_attr_setschedpolicy(&attr_spor, SCHED_SPORADIC); if (ret != OK) { printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", @@ -367,60 +374,61 @@ void sporadic_test(void) sparam.sched_priority = prio_high; sparam.sched_ss_low_priority = prio_low; sparam.sched_ss_repl_period.tv_sec = 0; - sparam.sched_ss_repl_period.tv_nsec = 10000000; + sparam.sched_ss_repl_period.tv_nsec = 100000000; sparam.sched_ss_init_budget.tv_sec = 0; - sparam.sched_ss_init_budget.tv_nsec = 2000000; + sparam.sched_ss_init_budget.tv_nsec = 30000000; sparam.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; - ret = pthread_attr_setschedparam(&attr, &sparam); + ret = pthread_attr_setschedparam(&attr_spor, &sparam); if (ret != OK) { printf("sporadic_test: ERROR: pthread_attr_setsched param failed, ret=%d\n", ret); } - ret = pthread_create(&sporadic_thread, &attr, sporadic_func, &sporadic_1_ms_cnt); + ret = pthread_create(&sporadic_thread, &attr_spor, sporadic_func, &sporadic_1_ms_cnt); if (ret != 0) { printf("sporadic_test: ERROR: sporadic thread creation failed: %d\n", ret); } - +/* + // second sporadic thread printf("sporadic_test: Starting sporadic thread 2 at priority %d\n", - prio_high-1, prio_low-1); - - ret = pthread_attr_setschedpolicy(&attr, SCHED_SPORADIC); + prio_high, prio_low); + ret = pthread_attr_init(&attr_spor2); + ret = pthread_attr_setschedpolicy(&attr_spor2, SCHED_SPORADIC); if (ret != OK) { printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", ret); } struct sched_param sparam2; - sparam2.sched_priority = prio_high-1; - sparam2.sched_ss_low_priority = prio_low-1; + sparam2.sched_priority = prio_high; + sparam2.sched_ss_low_priority = prio_low; sparam2.sched_ss_repl_period.tv_sec = 0; - sparam2.sched_ss_repl_period.tv_nsec = 10000000; + sparam2.sched_ss_repl_period.tv_nsec = 100000000; sparam2.sched_ss_init_budget.tv_sec = 0; - sparam2.sched_ss_init_budget.tv_nsec = 1000000; + sparam2.sched_ss_init_budget.tv_nsec = 30000000; sparam2.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; - ret = pthread_attr_setschedparam(&attr, &sparam2); + ret = pthread_attr_setschedparam(&attr_spor2, &sparam2); if (ret != OK) { printf("sporadic_test: ERROR: pthread_attr_setsched param failed, ret=%d\n", ret); } - ret = pthread_create(&sporadic_thread2, &attr, sporadic_func2, (pthread_addr_t)&sporadic_2_ms_cnt); + ret = pthread_create(&sporadic_thread2, &attr_spor2, sporadic_func2, (pthread_addr_t)&sporadic_2_ms_cnt); if (ret != 0) { printf("sporadic_test: ERROR: sporadic thread creation failed: %d\n", ret); } - +*/ printf("sporadic_test: Starting FIFO thread at priority %d\n", prio_mid); - - ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO); + ret = pthread_attr_init(&attr_fifo); + ret = pthread_attr_setschedpolicy(&attr_fifo, SCHED_FIFO); if (ret != OK) { printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", @@ -428,14 +436,14 @@ void sporadic_test(void) } struct sched_param sparam3; sparam3.sched_priority = prio_mid; - ret = pthread_attr_setschedparam(&attr, &sparam3); + ret = pthread_attr_setschedparam(&attr_fifo, &sparam3); if (ret != OK) { printf("sporadic_test: ERROR: pthread_attr_setschedparam failed, ret=%d\n", ret); } - ret = pthread_create(&fifo_thread, &attr, fifo_func, NULL); + ret = pthread_create(&fifo_thread, &attr_fifo, fifo_func, NULL); if (ret != 0) { printf("sporadic_test: ERROR: FIFO thread creation failed: %d\n", @@ -466,14 +474,19 @@ void sporadic_test(void) ret = pthread_cancel(sporadic_thread); //pthread_join(sporadic_thread, &result); - ret = pthread_cancel(sporadic_thread2); + // ret = pthread_cancel(sporadic_thread2); //pthread_join(sporadic_thread, &result); ret = pthread_cancel(fifo_thread); //pthread_join(fifo_thread, &result); sched_unlock(); + unsigned int sum = sporadic_1_ms_cnt + fifo_ms_cnt; + printf("sporadic_test: sporadic 1 %d ms sporadic 2 %d FIFO %d ms \n \ + percentages: sporadic %.2f FIFO %.2f \n", + sporadic_1_ms_cnt, sporadic_2_ms_cnt, fifo_ms_cnt, + ((float)sporadic_1_ms_cnt/(float)sum)*100, + ((float)fifo_ms_cnt/(float)sum)*100); - printf("sporadic_test: sporadic 1 %d ms sporadic 2 %d FIFO %d ms\n",sporadic_1_ms_cnt, sporadic_2_ms_cnt, fifo_ms_cnt); printf("sporadic_test: Done\n"); sem_destroy(&g_sporadic_sem); From 41999c44248b27aa29ec3a934e3f6531b5b0b8ab Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Tue, 9 Feb 2021 22:47:09 +0100 Subject: [PATCH 16/18] working version RTAS paper, results ok --- examples/uros_rbs/main_rbs.c | 851 ++++------------------------------- 1 file changed, 98 insertions(+), 753 deletions(-) diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index a75ecb85..aa044915 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -30,56 +30,23 @@ #include #include +#define STRING_BUFFER_LEN 100 -/* copied from sporadic.c*/ -/**************************************************************************** - * Included Files - ****************************************************************************/ -/* -#include - -#include -#include -#include -#include -#include -#include - -#include "ostest.h" -*/ -#ifdef CONFIG_SCHED_SPORADIC - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/* It is actually a better test without schedule locking because that - * forces the scheduler into an uninteresting fallback mode. - */ - -#undef sched_lock -#undef sched_unlock -#define sched_lock() -#define sched_unlock() - -#ifndef MIN -# define MIN(a,b) (((a) < (b)) ? (a) : (b)) -#endif - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -static sem_t g_sporadic_sem; -static time_t g_start_time; +#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){\ + printf("Failed status on line %d: %d: %s. Aborting.\n",__LINE__,(int)temp_rc, rcl_get_error_string().str);\ + rcutils_reset_error(); return 1;}} +#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d:\ + %s. Continuing.\n",__LINE__,(int)temp_rc, rcl_get_error_string().str);rcutils_reset_error();}} +#define RCUNUSED(fn) { rcl_ret_t temp_rc __attribute__((unused)); temp_rc = fn; } -static unsigned int sporadic_1_ms_cnt = 0; -static unsigned int sporadic_2_ms_cnt = 0; -static unsigned int fifo_ms_cnt = 0; +static unsigned int sleep_ms; -/**************************************************************************** - * Private Functions - ****************************************************************************/ +rcl_subscription_t high_ping_subscription_; +std_msgs__msg__Int32 high_ping_msg_; +rcl_publisher_t high_pong_publisher_; +rcl_subscription_t low_ping_subscription_; +std_msgs__msg__Int32 low_ping_msg_; +rcl_publisher_t low_pong_publisher_; static void my_mdelay(unsigned int milliseconds) { @@ -94,429 +61,48 @@ static void my_mdelay(unsigned int milliseconds) } } -static void *nuisance_func(void *parameter) +static void my_mdelay2(unsigned int milliseconds) { - /* Synchronized start */ - - while (sem_wait(&g_sporadic_sem) < 0); - - /* Sleep until we are cancelled */ - - for (;;) - { - /* Sleep gracefully for awhile */ - - usleep(500*1000); - - /* Then hog some CPU time */ - - my_mdelay(100); - } - - return NULL; -} - -static void *fifo_func(void *parameter) -{ - struct sched_param param; - time_t last; - time_t now; - int ret; - - while (sem_wait(&g_sporadic_sem) < 0); - - last = g_start_time; - - for (;;) - { - do - { - my_mdelay(100); - usleep(100000); - //fifo_ms_cnt += 4; - fifo_ms_cnt +=1; - sched_lock(); /* Just to exercise more logic */ - ret = sched_getparam(0, ¶m); - if (ret < 0) - { - printf("ERROR: sched_getparam failed\n"); - return NULL; - } - - now = time(NULL); - sched_unlock(); - } - while (now == last); - - sched_lock(); /* Just to exercise more logic */ - //printf("%4lu FIFO: %d\n", - // (unsigned long)(now-g_start_time), param.sched_priority); - last = now; - sched_unlock(); - } -} - -static void *sporadic_func(void *parameter) -{ - struct sched_param param; - time_t last; - time_t now; - int prio = 0; - int ret; - unsigned int * counter = (unsigned int *) parameter; - while (sem_wait(&g_sporadic_sem) < 0); - - last = g_start_time; - - for (;;) - { - do - { - my_mdelay(1); - (sporadic_1_ms_cnt)++; - sched_lock(); /* Just to exercise more logic */ - ret = sched_getparam(0, ¶m); - if (ret < 0) - { - printf("ERROR: sched_getparam failed\n"); - return NULL; - } - - now = time(NULL); - sched_unlock(); - } - while (now == last && prio == param.sched_priority); - - sched_lock(); /* Just to exercise more logic */ - // printf("%4lu SPORADIC 1: %d->%d\n", - // (unsigned long)(now-g_start_time), prio, param.sched_priority); - prio = param.sched_priority; - last = now; - sched_unlock(); - } -} - -static void *sporadic_func2(void *parameter) -{ - struct sched_param param; - time_t last; - time_t now; - int prio = 0; - int ret; - unsigned int * counter = (unsigned int *) parameter; - while (sem_wait(&g_sporadic_sem) < 0); - - last = g_start_time; + volatile unsigned int i; + volatile unsigned int j; - for (;;) + for (i = 0; i < milliseconds; i++) { - do + for (j = 0; j < CONFIG_BOARD_LOOPSPERMSEC; j++) { - my_mdelay(1); - (sporadic_2_ms_cnt)++; - sched_lock(); /* Just to exercise more logic */ - ret = sched_getparam(0, ¶m); - if (ret < 0) - { - printf("ERROR: sched_getparam failed\n"); - return NULL; - } - - now = time(NULL); - sched_unlock(); } - while (now == last && prio == param.sched_priority); - - sched_lock(); /* Just to exercise more logic */ - // printf("%4lu SPORADIC 2: %d->%d\n", - // (unsigned long)(now-g_start_time), prio, param.sched_priority); - prio = param.sched_priority; - last = now; - sched_unlock(); } } -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -void sporadic_test(void) +void delay_test(void) { - pthread_t nuisance_thread = (pthread_t)0; - pthread_t sporadic_thread = (pthread_t)0; - pthread_t sporadic_thread2 = (pthread_t)0; - pthread_t fifo_thread = (pthread_t)0; -#ifdef SDCC - pthread_addr_t result; -#endif - FAR void *result; - struct sched_param myparam; - struct sched_param sparam; - int prio_min; - int prio_max; - int prio_low; - int prio_mid; - int prio_high; - pthread_attr_t attr, attr_spor, attr_spor2, attr_fifo; - int ret; - -#if CONFIG_SCHED_SPORADIC_MAXREPL < 5 - printf("sporadic_test: CONFIG_SCHED_SPORADIC_MAXREPL is small: %d\n", - CONFIG_SCHED_SPORADIC_MAXREPL); - printf(" -- There will some errors in the replenishment interval\n"); -#endif - - printf("sporadic_test: Initializing semaphore to 0\n"); - sem_init(&g_sporadic_sem, 0, 0); - - sporadic_1_ms_cnt = 0; - sporadic_2_ms_cnt = 0; - fifo_ms_cnt = 0; - - prio_min = sched_get_priority_min(SCHED_FIFO); - prio_max = sched_get_priority_max(SCHED_FIFO); - - - // prio_low = prio_min + ((prio_max - prio_min) >> 2); - // prio_mid = (prio_min + prio_max) >> 1; - // prio_high = prio_max - ((prio_max - prio_min) >> 2); - - prio_low = 20; - prio_mid = 120; - prio_high = 180; - // Temporarily set our priority to prio_high + 2 - - ret = sched_getparam(0, &myparam); - if (ret != OK) - { - printf("sporadic_test: ERROR: sched_getparam failed, ret=%d\n", ret); - } - - sparam.sched_priority = prio_high + 2; - ret = sched_setparam(0, &sparam); - if (ret != OK) - { - printf("sporadic_test: ERROR: sched_setparam failed, ret=%d\n", ret); - } - - ret = pthread_attr_init(&attr); - if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_init failed, ret=%d\n", - ret); - } - - // This semaphore will prevent anything from running until we are ready - - sched_lock(); - sem_init(&g_sporadic_sem, 0, 0); - - // Start a FIFO thread at the highest priority (prio_max + 1) - - printf("sporadic_test: Starting FIFO thread at priority %d\n", prio_mid); - - ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO); - if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", - ret); - } - - sparam.sched_priority = prio_high + 1; - ret = pthread_attr_setschedparam(&attr, &sparam); - if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_setschedparam failed, ret=%d\n", - ret); - } -/* - ret = pthread_create(&nuisance_thread, &attr, nuisance_func, NULL); - if (ret != 0) - { - printf("sporadic_test: ERROR: FIFO thread creation failed: %d\n", - ret); - } -*/ - /* Start a FIFO thread at the middle priority */ -/* - sparam.sched_priority = prio_mid; - ret = pthread_attr_setschedparam(&attr, &sparam); - if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_setschedparam failed, ret=%d\n", - ret); - } - - ret = pthread_create(&fifo_thread, &attr, fifo_func, NULL); - if (ret != 0) - { - printf("sporadic_test: ERROR: FIFO thread creation failed: %d\n", - ret); - } -*/ - /* Start a sporadic thread, with the following parameters: */ - - printf("sporadic_test: Starting sporadic thread at priority %d\n", - prio_high, prio_low); - ret = pthread_attr_init(&attr_spor); - if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_init failed, ret=%d\n", - ret); - } - ret = pthread_attr_setschedpolicy(&attr_spor, SCHED_SPORADIC); - if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", - ret); - } - - sparam.sched_priority = prio_high; - sparam.sched_ss_low_priority = prio_low; - sparam.sched_ss_repl_period.tv_sec = 0; - sparam.sched_ss_repl_period.tv_nsec = 100000000; - sparam.sched_ss_init_budget.tv_sec = 0; - sparam.sched_ss_init_budget.tv_nsec = 30000000; - sparam.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; - - ret = pthread_attr_setschedparam(&attr_spor, &sparam); - if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_setsched param failed, ret=%d\n", - ret); - } - - ret = pthread_create(&sporadic_thread, &attr_spor, sporadic_func, &sporadic_1_ms_cnt); - if (ret != 0) - { - printf("sporadic_test: ERROR: sporadic thread creation failed: %d\n", - ret); - } -/* - // second sporadic thread - printf("sporadic_test: Starting sporadic thread 2 at priority %d\n", - prio_high, prio_low); - ret = pthread_attr_init(&attr_spor2); - ret = pthread_attr_setschedpolicy(&attr_spor2, SCHED_SPORADIC); - if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", - ret); - } - struct sched_param sparam2; - sparam2.sched_priority = prio_high; - sparam2.sched_ss_low_priority = prio_low; - sparam2.sched_ss_repl_period.tv_sec = 0; - sparam2.sched_ss_repl_period.tv_nsec = 100000000; - sparam2.sched_ss_init_budget.tv_sec = 0; - sparam2.sched_ss_init_budget.tv_nsec = 30000000; - sparam2.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; - - ret = pthread_attr_setschedparam(&attr_spor2, &sparam2); - if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_setsched param failed, ret=%d\n", - ret); - } - - ret = pthread_create(&sporadic_thread2, &attr_spor2, sporadic_func2, (pthread_addr_t)&sporadic_2_ms_cnt); - if (ret != 0) - { - printf("sporadic_test: ERROR: sporadic thread creation failed: %d\n", - ret); - } -*/ - printf("sporadic_test: Starting FIFO thread at priority %d\n", prio_mid); - ret = pthread_attr_init(&attr_fifo); - ret = pthread_attr_setschedpolicy(&attr_fifo, SCHED_FIFO); - if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", - ret); - } - struct sched_param sparam3; - sparam3.sched_priority = prio_mid; - ret = pthread_attr_setschedparam(&attr_fifo, &sparam3); - if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_setschedparam failed, ret=%d\n", - ret); - } - - ret = pthread_create(&fifo_thread, &attr_fifo, fifo_func, NULL); - if (ret != 0) - { - printf("sporadic_test: ERROR: FIFO thread creation failed: %d\n", - ret); - } - g_start_time = time(NULL); - - sem_post(&g_sporadic_sem); - sem_post(&g_sporadic_sem); - sem_post(&g_sporadic_sem); -/* - // Wait a while then kill the FIFO thread - - sleep(15); - ret = pthread_cancel(fifo_thread); - pthread_join(fifo_thread, &result); - - /// Wait a bit longer then kill the nuisance thread - - sleep(10); - ret = pthread_cancel(nuisance_thread); - pthread_join(nuisance_thread, &result); - - // Wait a bit longer then kill the sporadic thread -*/ - sleep(10); - - ret = pthread_cancel(sporadic_thread); - //pthread_join(sporadic_thread, &result); - - // ret = pthread_cancel(sporadic_thread2); - //pthread_join(sporadic_thread, &result); - - ret = pthread_cancel(fifo_thread); - //pthread_join(fifo_thread, &result); - sched_unlock(); - unsigned int sum = sporadic_1_ms_cnt + fifo_ms_cnt; - printf("sporadic_test: sporadic 1 %d ms sporadic 2 %d FIFO %d ms \n \ - percentages: sporadic %.2f FIFO %.2f \n", - sporadic_1_ms_cnt, sporadic_2_ms_cnt, fifo_ms_cnt, - ((float)sporadic_1_ms_cnt/(float)sum)*100, - ((float)fifo_ms_cnt/(float)sum)*100); + struct timespec startTimeP; + struct timespec endTimeP; + unsigned int delay = 10; + long measuredDuration = 0; + long sumDuration = 0; + + unsigned int iter = 10; + for (int i=0;i0){ + my_mdelay2(1000); + seconds--; } - printf("running worker-thread %d -1-: prio %d budget %d ns period %d ns\n",(*thread_id), p.sched_priority, - p.sched_ss_init_budget.tv_nsec,p.sched_ss_repl_period.tv_nsec); - - - ret = pthread_getschedparam(pthread_self(), &sched_policy, &p); - if (ret != 0) - { - printf("worker thread %d: sched_getparam failed: %d\n" , (*thread_id), ret); - return; - } - printf("running worker-thread %d:-2- policy %d prio %d budget %d ns period %d ns\n",(*thread_id), sched_policy, p.sched_priority, - p.sched_ss_init_budget.tv_nsec,p.sched_ss_repl_period.tv_nsec); - - // printf("running worker-thread %d: prio %d budget %lf ms period %lf ms\n",(*thread_id), p.sched_priority, - // (double) p.sched_ss_init_budget.tv_nsec/(double)1000000,(double)p.sched_ss_repl_period.tv_nsec/(double)1000000); - - unsigned int milliseconds = 1000; - // sleep(1); - while(1) - { - volatile unsigned int i; - volatile unsigned int j; - printf("thread %d\n", (*thread_id)); - for (i = 0; i < milliseconds; i++) - { - - for (j = 0; j < CONFIG_BOARD_LOOPSPERMSEC; j++) - { - } - if ((*thread_id)== 1) - { - thread_1_cnt++; // count progress in milliseconds - } - if ((*thread_id)== 2) - { - thread_2_cnt++; // count progress in milliseconds - } - } - } -} - -void test_sporadic_scheduling(int thr1_prio_high, int thr1_prio_low, int thr1_budget_ns, int thr1_period_ns, - int thr2_prio_high, int thr2_prio_low, int thr2_budget_ns, int thr2_period_ns -) -{ - struct sched_param sparam_high, sparam_low; - int ret; - - thread_1_cnt = 0; - thread_2_cnt = 0; - - sparam_high.sched_priority = thr1_prio_high; - sparam_high.sched_ss_low_priority = thr1_prio_low; - sparam_high.sched_ss_repl_period.tv_sec = 0; - sparam_high.sched_ss_repl_period.tv_nsec = thr1_period_ns; - sparam_high.sched_ss_init_budget.tv_sec = 0; - sparam_high.sched_ss_init_budget.tv_nsec = thr1_budget_ns; - sparam_high.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; - - sparam_low.sched_priority = thr2_prio_high; - sparam_low.sched_ss_low_priority = thr2_prio_low; - sparam_low.sched_ss_repl_period.tv_sec = 0; - sparam_low.sched_ss_repl_period.tv_nsec = thr2_period_ns; - sparam_low.sched_ss_init_budget.tv_sec = 0; - sparam_low.sched_ss_init_budget.tv_nsec = thr2_budget_ns; - sparam_low.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; - - printf("thread 1: high prio %d low prio %d budget %d ms period %d ms \n", - sparam_high.sched_priority, sparam_high.sched_ss_low_priority, - sparam_high.sched_ss_init_budget.tv_nsec/1000000, sparam_high.sched_ss_repl_period.tv_nsec / 1000000); - printf("thread 2: high prio %d low prio %d budget %d ms period %d ms \n", - sparam_low.sched_priority, sparam_low.sched_ss_low_priority, - sparam_low.sched_ss_init_budget.tv_nsec/1000000, sparam_low.sched_ss_repl_period.tv_nsec / 1000000); - - // first worker thread - pthread_attr_t attr_high, attr_low; - pthread_t worker_thread_high, worker_thread_low; - int id_high=1, id_low=2; - - (void)pthread_attr_init(&attr_high); - #ifdef _POSIX_SPORADIC_SERVER - printf("var _POSIX_SPORADIC_SERVER defined\n"); - #endif - printf("SCHED_SPORADIC = %d\n",SCHED_SPORADIC); - ret = pthread_attr_setschedpolicy(&attr_high, SCHED_SPORADIC); - if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", - ret); - } - (void)pthread_attr_setschedparam(&attr_high, &sparam_high); - - printf("param policy %d low_prio %d budget %d period %d\n", - attr_high.policy, - attr_high.low_priority, - attr_high.budget.tv_nsec, - attr_high.repl_period.tv_nsec); - - ret = pthread_create(&worker_thread_high, &attr_high, &worker_thread_run, &id_high); - if (ret != 0) - { - printf("uros_rbs: pthread_create worker_thread_high failed: %d\n", ret); - return 1; - } else { - printf("uros_rbs: started worker_thread_high\n"); - } - - // second worker thread - (void)pthread_attr_init(&attr_low); - (void)pthread_attr_setschedparam(&attr_low, &sparam_low); - ret = pthread_attr_setschedpolicy(&attr_low, SCHED_SPORADIC); - if (ret != OK) - { - printf("sporadic_test: ERROR: pthread_attr_setschedpolicy failed, ret=%d\n", - ret); - } - ret = pthread_create(&worker_thread_low, &attr_low, &worker_thread_run, &id_low); - if (ret != 0) - { - printf("uros_rbs: pthread_create worker_thread_low failed: %d\n", ret); - return 1; - } else { - printf("uros_rbs: started worker_thread_low\n"); - } - - -// debugging parameters: - struct sched_param p; - - ret = nxsched_getparam(worker_thread_high, &p); - if (ret != 0) - { - printf("worker thread %d: nxsched_getparam failed: %d\n" , worker_thread_high, ret); - return; - } - printf("running worker-thread-high: pid %d prio %d budget %d ns period %d ns\n",worker_thread_high, p.sched_priority, - p.sched_ss_init_budget.tv_nsec, p.sched_ss_repl_period.tv_nsec); - - ret = nxsched_getparam(worker_thread_low, &p); - if (ret != 0) - { - printf("worker thread %d: nxsched_getparam failed: %d\n" , worker_thread_low, ret); - return; - } - printf("running worker-thread-high: pid %d prio %d budget %d ns period %d ns\n",worker_thread_low, p.sched_priority, - p.sched_ss_init_budget.tv_nsec, p.sched_ss_repl_period.tv_nsec); - - for(unsigned int i=0;i<10;i++) - { - // printf("main sleeping\n"); - sleep(1); - } - - ret = pthread_cancel(worker_thread_high); - if (ret != 0) - { - fprintf(stderr, "uros_rbs: pthread_cancel worker_thread_high failed\n"); - } - else - { - printf("uros_rbs: worker_thread_high returned.\n"); - } - - ret = pthread_cancel(worker_thread_low); - if (ret != 0) - { - fprintf(stderr, "uros_rbs: pthread_cancel worker_thread_low failed\n"); - } - else - { - printf("uros_rbs: worker_thread_low returned.\n"); - } - - printf("progress high %d ms progress low %d ms\n", thread_1_cnt, thread_2_cnt); -} */ +} #if defined(BUILD_MODULE) int main(int argc, char *argv[]) @@ -823,21 +187,28 @@ int main(int argc, char *argv[]) int uros_rbs_main(int argc, char* argv[]) #endif { - // rcl_allocator_t allocator = rcl_get_default_allocator(); - // rclc_support_t support; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rclc_support_t support; int ret; - - sporadic_test(); - // test_sporadic_scheduling(40, 4, 4000000, 10000000, - // 50, 5, 2000000, 10000000); - + int budget_ms = 30; -/* + if (argc ==2){ + // budget_ms = atoi(argv[1]); + sleep_ms = atoi(argv[1]); + } + // period_ms + // low_pong_policy + // high_ping_prio + // low_ping_prio + printf("high_pong budget %d ms\n", budget_ms); + printf("low_pong sleep %d ms\n", sleep_ms); + // tests + delay_test(); // Set the executor thread priority struct sched_param exe_param; - exe_param.sched_priority = 100; + exe_param.sched_priority = 110; ret = sched_setparam(0, &exe_param); if (ret < 0) { @@ -845,53 +216,45 @@ int uros_rbs_main(int argc, char* argv[]) return 1; } - RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); - rcl_node_t node = rcl_get_zero_initialized_node(); RCCHECK(rclc_node_init_default(&node, "pong_rclc_node", "", &support)); - RCCHECK(rclc_publisher_init_best_effort(&high_pong_publisher_, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "high_pong")); RCCHECK(rclc_subscription_init_best_effort(&high_ping_subscription_, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "high_ping")); - RCCHECK(rclc_publisher_init_best_effort(&low_pong_publisher_, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "low_pong")); RCCHECK(rclc_subscription_init_best_effort(&low_ping_subscription_, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "low_ping")); rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator)); - struct sched_param sparam_high, sparam_low; - //sparam_high.priority = sched_get_priority_max(SCHED_FIFO); - //sparam_low.priority = sched_get_priority_min(SCHED_FIFO); - printf("max prio %d min prio %d\n", SCHED_PRIORITY_MAX, SCHED_PRIORITY_MIN); - - sparam_high.sched_priority = 60; - sparam_high.sched_ss_low_priority = 3; - sparam_high.sched_ss_repl_period.tv_sec = 0; - sparam_high.sched_ss_repl_period.tv_nsec = 10000000; // 10ms - sparam_high.sched_ss_init_budget.tv_sec = 1; - sparam_high.sched_ss_init_budget.tv_nsec = 6000000; // 6ms - sparam_high.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; - - sparam_low.sched_priority = 50; - sparam_low.sched_ss_low_priority = 2; - sparam_low.sched_ss_repl_period.tv_sec = 0; - sparam_low.sched_ss_repl_period.tv_nsec = 10000000; // 10ms - sparam_low.sched_ss_init_budget.tv_sec = 1; - sparam_low.sched_ss_init_budget.tv_nsec = 4000000; // 4ms - sparam_low.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; - printf("high prio %d low prio %d\n", sparam_high.sched_priority, sparam_low.sched_priority); - - printf("subscription high ping \n"); + // sparam_high.priority = sched_get_priority_max(SCHED_FIFO); + // sparam_low.priority = sched_get_priority_min(SCHED_FIFO); + // printf("max prio %d min prio %d\n", SCHED_PRIORITY_MAX, SCHED_PRIORITY_MIN); => [0, 255] + rclc_executor_sched_parameter_t sparam_high; + sparam_high.policy = SCHED_SPORADIC; + sparam_high.param.sched_priority = 60; + sparam_high.param.sched_ss_low_priority = 6; + sparam_high.param.sched_ss_repl_period.tv_sec = 0; + sparam_high.param.sched_ss_repl_period.tv_nsec = 100000000; // 100ms + sparam_high.param.sched_ss_init_budget.tv_sec = 0; + sparam_high.param.sched_ss_init_budget.tv_nsec = budget_ms*1000000; + sparam_high.param.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; + + rclc_executor_sched_parameter_t sparam_low; + sparam_low.policy = SCHED_FIFO; + sparam_low.param.sched_priority = 50; + /* + sparam_low.param.sched_ss_low_priority = 2; + sparam_low.param.sched_ss_repl_period.tv_sec = 0; + sparam_low.param.sched_ss_repl_period.tv_nsec = 10000000; // 10ms + sparam_low.param.sched_ss_init_budget.tv_sec = 0; + sparam_low.param.sched_ss_init_budget.tv_nsec = 4000000; // 4ms + sparam_low.param.sched_ss_max_repl = CONFIG_SCHED_SPORADIC_MAXREPL; + */ + printf("high_pong prio %d low_pong prio %d\n", sparam_high.param.sched_priority, sparam_low.param.sched_priority); RCCHECK(rclc_executor_add_subscription_sched(&executor, &high_ping_subscription_, &high_ping_msg_, &high_ping_received, ON_NEW_DATA, &sparam_high)); - - printf("subscription low ping \n"); RCCHECK(rclc_executor_add_subscription_sched(&executor, &low_ping_subscription_, &low_ping_msg_, &low_ping_received, ON_NEW_DATA, &sparam_low)); - - // int sched_policy = SCHED_SPORADIC; - int sched_policy = SCHED_FIFO; - - RCCHECK(rclc_executor_start_multi_threading_for_nuttx(&executor, sched_policy)); + RCCHECK(rclc_executor_start_multi_threading_for_nuttx(&executor)); printf("clean up \n"); RCCHECK(rclc_executor_fini(&executor)); @@ -899,25 +262,7 @@ int uros_rbs_main(int argc, char* argv[]) RCCHECK(rcl_publisher_fini(&high_pong_publisher_, &node)); RCCHECK(rcl_subscription_fini(&low_ping_subscription_, &node)); RCCHECK(rcl_publisher_fini(&low_pong_publisher_, &node)); - RCCHECK(rcl_node_fini(&node)); - */ - return 0; + return 0; } -/* -Error message missing library rt for clock_getcpuclockid(pthread_self(), &clockId); -but this function is not implemented in NuttX! (01-02-2021) - -arm-none-eabi-ld --entry=__start -nostartfiles -nodefaultlibs -g -T/home/jst3si/olimex_ws/firmware/NuttX/configs/olimex-stm32-e407/scripts/ld.script -L"/home/jst3si/olimex_ws/firmware/NuttX/staging" -L"/home/jst3si/olimex_ws/firmware/NuttX/arch/arm/src/board" -L "/usr/lib/gcc/arm-none-eabi/6.3.1/../../../arm-none-eabi/lib/thumb/v7e-m/fpv4-sp/hard" \ - -o "/home/jst3si/olimex_ws/firmware/NuttX/nuttx" \ - --start-group -lsched -ldrivers -lconfigs -lc -lmm -larch -lxx -lapps -lnet -lfs -lbinfmt -lxx -lboard -lsupc++ "/usr/lib/gcc/arm-none-eabi/6.3.1/thumb/v7e-m/fpv4-sp/hard/libgcc.a" --end-group -/home/jst3si/olimex_ws/firmware/NuttX/staging/libapps.a(main_rbs.o): In function `burn_cpu_cycles': -/home/jst3si/olimex_ws/firmware/apps/examples/uros_rbs/main_rbs.c:49: undefined reference to `clock_getcpuclockid' -Makefile:184: recipe for target 'nuttx' failed -make[1]: *** [nuttx] Error 1 -make[1]: Verzeichnis „/home/jst3si/olimex_ws/firmware/NuttX/arch/arm/src“ wird verlassen -tools/Makefile.unix:422: recipe for target 'pass2' failed -make: *** [pass2] Error 2 - -*/ From 89fe34ea611e765c1192d2df380a34df64577326 Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Thu, 11 Feb 2021 15:09:49 +0100 Subject: [PATCH 17/18] added setup of rcl_wait timeout --- examples/uros_rbs/main_rbs.c | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index aa044915..f78c91ae 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -168,7 +168,7 @@ void high_ping_received(const void * pong_msg) void low_ping_received(const void * pong_msg) { my_mdelay(10); - usleep(sleep_ms*1000); + // usleep(sleep_ms*1000); printf("low ping received.\n"); RCUNUSED(rclc_executor_publish(&low_pong_publisher_, pong_msg, NULL)); @@ -191,11 +191,14 @@ int uros_rbs_main(int argc, char* argv[]) rclc_support_t support; int ret; int budget_ms = 30; + sleep_ms = 0; + unsigned int timeout_ms = 100; if (argc ==2){ // budget_ms = atoi(argv[1]); - sleep_ms = atoi(argv[1]); + // sleep_ms = atoi(argv[1]); + timeout_ms = atoi(argv[1]); } // period_ms // low_pong_policy @@ -203,6 +206,7 @@ int uros_rbs_main(int argc, char* argv[]) // low_ping_prio printf("high_pong budget %d ms\n", budget_ms); printf("low_pong sleep %d ms\n", sleep_ms); + printf("parameter rcl_wait timeout %d ms\n", timeout_ms); // tests delay_test(); @@ -226,7 +230,8 @@ int uros_rbs_main(int argc, char* argv[]) rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator)); - + rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(timeout_ms)); + // sparam_high.priority = sched_get_priority_max(SCHED_FIFO); // sparam_low.priority = sched_get_priority_min(SCHED_FIFO); // printf("max prio %d min prio %d\n", SCHED_PRIORITY_MAX, SCHED_PRIORITY_MIN); => [0, 255] From b8e8001a63ae41cfd48763b03fa5741c43ce4b09 Mon Sep 17 00:00:00 2001 From: Jan Staschulat Date: Mon, 1 Mar 2021 13:55:10 +0100 Subject: [PATCH 18/18] some cleanup --- examples/uros_rbs/main_rbs.c | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/examples/uros_rbs/main_rbs.c b/examples/uros_rbs/main_rbs.c index f78c91ae..007f55f8 100644 --- a/examples/uros_rbs/main_rbs.c +++ b/examples/uros_rbs/main_rbs.c @@ -39,7 +39,7 @@ %s. Continuing.\n",__LINE__,(int)temp_rc, rcl_get_error_string().str);rcutils_reset_error();}} #define RCUNUSED(fn) { rcl_ret_t temp_rc __attribute__((unused)); temp_rc = fn; } -static unsigned int sleep_ms; +// static unsigned int sleep_ms; rcl_subscription_t high_ping_subscription_; std_msgs__msg__Int32 high_ping_msg_; @@ -73,7 +73,7 @@ static void my_mdelay2(unsigned int milliseconds) } } } - +/* void delay_test(void) { struct timespec startTimeP; @@ -103,6 +103,7 @@ void delay_test(void) printf("usleep test: %d ms duration %ld\n", sleep_ms, measuredDuration/1000000); } +*/ /* void burn_cpu_cycles_high(long duration) { @@ -160,7 +161,7 @@ void burn_cpu_cycles_low(long duration) void high_ping_received(const void * pong_msg) { my_mdelay(10); // 10ms TODO: Get this value from parameter 'high_busyloop'. - printf("high ping received.\n"); + // printf("high ping received.\n"); RCUNUSED(rclc_executor_publish(&high_pong_publisher_, pong_msg, NULL)); } @@ -169,7 +170,7 @@ void low_ping_received(const void * pong_msg) { my_mdelay(10); // usleep(sleep_ms*1000); - printf("low ping received.\n"); + // printf("low ping received.\n"); RCUNUSED(rclc_executor_publish(&low_pong_publisher_, pong_msg, NULL)); /* @@ -191,7 +192,7 @@ int uros_rbs_main(int argc, char* argv[]) rclc_support_t support; int ret; int budget_ms = 30; - sleep_ms = 0; + // sleep_ms = 0; unsigned int timeout_ms = 100; @@ -205,10 +206,10 @@ int uros_rbs_main(int argc, char* argv[]) // high_ping_prio // low_ping_prio printf("high_pong budget %d ms\n", budget_ms); - printf("low_pong sleep %d ms\n", sleep_ms); + // printf("low_pong sleep %d ms\n", sleep_ms); printf("parameter rcl_wait timeout %d ms\n", timeout_ms); // tests - delay_test(); + // delay_test(); // Set the executor thread priority struct sched_param exe_param; @@ -230,7 +231,12 @@ int uros_rbs_main(int argc, char* argv[]) rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); RCCHECK(rclc_executor_init(&executor, &support.context, 2, &allocator)); - rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(timeout_ms)); + RCCHECK(rclc_executor_set_timeout(&executor, RCL_MS_TO_NS(timeout_ms))); + printf("main: executor->timeout_ns %d\n", executor.timeout_ns); + RCCHECK(rclc_executor_set_timeout(&executor, timeout_ms)); + printf("main: executor->timeout_ns %d\n", executor.timeout_ns); + RCCHECK(rclc_executor_set_timeout(&executor, 420420420)); + printf("main: executor->timeout_ns %d\n", executor.timeout_ns); // sparam_high.priority = sched_get_priority_max(SCHED_FIFO); // sparam_low.priority = sched_get_priority_min(SCHED_FIFO);