diff --git a/README.md b/README.md index 0cc4d2c..901160a 100644 --- a/README.md +++ b/README.md @@ -9,11 +9,12 @@ This tool aims to ease the micro-ROS integration in a STM32CubeMX/IDE project. - [Middlewares available](#middlewares-available) - [Using this package with STM32CubeMX](#using-this-package-with-stm32cubemx) - [Using this package with STM32CubeIDE](#using-this-package-with-stm32cubeide) - - [Windows 11 (Community Contributed)](#STM32CubeIDE-Win11) + - [Windows 11 (Community Contributed)](#windows-11-community-contributed) - [Transport configuration](#transport-configuration) - [U(S)ART with DMA](#usart-with-dma) - [U(S)ART with Interrupts](#usart-with-interrupts) - [USB CDC](#usb-cdc) + - [UDP](#udp) - [Customizing the micro-ROS library](#customizing-the-micro-ros-library) - [Adding custom packages](#adding-custom-packages) - [Purpose of the Project](#purpose-of-the-project) @@ -134,6 +135,31 @@ Steps to configure: **Note: The micro-ROS transport will override the autogenerated `USB_DEVICE/App/usbd_cdc_if.c` methods.** +### UDP + +Steps to configure: + - Enable Ethernet in your STM32CubeMX/IDE `Connectivity` tab. + - Enable LwIP in your STM32CubeMX/IDE `Middleware` tab. + - Make sure that LwIP has the following configuration: + + ``` + Platform Setting according to your own board + LwIP -> General Settings -> LWIP_DHCP -> Disabled + LwIP -> General Settings -> IP Address Settings (Set here the board address and mask) + LwIP -> General Settings -> LWIP UDP -> Enabled + LwIP -> General Settings -> Procols Options -> MEMP_NUM_UDP_PCB -> 15 + LwIP -> Key Options -> LWIP_SO_RCVTIMEO -> Enable + ``` + + **Note: Ensure your board and Agent are within the same LAN. The default port is 8888. You can modify it in `udp_transport.c`.If you are using a board from the STM32H7 series, please set up the MPU correctly.** + + - Use `sample_main_udp.c` as a reference for writing your application code. + - Start the micro-ROS Agent with the following arguments: + + ``` + ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -v 6 + ``` + ## Customizing the micro-ROS library All the micro-ROS configuration can be done in `colcon.meta` file before step 3. You can find detailed information about how to tune the static memory usage of the library in the [Middleware Configuration tutorial](https://micro.ros.org/docs/tutorials/advanced/microxrcedds_rmw_configuration/). diff --git a/extra_sources/microros_transports/udp_transport.c b/extra_sources/microros_transports/udp_transport.c new file mode 100644 index 0000000..4427a51 --- /dev/null +++ b/extra_sources/microros_transports/udp_transport.c @@ -0,0 +1,79 @@ +#include + +#include + +#include "main.h" +#include "cmsis_os.h" + +#include +#include +#include +#include + +// --- LWIP --- +#include "lwip/opt.h" +#include "lwip/sys.h" +#include "lwip/api.h" +#include + +#ifdef RMW_UXRCE_TRANSPORT_CUSTOM + +// --- micro-ROS Transports --- +#define UDP_PORT 8888 +static int sock_fd = -1; + +bool cubemx_transport_open(struct uxrCustomTransport * transport){ + sock_fd = socket(AF_INET, SOCK_DGRAM, 0); + struct sockaddr_in addr; + addr.sin_family = AF_INET; + addr.sin_port = htons(UDP_PORT); + addr.sin_addr.s_addr = htonl(INADDR_ANY); + + if (bind(sock_fd, (struct sockaddr *)&addr, sizeof(addr)) == -1) + { + return false; + } + + return true; +} + +bool cubemx_transport_close(struct uxrCustomTransport * transport){ + if (sock_fd != -1) + { + closesocket(sock_fd); + sock_fd = -1; + } + return true; +} + +size_t cubemx_transport_write(struct uxrCustomTransport* transport, uint8_t * buf, size_t len, uint8_t * err){ + if (sock_fd == -1) + { + return 0; + } + const char * ip_addr = (const char*) transport->args; + struct sockaddr_in addr; + addr.sin_family = AF_INET; + addr.sin_port = htons(UDP_PORT); + addr.sin_addr.s_addr = inet_addr(ip_addr); + int ret = 0; + ret = sendto(sock_fd, buf, len, 0, (struct sockaddr *)&addr, sizeof(addr)); + size_t writed = ret>0? ret:0; + + return writed; +} + +size_t cubemx_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err){ + + int ret = 0; + //set timeout + struct timeval tv_out; + tv_out.tv_sec = timeout / 1000; + tv_out.tv_usec = (timeout % 1000) * 1000; + setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO,&tv_out, sizeof(tv_out)); + ret = recv(sock_fd, buf, len, MSG_WAITALL); + size_t readed = ret > 0 ? ret : 0; + return readed; +} + +#endif \ No newline at end of file diff --git a/sample_main_udp.c b/sample_main_udp.c new file mode 100644 index 0000000..2131d82 --- /dev/null +++ b/sample_main_udp.c @@ -0,0 +1,347 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : main.c + * @brief : Main program body + ****************************************************************************** + * @attention + * + * Copyright (c) 2023 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Includes ------------------------------------------------------------------*/ +#include "main.h" +#include "cmsis_os.h" +#include "lwip.h" + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ +#include +#include +#include +#include +#include +#include +#include + +#include +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN PTD */ + +/* USER CODE END PTD */ + +/* Private define ------------------------------------------------------------*/ +/* USER CODE BEGIN PD */ + +/* USER CODE END PD */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN PM */ + +/* USER CODE END PM */ + +/* Private variables ---------------------------------------------------------*/ +/* Definitions for defaultTask */ +osThreadId_t defaultTaskHandle; +const osThreadAttr_t defaultTask_attributes = { + .name = "defaultTask", + .priority = (osPriority_t) osPriorityNormal, + .stack_size = 3000 * 4, +}; +/* USER CODE BEGIN PV */ + +/* USER CODE END PV */ + +/* Private function prototypes -----------------------------------------------*/ +void SystemClock_Config(void); +static void MX_GPIO_Init(void); +void StartDefaultTask(void *argument); + +/* USER CODE BEGIN PFP */ + +/* USER CODE END PFP */ + +/* Private user code ---------------------------------------------------------*/ +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ + +/** + * @brief The application entry point. + * @retval int + */ +int main(void) +{ + /* USER CODE BEGIN 1 */ + + /* USER CODE END 1 */ + + /* MCU Configuration--------------------------------------------------------*/ + + /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ + HAL_Init(); + + /* USER CODE BEGIN Init */ + + /* USER CODE END Init */ + + /* Configure the system clock */ + SystemClock_Config(); + + /* USER CODE BEGIN SysInit */ + + /* USER CODE END SysInit */ + + /* Initialize all configured peripherals */ + MX_GPIO_Init(); + /* USER CODE BEGIN 2 */ + + /* USER CODE END 2 */ + + /* Init scheduler */ + osKernelInitialize(); + + /* USER CODE BEGIN RTOS_MUTEX */ + /* add mutexes, ... */ + /* USER CODE END RTOS_MUTEX */ + + /* USER CODE BEGIN RTOS_SEMAPHORES */ + /* add semaphores, ... */ + /* USER CODE END RTOS_SEMAPHORES */ + + /* USER CODE BEGIN RTOS_TIMERS */ + /* start timers, add new ones, ... */ + /* USER CODE END RTOS_TIMERS */ + + /* USER CODE BEGIN RTOS_QUEUES */ + /* add queues, ... */ + /* USER CODE END RTOS_QUEUES */ + + /* Create the thread(s) */ + /* creation of defaultTask */ + defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes); + + /* USER CODE BEGIN RTOS_THREADS */ + /* add threads, ... */ + /* USER CODE END RTOS_THREADS */ + + /* USER CODE BEGIN RTOS_EVENTS */ + /* add events, ... */ + /* USER CODE END RTOS_EVENTS */ + + /* Start scheduler */ + osKernelStart(); + /* We should never get here as control is now taken by the scheduler */ + /* Infinite loop */ + /* USER CODE BEGIN WHILE */ + while (1) + { + /* USER CODE END WHILE */ + + /* USER CODE BEGIN 3 */ + } + /* USER CODE END 3 */ +} + +/** + * @brief System Clock Configuration + * @retval None + */ +void SystemClock_Config(void) +{ + RCC_OscInitTypeDef RCC_OscInitStruct = {0}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; + + /** Configure the main internal regulator output voltage + */ + __HAL_RCC_PWR_CLK_ENABLE(); + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); + + /** Initializes the RCC Oscillators according to the specified parameters + * in the RCC_OscInitTypeDef structure. + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; + RCC_OscInitStruct.HSIState = RCC_HSI_ON; + RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; + RCC_OscInitStruct.PLL.PLLM = 8; + RCC_OscInitStruct.PLL.PLLN = 168; + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; + RCC_OscInitStruct.PLL.PLLQ = 4; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK + |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) + { + Error_Handler(); + } +} + +/** + * @brief GPIO Initialization Function + * @param None + * @retval None + */ +static void MX_GPIO_Init(void) +{ +/* USER CODE BEGIN MX_GPIO_Init_1 */ +/* USER CODE END MX_GPIO_Init_1 */ + + /* GPIO Ports Clock Enable */ + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + +/* USER CODE BEGIN MX_GPIO_Init_2 */ +/* USER CODE END MX_GPIO_Init_2 */ +} + +/* USER CODE BEGIN 4 */ + +/* USER CODE END 4 */ + +/* USER CODE BEGIN Header_StartDefaultTask */ +/** + * @brief Function implementing the defaultTask thread. + * @param argument: Not used + * @retval None + */ +/* USER CODE END Header_StartDefaultTask */ +void StartDefaultTask(void *argument) +{ + /* init code for LWIP */ + MX_LWIP_Init(); + /* USER CODE BEGIN 5 */ + + // micro-ROS configuration + + rmw_uros_set_custom_transport( + false, //Framing disable here. Udp should Use Packet-oriented mode. + "192.168.1.121", //your Agent's ip address + cubemx_transport_open, + cubemx_transport_close, + cubemx_transport_write, + cubemx_transport_read); + + rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator(); + freeRTOS_allocator.allocate = microros_allocate; + freeRTOS_allocator.deallocate = microros_deallocate; + freeRTOS_allocator.reallocate = microros_reallocate; + freeRTOS_allocator.zero_allocate = microros_zero_allocate; + + if (!rcutils_set_default_allocator(&freeRTOS_allocator)) { + printf("Error on default allocators (line %d)\n", __LINE__); + } + + // micro-ROS app + + rcl_publisher_t publisher; + std_msgs__msg__Int32 msg; + rclc_support_t support; + rcl_allocator_t allocator; + rcl_node_t node; + + allocator = rcl_get_default_allocator(); + + //create init_options + rclc_support_init(&support, 0, NULL, &allocator); + + // create node + rclc_node_init_default(&node, "cubemx_node", "", &support); + + // create publisher + rclc_publisher_init_default( + &publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), + "cubemx_publisher"); + + msg.data = 0; + + for(;;) + { + rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL); + if (ret != RCL_RET_OK) + { + printf("Error publishing (line %d)\n", __LINE__); + } + + msg.data++; + osDelay(10); + } + /* USER CODE END 5 */ +} + +/** + * @brief Period elapsed callback in non blocking mode + * @note This function is called when TIM1 interrupt took place, inside + * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment + * a global variable "uwTick" used as application time base. + * @param htim : TIM handle + * @retval None + */ +void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) +{ + /* USER CODE BEGIN Callback 0 */ + + /* USER CODE END Callback 0 */ + if (htim->Instance == TIM1) { + HAL_IncTick(); + } + /* USER CODE BEGIN Callback 1 */ + + /* USER CODE END Callback 1 */ +} + +/** + * @brief This function is executed in case of error occurrence. + * @retval None + */ +void Error_Handler(void) +{ + /* USER CODE BEGIN Error_Handler_Debug */ + /* User can add his own implementation to report the HAL error return state */ + __disable_irq(); + while (1) + { + } + /* USER CODE END Error_Handler_Debug */ +} + +#ifdef USE_FULL_ASSERT +/** + * @brief Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * @param file: pointer to the source file name + * @param line: assert_param error line source number + * @retval None + */ +void assert_failed(uint8_t *file, uint32_t line) +{ + /* USER CODE BEGIN 6 */ + /* User can add his own implementation to report the file name and line number, + ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ + /* USER CODE END 6 */ +} +#endif /* USE_FULL_ASSERT */ \ No newline at end of file