From 270831d70d4d52c1452d6f79c3044bf5abe2f55e Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sat, 28 Sep 2019 21:33:39 +1000
Subject: [PATCH] SITL: add bidirectional communication for simulated serial
 devices

---
 libraries/SITL/SIM_SerialDevice.cpp | 50 +++++++++++++++++++++++++++++
 libraries/SITL/SIM_SerialDevice.h   | 10 +++++-
 2 files changed, 59 insertions(+), 1 deletion(-)

diff --git a/libraries/SITL/SIM_SerialDevice.cpp b/libraries/SITL/SIM_SerialDevice.cpp
index e2345b84ce..6ba0f0607f 100644
--- a/libraries/SITL/SIM_SerialDevice.cpp
+++ b/libraries/SITL/SIM_SerialDevice.cpp
@@ -25,6 +25,7 @@ using namespace SITL;
 
 SerialDevice::SerialDevice()
 {
+    // pipe for device to write to:
     int tmp[2];
     if (pipe(tmp) == -1) {
         AP_HAL::panic("pipe() failed");
@@ -39,6 +40,23 @@ SerialDevice::SerialDevice()
     // 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);
+
 }
 
 bool SerialDevice::init_sitl_pointer()
@@ -52,3 +70,35 @@ bool SerialDevice::init_sitl_pointer()
     return true;
 }
 
+
+ssize_t SerialDevice::read_from_autopilot(char *buffer, const size_t size)
+{
+    const ssize_t ret = ::read(read_fd_my_end, buffer, size);
+    // if (ret > 0) {
+    //     ::fprintf(stderr, "SIM_SerialDevice: read from autopilot: (");
+    //     for (ssize_t i=0; i<ret; i++) {
+    //         ::fprintf(stderr, "%02X", buffer[i]);
+    //     }
+    //     ::fprintf(stderr, " ");
+    //     for (ssize_t i=0; i<ret; i++) {
+    //         ::fprintf(stderr, "%c", buffer[i]);
+    //     }
+    //     ::fprintf(stderr, ")\n");
+    // }
+    return ret;
+}
+
+ssize_t SerialDevice::write_to_autopilot(const char *buffer, const size_t size)
+{
+    const ssize_t ret = write(fd_my_end, buffer, size);
+    // ::fprintf(stderr, "write to autopilot: (");
+    // for (ssize_t i=0; i<ret; i++) {
+    //     ::fprintf(stderr, "%02X", (uint8_t)buffer[i]);
+    // }
+    // ::fprintf(stderr, ") (\n");
+    // for (ssize_t i=0; i<ret; i++) {
+    //     ::fprintf(stderr, "%c", (uint8_t)buffer[i]);
+    // }
+    // ::fprintf(stderr, ")\n");
+    return ret;
+}
diff --git a/libraries/SITL/SIM_SerialDevice.h b/libraries/SITL/SIM_SerialDevice.h
index 1be2933732..ed166d6566 100644
--- a/libraries/SITL/SIM_SerialDevice.h
+++ b/libraries/SITL/SIM_SerialDevice.h
@@ -29,9 +29,14 @@ public:
 
     SerialDevice();
 
-    // return fd on which data from the device can be read, and data
+    // return fd on which data from the device can be read
     // to the device can be written
     int fd() { return fd_their_end; }
+    // return fd on which data to the device can be written
+    int write_fd() { return read_fd_their_end; }
+
+    ssize_t read_from_autopilot(char *buffer, size_t size);
+    ssize_t write_to_autopilot(const char *buffer, size_t size);
 
 protected:
 
@@ -40,6 +45,9 @@ protected:
     int fd_their_end;
     int fd_my_end;
 
+    int read_fd_their_end;
+    int read_fd_my_end;
+
     bool init_sitl_pointer();
 };