Refactor UDP transports (#739) (#742)

Signed-off-by: Pablo Garrido <[email protected]>
(cherry picked from commit 6fe2fd4)

Co-authored-by: Pablo Garrido <[email protected]>

Author: mergify[bot]

src/native_ethernet_transport.cpp

@@ -24,8 +24,7 @@ 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;
+  return 1 == udp_client.begin(locator->port);
 }
 
 bool arduino_native_ethernet_udp_transport_close(
@@ -41,9 +40,12 @@ size_t arduino_native_ethernet_udp_transport_write(
   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();
+  size_t sent = 0;
+  if(1 == udp_client.beginPacket(locator->address, locator->port)){
+    sent = udp_client.write(buf, len);
+    sent = 1 == udp_client.endPacket() ? sent : 0;
+  }
+
   udp_client.flush();
 
   return sent;
@@ -53,33 +55,18 @@ 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) {
+  int64_t start_time = uxr_millis();
+
+  while ((uxr_millis() - start_time) < ((int64_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;
+  size_t available = 0;
+  if(udp_client.available()){
+    available = udp_client.read(buf, len);
+  }
+  return (available < 0) ? 0 : available;
 }
 }