mirror of https://github.com/ArduPilot/ardupilot
SITL: use SITL::SerialDevice in place of pipe for communication
This commit is contained in:
parent
9ebd0cdeed
commit
13bd2379cf
|
@ -29,38 +29,8 @@ using namespace SITL;
|
||||||
|
|
||||||
SerialDevice::SerialDevice()
|
SerialDevice::SerialDevice()
|
||||||
{
|
{
|
||||||
// pipe for device to write to:
|
to_autopilot = new ByteBuffer{512};
|
||||||
int tmp[2];
|
from_autopilot = new ByteBuffer{512};
|
||||||
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);
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool SerialDevice::init_sitl_pointer()
|
bool SerialDevice::init_sitl_pointer()
|
||||||
|
@ -74,10 +44,9 @@ bool SerialDevice::init_sitl_pointer()
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
ssize_t SerialDevice::read_from_autopilot(char *buffer, const size_t size) const
|
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) {
|
// if (ret > 0) {
|
||||||
// ::fprintf(stderr, "SIM_SerialDevice: read from autopilot (%u): (", (unsigned)ret);
|
// ::fprintf(stderr, "SIM_SerialDevice: read from autopilot (%u): (", (unsigned)ret);
|
||||||
// for (ssize_t i=0; i<ret; i++) {
|
// 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
|
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: (");
|
// ::fprintf(stderr, "write to autopilot: (");
|
||||||
// for (ssize_t i=0; i<ret; i++) {
|
// for (ssize_t i=0; i<ret; i++) {
|
||||||
// ::fprintf(stderr, "%02X", (uint8_t)buffer[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");
|
// ::fprintf(stderr, ")\n");
|
||||||
return ret;
|
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;
|
||||||
|
}
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
|
#include <AP_HAL/utility/RingBuffer.h>
|
||||||
|
|
||||||
namespace SITL {
|
namespace SITL {
|
||||||
|
|
||||||
|
@ -27,12 +28,12 @@ public:
|
||||||
|
|
||||||
SerialDevice();
|
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;
|
ssize_t read_from_autopilot(char *buffer, size_t size) const;
|
||||||
virtual ssize_t write_to_autopilot(const 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;
|
class SIM *_sitl;
|
||||||
|
|
||||||
int fd_their_end;
|
ByteBuffer *to_autopilot;
|
||||||
int fd_my_end;
|
ByteBuffer *from_autopilot;
|
||||||
|
|
||||||
int read_fd_their_end;
|
|
||||||
int read_fd_my_end;
|
|
||||||
|
|
||||||
bool init_sitl_pointer();
|
bool init_sitl_pointer();
|
||||||
};
|
};
|
||||||
|
|
|
@ -90,7 +90,7 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
|
||||||
uint8_t buf[300];
|
uint8_t buf[300];
|
||||||
uint16_t buf_len = mavlink_msg_to_send_buffer(buf, &msg_buf[i].obs_msg);
|
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");
|
hal.console->printf("Vicon: write failure\n");
|
||||||
}
|
}
|
||||||
msg_buf[i].time_send_us = 0;
|
msg_buf[i].time_send_us = 0;
|
||||||
|
|
Loading…
Reference in New Issue