From 13bd2379cfe1d34ff92b707b072a045ef2fbe384 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 26 Oct 2021 23:15:41 +1100
Subject: [PATCH] SITL: use SITL::SerialDevice in place of pipe for
 communication

---
 libraries/SITL/SIM_SerialDevice.cpp | 50 +++++++++--------------------
 libraries/SITL/SIM_SerialDevice.h   | 18 +++++------
 libraries/SITL/SIM_Vicon.cpp        |  2 +-
 3 files changed, 24 insertions(+), 46 deletions(-)

diff --git a/libraries/SITL/SIM_SerialDevice.cpp b/libraries/SITL/SIM_SerialDevice.cpp
index 7ff1058ee0..cf442083dc 100644
--- a/libraries/SITL/SIM_SerialDevice.cpp
+++ b/libraries/SITL/SIM_SerialDevice.cpp
@@ -29,38 +29,8 @@ using namespace SITL;
 
 SerialDevice::SerialDevice()
 {
-    // pipe for device to write to:
-    int tmp[2];
-    if (pipe(tmp) == -1) {
-        AP_HAL::panic("pipe() failed");
-    }
-    fd_my_end    = tmp[1];
-    fd_their_end = tmp[0];
-
-    // close file descriptors on exec:
-    fcntl(fd_my_end, F_SETFD, FD_CLOEXEC);
-    fcntl(fd_their_end, F_SETFD, FD_CLOEXEC);
-
-    // make sure we don't screw the simulation up by blocking:
-    fcntl(fd_my_end, F_SETFL, fcntl(fd_my_end, F_GETFL, 0) | O_NONBLOCK);
-    fcntl(fd_their_end, F_SETFL, fcntl(fd_their_end, F_GETFL, 0) | O_NONBLOCK);
-
-
-    // pipe for device to read from:
-    if (pipe(tmp) == -1) {
-        AP_HAL::panic("pipe() failed");
-    }
-    read_fd_my_end    = tmp[0];
-    read_fd_their_end = tmp[1];
-
-    // close file descriptors on exec:
-    fcntl(read_fd_my_end, F_SETFD, FD_CLOEXEC);
-    fcntl(read_fd_their_end, F_SETFD, FD_CLOEXEC);
-
-    // make sure we don't screw the simulation up by blocking:
-    fcntl(read_fd_my_end, F_SETFL, fcntl(fd_my_end, F_GETFL, 0) | O_NONBLOCK);
-    fcntl(read_fd_their_end, F_SETFL, fcntl(fd_their_end, F_GETFL, 0) | O_NONBLOCK);
-
+    to_autopilot = new ByteBuffer{512};
+    from_autopilot = new ByteBuffer{512};
 }
 
 bool SerialDevice::init_sitl_pointer()
@@ -74,10 +44,9 @@ bool SerialDevice::init_sitl_pointer()
     return true;
 }
 
-
 ssize_t SerialDevice::read_from_autopilot(char *buffer, const size_t size) const
 {
-    const ssize_t ret = ::read(read_fd_my_end, buffer, size);
+    const ssize_t ret = from_autopilot->read((uint8_t*)buffer, size);
     // if (ret > 0) {
     //     ::fprintf(stderr, "SIM_SerialDevice: read from autopilot (%u): (", (unsigned)ret);
     //     for (ssize_t i=0; i<ret; i++) {
@@ -95,7 +64,7 @@ ssize_t SerialDevice::read_from_autopilot(char *buffer, const size_t size) const
 
 ssize_t SerialDevice::write_to_autopilot(const char *buffer, const size_t size) const
 {
-    const ssize_t ret = write(fd_my_end, buffer, size);
+    const ssize_t ret = to_autopilot->write((uint8_t*)buffer, size);
     // ::fprintf(stderr, "write to autopilot: (");
     // for (ssize_t i=0; i<ret; i++) {
     //     ::fprintf(stderr, "%02X", (uint8_t)buffer[i]);
@@ -107,3 +76,14 @@ ssize_t SerialDevice::write_to_autopilot(const char *buffer, const size_t size)
     // ::fprintf(stderr, ")\n");
     return ret;
 }
+
+ssize_t SerialDevice::read_from_device(char *buffer, const size_t size) const
+{
+    const ssize_t ret = to_autopilot->read((uint8_t*)buffer, size);
+    return ret;
+}
+ssize_t SerialDevice::write_to_device(const char *buffer, const size_t size) const
+{
+    const ssize_t ret = from_autopilot->write((uint8_t*)buffer, size);
+    return ret;
+}
diff --git a/libraries/SITL/SIM_SerialDevice.h b/libraries/SITL/SIM_SerialDevice.h
index 94f23629a9..cdfe5637f2 100644
--- a/libraries/SITL/SIM_SerialDevice.h
+++ b/libraries/SITL/SIM_SerialDevice.h
@@ -19,6 +19,7 @@
 #pragma once
 
 #include <unistd.h>
+#include <AP_HAL/utility/RingBuffer.h>
 
 namespace SITL {
 
@@ -27,12 +28,12 @@ public:
 
     SerialDevice();
 
-    // return fd on which data from the device can be read
-    // to the device can be written
-    int fd() const { return fd_their_end; }
-    // return fd on which data to the device can be written
-    int write_fd() const { return read_fd_their_end; }
 
+    // methods for autopilot to use to talk to device:
+    ssize_t read_from_device(char *buffer, size_t size) const;
+    ssize_t write_to_device(const char *buffer, size_t size) const;
+
+    // methods for simulated device to use:
     ssize_t read_from_autopilot(char *buffer, size_t size) const;
     virtual ssize_t write_to_autopilot(const char *buffer, size_t size) const;
 
@@ -40,11 +41,8 @@ protected:
 
     class SIM *_sitl;
 
-    int fd_their_end;
-    int fd_my_end;
-
-    int read_fd_their_end;
-    int read_fd_my_end;
+    ByteBuffer *to_autopilot;
+    ByteBuffer *from_autopilot;
 
     bool init_sitl_pointer();
 };
diff --git a/libraries/SITL/SIM_Vicon.cpp b/libraries/SITL/SIM_Vicon.cpp
index 07c24f4592..6b4251d73a 100644
--- a/libraries/SITL/SIM_Vicon.cpp
+++ b/libraries/SITL/SIM_Vicon.cpp
@@ -90,7 +90,7 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
             uint8_t buf[300];
             uint16_t buf_len = mavlink_msg_to_send_buffer(buf, &msg_buf[i].obs_msg);
 
-            if (::write(fd_my_end, (void*)&buf, buf_len) != buf_len) {
+            if (write_to_autopilot((char*)&buf, buf_len) != buf_len) {
                 hal.console->printf("Vicon: write failure\n");
             }
             msg_buf[i].time_send_us = 0;