From 12a0fe78d2b7afefa210fed6c248d35e82978a54 Mon Sep 17 00:00:00 2001
From: Jenifen <46198024+Jenifen@users.noreply.github.com>
Date: Mon, 21 Mar 2022 16:20:36 +0100
Subject: [PATCH]  portenta h7 m7 ethernet support (#852)

* add ethernet trasport for PORTENTA_H7_M7 in uros header

* add ethernet trasport for PORTENTA_H7_M7 cpp

* add ethernet publish example

* correct error msg add

* add comment ip

* change topic_name, namespace

* update mac address

Co-authored-by: Pablo Garrido <pablogs9@gmail.com>

* remove comment

Co-authored-by: Pablo Garrido <pablogs9@gmail.com>

* remove not used headers

* fix error condition

Co-authored-by: Pablo Garrido <pablogs9@gmail.com>
(cherry picked from commit b12ed9b5aea001768bb57492c168813360a8c5cf)
---
 .../micro-ros_publisher_ethernet.ino          | 76 +++++++++++++++++++
 src/micro_ros_arduino.h                       |  9 ++-
 src/native_ethernet_transport.cpp             |  7 +-
 3 files changed, 89 insertions(+), 3 deletions(-)
 create mode 100644 examples/micro-ros_publisher_ethernet/micro-ros_publisher_ethernet.ino

diff --git a/examples/micro-ros_publisher_ethernet/micro-ros_publisher_ethernet.ino b/examples/micro-ros_publisher_ethernet/micro-ros_publisher_ethernet.ino
new file mode 100644
index 000000000..00cd9011a
--- /dev/null
+++ b/examples/micro-ros_publisher_ethernet/micro-ros_publisher_ethernet.ino
@@ -0,0 +1,76 @@
+#include <micro_ros_arduino.h>
+
+#include <stdio.h>
+#include <rcl/rcl.h>
+#include <rcl/error_handling.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
+#include <std_msgs/msg/int32.h>
+
+#if !defined(TARGET_STM32F4) && !defined(ARDUINO_TEENSY41) && !defined(TARGET_PORTENTA_H7_M7)
+#error This example is only avaible for Arduino Portenta, Arduino Teensy41 and STM32F4
+#endif
+
+rcl_publisher_t publisher;
+std_msgs__msg__Int32 msg;
+rclc_support_t support;
+rcl_allocator_t allocator;
+rcl_node_t node;
+
+#define LED_PIN 13
+
+#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
+#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
+
+
+
+void error_loop(){
+  while(1){
+    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
+    delay(100);
+  }
+}
+
+void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
+{
+  RCLC_UNUSED(last_call_time);
+  if (timer != NULL) {
+    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
+    msg.data++;
+  }
+}
+
+void setup() {
+  byte arduino_mac[] = { 0xAA, 0xBB, 0xCC, 0xEE, 0xDD, 0xFF };
+  IPAddress arduino_ip(192, 168, 1, 177);
+  IPAddress agent_ip(192, 168, 1, 113);
+  set_microros_native_ethernet_udp_transports(arduino_mac, arduino_ip, agent_ip, 9999);
+  
+  pinMode(LED_PIN, OUTPUT);
+  digitalWrite(LED_PIN, HIGH);
+
+  delay(2000);
+
+  allocator = rcl_get_default_allocator();
+
+  //create init_options
+  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
+
+  // create node
+  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_ethernet_node", "namespace", &support));
+
+  // create publisher
+  RCCHECK(rclc_publisher_init_best_effort(
+    &publisher,
+    &node,
+    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
+    "topic_name"));
+
+  msg.data = 0;
+}
+
+void loop() {
+    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
+    msg.data++;
+    delay(100);
+}
diff --git a/src/micro_ros_arduino.h b/src/micro_ros_arduino.h
index 189d9a0d6..71cc91f13 100755
--- a/src/micro_ros_arduino.h
+++ b/src/micro_ros_arduino.h
@@ -48,7 +48,11 @@ static inline void set_microros_transports(){
 #include <NativeEthernet.h>
 #endif
 
-#if defined(TARGET_STM32F4) || defined(ARDUINO_TEENSY41)
+#if defined(TARGET_PORTENTA_H7_M7) 
+#include <PortentaEthernet.h>
+#endif
+
+#if defined(TARGET_STM32F4) || defined(ARDUINO_TEENSY41)  || defined(TARGET_PORTENTA_H7_M7)
 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);
@@ -95,11 +99,12 @@ extern "C" bool arduino_wifi_transport_open(struct uxrCustomTransport * transpor
 extern "C" bool arduino_wifi_transport_close(struct uxrCustomTransport * transport);
 extern "C" size_t arduino_wifi_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
 extern "C" size_t arduino_wifi_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);
-
+#ifndef TARGET_PORTENTA_H7_M7
 struct micro_ros_agent_locator {
 	IPAddress address;
 	int port;
 };
+#endif
 
 static inline void set_microros_wifi_transports(char * ssid, char * pass, char * agent_ip, uint agent_port){
 
diff --git a/src/native_ethernet_transport.cpp b/src/native_ethernet_transport.cpp
index 462567e32..485b4f769 100755
--- a/src/native_ethernet_transport.cpp
+++ b/src/native_ethernet_transport.cpp
@@ -10,8 +10,13 @@
 #include <NativeEthernet.h>
 #include <micro_ros_arduino.h>
 #endif
+#ifdef TARGET_PORTENTA_H7_M7
+#include <Arduino.h>
+#include <EthernetUdp.h>
+#include <micro_ros_arduino.h>
+#endif
 
-#if defined(TARGET_STM32F4) || defined(ARDUINO_TEENSY41)
+#if defined(TARGET_STM32F4) || defined(ARDUINO_TEENSY41) || defined(TARGET_PORTENTA_H7_M7)
 extern "C" {
 
 #include <stdbool.h>