From 7800118e599055eb936496c7f689d559c18174ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Aleksander=20Szyma=C5=84ski?= Date: Thu, 4 Jan 2024 14:57:22 +0100 Subject: [PATCH 1/3] added mechanisms of reseting the current_pointer after controller reset MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Aleksander Szymański --- Inc/firmware/microros_allocators.hpp | 3 ++- Src/firmware/mainf.cpp | 5 +++++ Src/firmware/microros_allocators.cpp | 6 +++++- 3 files changed, 12 insertions(+), 2 deletions(-) diff --git a/Inc/firmware/microros_allocators.hpp b/Inc/firmware/microros_allocators.hpp index 363570a..d0b5c1e 100644 --- a/Inc/firmware/microros_allocators.hpp +++ b/Inc/firmware/microros_allocators.hpp @@ -5,9 +5,10 @@ uint32_t max_used_heap(); void free_all_heap(); +void set_heap(uint32_t pointer); void* microros_allocate(size_t size, void* state); void microros_deallocate(void* pointer, void* state); void* microros_reallocate(void* pointer, size_t size, void* state); void* microros_zero_allocate(size_t number_of_elements, size_t size_of_element, - void* state); + void* state); diff --git a/Src/firmware/mainf.cpp b/Src/firmware/mainf.cpp index 7a93cee..f511ece 100644 --- a/Src/firmware/mainf.cpp +++ b/Src/firmware/mainf.cpp @@ -72,6 +72,8 @@ static std::atomic_bool publish_param_trigger(true); static bool mecanum_wheels = false; static std::atomic_bool controller_replacement(false); +static uint32_t reset_pointer_position; + #define WHEEL_WRAPPER(NAME) \ constexpr const char* NAME##_cmd_pwm_topic = \ "~/wheel_" #NAME "/cmd_pwm_duty"; \ @@ -401,6 +403,7 @@ void setup() { void initController() { mecanum_wheels = params.mecanum_wheels; + reset_pointer_position = max_used_heap(); if (mecanum_wheels) { rclc_publisher_init_best_effort( &wheel_odom_mecanum_pub, &node, @@ -424,6 +427,8 @@ void finiController() { } else { (void)!rcl_publisher_fini(&wheel_odom_pub, &node); } + controller->~RobotController(); + set_heap(reset_pointer_position); } void loop() { diff --git a/Src/firmware/microros_allocators.cpp b/Src/firmware/microros_allocators.cpp index e690b69..8d2560c 100644 --- a/Src/firmware/microros_allocators.cpp +++ b/Src/firmware/microros_allocators.cpp @@ -14,6 +14,10 @@ void free_all_heap() { current_pointer = 0; } +void set_heap(uint32_t pointer) { + current_pointer = pointer; +} + void assert_position() { if (current_pointer >= sizeof(heap)) { while (1) { @@ -56,7 +60,7 @@ void *microros_reallocate(void *pointer, size_t size, void *state) { } void *microros_zero_allocate(size_t number_of_elements, size_t size_of_element, - void *state) { + void *state) { size_t size = number_of_elements * size_of_element; if (size % 4 != 0) { From f6160e2f20fb9db62bfccae4996c1a95ea972f99 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 1 Feb 2024 22:47:52 +0100 Subject: [PATCH 2/3] Update diff_drive_lib library --- platformio.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platformio.ini b/platformio.ini index e001e05..5219d5a 100644 --- a/platformio.ini +++ b/platformio.ini @@ -25,7 +25,7 @@ board_microros_distro = humble-fictionlab board_microros_transport = serial board_microros_user_meta = colcon.meta lib_deps = - https://github.com/fictionlab/diff_drive_lib.git#1.5 + https://github.com/fictionlab/diff_drive_lib.git#1.6 https://github.com/fictionlab/micro_ros_platformio#fictionlab-v1 lib_compat_mode = off From 05669bdc4667966b0f804c26c1fb1d0ee1ad19ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 1 Feb 2024 22:55:42 +0100 Subject: [PATCH 3/3] Rename heap functions --- Inc/firmware/microros_allocators.hpp | 6 +++--- Src/firmware/mainf.cpp | 6 +++--- Src/firmware/microros_allocators.cpp | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/Inc/firmware/microros_allocators.hpp b/Inc/firmware/microros_allocators.hpp index d0b5c1e..f41cfa5 100644 --- a/Inc/firmware/microros_allocators.hpp +++ b/Inc/firmware/microros_allocators.hpp @@ -3,9 +3,9 @@ #include #include -uint32_t max_used_heap(); -void free_all_heap(); -void set_heap(uint32_t pointer); +uint32_t heap_get_current_pointer(); +void heap_free_all(); +void heap_set_current_pointer(uint32_t pointer); void* microros_allocate(size_t size, void* state); void microros_deallocate(void* pointer, void* state); diff --git a/Src/firmware/mainf.cpp b/Src/firmware/mainf.cpp index f511ece..f43b931 100644 --- a/Src/firmware/mainf.cpp +++ b/Src/firmware/mainf.cpp @@ -369,7 +369,7 @@ static void finiROS() { (void)!rcl_init_options_fini(&init_options); rclc_support_fini(&support); - free_all_heap(); + heap_free_all(); } static uint8_t uart_rbuffer[2048]; @@ -403,7 +403,7 @@ void setup() { void initController() { mecanum_wheels = params.mecanum_wheels; - reset_pointer_position = max_used_heap(); + reset_pointer_position = heap_get_current_pointer(); if (mecanum_wheels) { rclc_publisher_init_best_effort( &wheel_odom_mecanum_pub, &node, @@ -428,7 +428,7 @@ void finiController() { (void)!rcl_publisher_fini(&wheel_odom_pub, &node); } controller->~RobotController(); - set_heap(reset_pointer_position); + heap_set_current_pointer(reset_pointer_position); } void loop() { diff --git a/Src/firmware/microros_allocators.cpp b/Src/firmware/microros_allocators.cpp index 8d2560c..64dd9ea 100644 --- a/Src/firmware/microros_allocators.cpp +++ b/Src/firmware/microros_allocators.cpp @@ -6,15 +6,15 @@ static uint8_t heap[UROS_HEAP_SIZE]; static size_t current_pointer = 0; -uint32_t max_used_heap() { +uint32_t heap_get_current_pointer() { return current_pointer; } -void free_all_heap() { +void heap_free_all() { current_pointer = 0; } -void set_heap(uint32_t pointer) { +void heap_set_current_pointer(uint32_t pointer) { current_pointer = pointer; }