Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_SITL: add support for bi-directional simulated serial devices

Browse Source
This commit is contained in:
Peter Barker 2019-10-29 13:52:19 +11:00 committed by Peter Barker
parent 270831d70d
commit 9da0e480f2
4 changed files with 49 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -224,6 +224,17 @@ int SITL_State::sim_fd(const char *name, const char *arg)
vicon = new SITL::Vicon();
return vicon->fd();
}
AP_HAL::panic("unknown simulated device: %s", name);
}
int SITL_State::sim_fd_write(const char *name)
{
if (streq(name, "vicon")) {
if (vicon == nullptr) {
AP_HAL::panic("No vicon created");
}
return vicon->write_fd();
}
AP_HAL::panic("unknown simulated device: %s", name);
}

4
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -54,9 +54,11 @@ public:
return _base_port;
}
// create a file desciptor attached to a virtual device; type of
// create a file descriptor attached to a virtual device; type of
// device is given by name parameter
int sim_fd(const char *name, const char *arg);
// returns a write file descriptor for a created virtual device
int sim_fd_write(const char *name);
bool use_rtscts(void) const {
return _use_rtscts;

39
libraries/AP_HAL_SITL/UARTDriver.cpp
View File

@ -104,10 +104,11 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
_uart_baudrate = baudrate;
_uart_start_connection();
} else if (strcmp(devtype, "sim") == 0) {
::printf("SIM connection %s:%s on port %u\n", args1, args2, _portNumber);
if (!_connected) {
::printf("SIM connection %s:%s on port %u\n", args1, args2, _portNumber);
_connected = true;
_fd = _sitlState->sim_fd(args1, args2);
_fd_write = _sitlState->sim_fd_write(args1);
}
} else if (strcmp(devtype, "udpclient") == 0) {
// udp client connection
@ -177,15 +178,19 @@ void UARTDriver::flush(void)
{
}
// size_t UARTDriver::write(uint8_t c)
// {
// if (txspace() <= 0) {
// return 0;
// }
// _writebuffer.write(&c, 1);
// return 1;
// }
size_t UARTDriver::write(uint8_t c)
{
if (txspace() <= 0) {
return 0;
}
_writebuffer.write(&c, 1);
return 1;
return write(&c, 1);
}
size_t UARTDriver::write(const uint8_t *buffer, size_t size)
{
if (txspace() <= size) {
@ -196,8 +201,16 @@ size_t UARTDriver::write(const uint8_t *buffer, size_t size)
}
if (_unbuffered_writes) {
// write buffer straight to the file descriptor
const ssize_t nwritten = ::write(_fd, buffer, size);
int fd = _fd_write;
if (fd == -1) {
fd = _fd;
}
const ssize_t nwritten = ::write(fd, buffer, size);
if (nwritten == -1 && errno != EAGAIN && _uart_path) {
if (_fd_write != -1) {
close(_fd_write);
_fd_write = 1;
}
close(_fd);
_fd = -1;
_connected = false;
@ -650,8 +663,16 @@ void UARTDriver::_timer_tick(void)
if (readptr && navail > 0) {
navail = MIN(navail, max_bytes);
if (!_use_send_recv) {
nwritten = ::write(_fd, readptr, navail);
int fd = _fd_write;
if (fd == -1) {
fd = _fd;
}
nwritten = ::write(fd, readptr, navail);
if (nwritten == -1 && errno != EAGAIN && _uart_path) {
if (_fd_write != -1){
close(_fd_write);
_fd_write = -1;
}
close(_fd);
_fd = -1;
_connected = false;

5
libraries/AP_HAL_SITL/UARTDriver.h
View File

@ -120,6 +120,11 @@ private:
bool _packetise;
uint16_t _mc_myport;
uint32_t last_tick_us;
// if this is not -1 then data should be written here instead of
// _fd. This is to support simulated serial devices, which use a
// pipe for read and a pipe for write
int _fd_write = -1;
};
#endif