aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: add bidirectional communication for simulated serial devices

Browse Source
This commit is contained in:
Peter Barker 2019-09-28 21:33:39 +10:00 committed by Peter Barker
parent 48657dd2c9
commit 270831d70d
2 changed files with 59 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/SITL/SIM_SerialDevice.cpp
View File

@ -25,6 +25,7 @@ using namespace SITL;
SerialDevice::SerialDevice() SerialDevice::SerialDevice()
{ {
// pipe for device to write to:
int tmp[2]; int tmp[2];
if (pipe(tmp) == -1) { if (pipe(tmp) == -1) {
AP_HAL::panic("pipe() failed"); AP_HAL::panic("pipe() failed");
@ -39,6 +40,23 @@ SerialDevice::SerialDevice()
// make sure we don't screw the simulation up by blocking: // 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_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); 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() bool SerialDevice::init_sitl_pointer()
@ -52,3 +70,35 @@ bool SerialDevice::init_sitl_pointer()
return true; 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;
}

10
libraries/SITL/SIM_SerialDevice.h
View File

@ -29,9 +29,14 @@ public:
SerialDevice(); 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 // to the device can be written
int fd() { return fd_their_end; } 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: protected:
@ -40,6 +45,9 @@ protected:
int fd_their_end; int fd_their_end;
int fd_my_end; int fd_my_end;
int read_fd_their_end;
int read_fd_my_end;
bool init_sitl_pointer(); bool init_sitl_pointer();
}; };