added support for STM32F4 and Ethernet (#593) (#602)

* added support for STM32F4 and Ethernet

* Revert library changes

Signed-off-by: Pablo Garrido <[email protected]>

Co-authored-by: Pablo Garrido <[email protected]>
(cherry picked from commit 03779a8)

Co-authored-by: Dominik Nowak <[email protected]>

Author: mergify[bot]

extras/library_generation/cortex_m4_toolchain.cmake

@@ -0,0 +1,16 @@
+SET(CMAKE_SYSTEM_NAME Generic)
+set(CMAKE_CROSSCOMPILING 1)
+set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
+
+set(CMAKE_C_COMPILER $ENV{TOOLCHAIN_PREFIX}gcc)
+set(CMAKE_CXX_COMPILER $ENV{TOOLCHAIN_PREFIX}g++)
+
+SET(CMAKE_C_COMPILER_WORKS 1 CACHE INTERNAL "")
+SET(CMAKE_CXX_COMPILER_WORKS 1 CACHE INTERNAL "")
+
+set(FLAGS "-O2 -mfloat-abi=hard -mfpu=vfpv3-d16 -ffunction-sections -fdata-sections -fno-exceptions -mcpu=cortex-m4 -nostdlib -DARDUINO=10813 -mthumb --param max-inline-insns-single=500 -DF_CPU=168000000L -D'RCUTILS_LOG_MIN_SEVERITY=RCUTILS_LOG_MIN_SEVERITY_NONE'" CACHE STRING "" FORCE)
+
+set(CMAKE_C_FLAGS_INIT "-std=c11 ${FLAGS} -DCLOCK_MONOTONIC=0 -D'__attribute__(x)='" CACHE STRING "" FORCE)
+set(CMAKE_CXX_FLAGS_INIT "-std=c++14 ${FLAGS} -fno-rtti -DCLOCK_MONOTONIC=0 -D'__attribute__(x)='" CACHE STRING "" FORCE)
+
+set(__BIG_ENDIAN__ 0)

extras/library_generation/library_generation.sh

@@ -16,6 +16,7 @@ if [ $OPTIND -eq 1 ]; then
     PLATFORMS+=("teensy35")
     PLATFORMS+=("cortex_m0")
     PLATFORMS+=("cortex_m3")
+    PLATFORMS+=("cortex_m4")
     # PLATFORMS+=("portenta-m4")
     PLATFORMS+=("portenta-m7")
     PLATFORMS+=("kakutef7-m7")
@@ -96,6 +97,20 @@ if [[ " ${PLATFORMS[@]} " =~ " cortex_m3 " ]]; then
     cp -R firmware/build/libmicroros.a /project/src/cortex-m3/libmicroros.a
 fi
 
+######## Build for STM32F4 ########
+if [[ " ${PLATFORMS[@]} " =~ " cortex_m4 " ]]; then
+    rm -rf firmware/build
+
+    export TOOLCHAIN_PREFIX=/uros_ws/gcc-arm-none-eabi-9-2020-q2-update/bin/arm-none-eabi-
+    ros2 run micro_ros_setup build_firmware.sh /project/extras/library_generation/cortex_m4_toolchain.cmake /project/extras/library_generation/colcon_lowmem.meta
+
+    find firmware/build/include/ -name "*.c"  -delete
+    cp -R firmware/build/include/* /project/src/
+
+    mkdir -p /project/src/cortex-m4
+    cp -R firmware/build/libmicroros.a /project/src/cortex-m4/libmicroros.a
+fi
+
 ######## Build for Teensy 3.2 ########
 if [[ " ${PLATFORMS[@]} " =~ " teensy32 " ]]; then
     rm -rf firmware/build

library.properties

@@ -7,4 +7,4 @@ paragraph=micro-ROS Arduino library
 url=https://github.com/micro-ROS
 precompiled=true
 category=Other
-architectures=OpenCR,Teensyduino,samd,sam,mbed
+architectures=stm32,OpenCR,Teensyduino,samd,sam,mbed

src/micro_ros_arduino.h

@@ -32,6 +32,49 @@ static inline void set_microros_transports(){
 	);
 }
 
+#if defined(TARGET_STM32F4)
+
+#include <Arduino.h>
+#include <EthernetUdp.h>
+#include <LwIP.h>
+#include <STM32Ethernet.h>
+
+#include <uxr/client/transport.h>
+#include <rmw_microros/rmw_microros.h>
+#include "IPAddress.h"
+
+extern "C" bool arduino_native_ethernet_udp_transport_open(struct uxrCustomTransport * transport);
+extern "C" bool arduino_native_ethernet_udp_transport_close(struct uxrCustomTransport * transport);
+extern "C" size_t arduino_native_ethernet_udp_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
+extern "C" size_t arduino_native_ethernet_udp_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);
+
+struct micro_ros_agent_locator {
+	IPAddress address;
+	int port;
+};
+
+static inline void set_microros_native_ethernet_udp_transports(byte mac[], IPAddress client_ip, IPAddress agent_ip, uint16_t agent_port){
+
+	static struct micro_ros_agent_locator locator;
+
+   	Ethernet.begin(mac, client_ip);
+	delay(1000);
+
+	locator.address = agent_ip;
+	locator.port = agent_port;
+
+	rmw_uros_set_custom_transport(
+		false,
+		(void *) &locator,
+		arduino_native_ethernet_udp_transport_open,
+		arduino_native_ethernet_udp_transport_close,
+		arduino_native_ethernet_udp_transport_write,
+		arduino_native_ethernet_udp_transport_read
+	);
+}
+
+#endif
+
 #if defined(TARGET_PORTENTA_H7_M7) || defined(ARDUINO_NANO_RP2040_CONNECT)
 
 #if defined(TARGET_PORTENTA_H7_M7)

src/native_ethernet_transport.cpp

@@ -0,0 +1,80 @@
+#if defined(TARGET_STM32F4)
+
+#include <Arduino.h>
+#include <EthernetUdp.h>
+#include <STM32Ethernet.h>
+#include <micro_ros_arduino.h>
+
+extern "C" {
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <sys/time.h>
+
+static EthernetUDP udp_client;
+
+bool arduino_native_ethernet_udp_transport_open(
+    struct uxrCustomTransport *transport) {
+  struct micro_ros_agent_locator *locator =
+      (struct micro_ros_agent_locator *)transport->args;
+  udp_client.begin(locator->port);
+  return true;
+}
+
+bool arduino_native_ethernet_udp_transport_close(
+    struct uxrCustomTransport *transport) {
+  udp_client.stop();
+  return true;
+}
+
+size_t arduino_native_ethernet_udp_transport_write(
+    struct uxrCustomTransport *transport, const uint8_t *buf, size_t len,
+    uint8_t *errcode) {
+  (void)errcode;
+  struct micro_ros_agent_locator *locator =
+      (struct micro_ros_agent_locator *)transport->args;
+
+  udp_client.beginPacket(locator->address, locator->port);
+  size_t sent = udp_client.write(buf, len);
+  udp_client.endPacket();
+  udp_client.flush();
+
+  return sent;
+}
+
+size_t arduino_native_ethernet_udp_transport_read(
+    struct uxrCustomTransport *transport, uint8_t *buf, size_t len, int timeout,
+    uint8_t *errcode) {
+  (void)errcode;
+  uint32_t start_time = millis();
+
+  while (millis() - start_time < ((uint32_t)timeout) &&
+         udp_client.parsePacket() == 0) {
+    delay(1);
+  }
+
+  size_t readed = udp_client.read(buf, len);
+  return (readed < 0) ? 0 : readed;
+}
+
+#define micro_rollover_useconds 4294967295
+
+int clock_gettime(clockid_t unused, struct timespec *tp) {
+  (void)unused;
+  static uint32_t rollover = 0;
+  static uint32_t last_measure = 0;
+
+  uint32_t m = micros();
+  rollover += (m < last_measure) ? 1 : 0;
+
+  uint64_t real_us = (uint64_t)(m + rollover * micro_rollover_useconds);
+  tp->tv_sec = real_us / 1000000;
+  tp->tv_nsec = (real_us % 1000000) * 1000;
+  last_measure = m;
+
+  return 0;
+}
+
+}
+
+#endif