micro_ros_arduino/src/native_ethernet_transport.cpp at d4ffdbd304cfa6b4c34f34a9dbc6999a73b33333 · micro-ROS/micro_ros_arduino · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
#if defined(TARGET_STM32F4)

#include <Arduino.h>
#include <EthernetUdp.h>
#include <STM32Ethernet.h>
#include <micro_ros_arduino.h>
#endif

#ifdef ARDUINO_TEENSY41
#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(ARDUINO_ARCH_MBED_GIGA)
#include <Arduino.h>
#include <EthernetUdp.h>
#include <Ethernet.h>
#include <micro_ros_arduino.h>

#endif

#if defined(TARGET_STM32F4) || defined(ARDUINO_TEENSY41) || defined(TARGET_PORTENTA_H7_M7)|| defined(ARDUINO_ARCH_MBED_GIGA)
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;
  return 1 == udp_client.begin(locator->port);
}

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;

  size_t sent = 0;
  if(1 == udp_client.beginPacket(locator->address, locator->port)){
    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;

  int64_t start_time = uxr_millis();

  while ((uxr_millis() - start_time) < ((int64_t)timeout) && udp_client.parsePacket() == 0) {
    delay(1);
  }

  size_t available = 0;
  if(udp_client.available()){
    available = udp_client.read(buf, len);
  }
  return (available < 0) ? 0 : available;
}
}

#endif