zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_HAL_SITL: use SITL::SerialDevice in place of pipe for communication

gps-1.3.1
Peter Barker 3 years ago committed by Peter Barker
parent
a0418eaab5
commit
9ebd0cdeed
6 changed files with 57 additions and 227 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 12
      libraries/AP_HAL_SITL/SITL_Periph_State.cpp
  2. 11
      libraries/AP_HAL_SITL/SITL_Periph_State.h
  3. 210
      libraries/AP_HAL_SITL/SITL_State.cpp
  4. 8
      libraries/AP_HAL_SITL/SITL_State.h
  5. 36
      libraries/AP_HAL_SITL/UARTDriver.cpp
  6. 7
      libraries/AP_HAL_SITL/UARTDriver.h

12
libraries/AP_HAL_SITL/SITL_Periph_State.cpp
Unescape View File

@ -59,14 +59,10 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) { @@ -59,14 +59,10 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) {
}
}
// when Periph can use SITL simulated devices we should remove these
// stubs:
ssize_t SITL::SerialDevice::read_from_device(char*, unsigned int) const { return -1; }
int SITL_State::sim_fd(const char *name, const char *arg) {
return 0;
}
int SITL_State::sim_fd_write(const char *name) {
return 0;
}
ssize_t SITL::SerialDevice::write_to_device(char const*, unsigned int) const { return -1; }
#endif //CONFIG_HAL_BOARD == HAL_BOARD_SITL && defined(HAL_BUILD_AP_PERIPH)

11
libraries/AP_HAL_SITL/SITL_Periph_State.h
Unescape View File

@ -15,6 +15,7 @@ @@ -15,6 +15,7 @@
#include <netinet/udp.h>
#include <arpa/inet.h>
#include <SITL/SITL.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_Compass/AP_Compass.h>
@ -30,12 +31,6 @@ class HALSITL::SITL_State { @@ -30,12 +31,6 @@ class HALSITL::SITL_State {
public:
void init(int argc, char * const argv[]);
// 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;
}
@ -67,6 +62,10 @@ public: @@ -67,6 +62,10 @@ public:
uint8_t get_instance() const { return _instance; }
SITL::SerialDevice *create_serial_sim(const char *name, const char *arg) {
return nullptr;
}
private:
void wait_clock(uint64_t wait_time_usec);

210
libraries/AP_HAL_SITL/SITL_State.cpp
Unescape View File

@ -212,182 +212,182 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) @@ -212,182 +212,182 @@ void SITL_State::wait_clock(uint64_t wait_time_usec)
}
#define streq(a, b) (!strcmp(a, b))
int SITL_State::sim_fd(const char *name, const char *arg)
SITL::SerialDevice *SITL_State::create_serial_sim(const char *name, const char *arg)
{
if (streq(name, "vicon")) {
if (vicon != nullptr) {
AP_HAL::panic("Only one vicon system at a time");
}
vicon = new SITL::Vicon();
return vicon->fd();
return vicon;
} else if (streq(name, "benewake_tf02")) {
if (benewake_tf02 != nullptr) {
AP_HAL::panic("Only one benewake_tf02 at a time");
}
benewake_tf02 = new SITL::RF_Benewake_TF02();
return benewake_tf02->fd();
return benewake_tf02;
} else if (streq(name, "benewake_tf03")) {
if (benewake_tf03 != nullptr) {
AP_HAL::panic("Only one benewake_tf03 at a time");
}
benewake_tf03 = new SITL::RF_Benewake_TF03();
return benewake_tf03->fd();
return benewake_tf03;
} else if (streq(name, "benewake_tfmini")) {
if (benewake_tfmini != nullptr) {
AP_HAL::panic("Only one benewake_tfmini at a time");
}
benewake_tfmini = new SITL::RF_Benewake_TFmini();
return benewake_tfmini->fd();
return benewake_tfmini;
} else if (streq(name, "lightwareserial")) {
if (lightwareserial != nullptr) {
AP_HAL::panic("Only one lightwareserial at a time");
}
lightwareserial = new SITL::RF_LightWareSerial();
return lightwareserial->fd();
return lightwareserial;
} else if (streq(name, "lightwareserial-binary")) {
if (lightwareserial_binary != nullptr) {
AP_HAL::panic("Only one lightwareserial-binary at a time");
}
lightwareserial_binary = new SITL::RF_LightWareSerialBinary();
return lightwareserial_binary->fd();
return lightwareserial_binary;
} else if (streq(name, "lanbao")) {
if (lanbao != nullptr) {
AP_HAL::panic("Only one lanbao at a time");
}
lanbao = new SITL::RF_Lanbao();
return lanbao->fd();
return lanbao;
} else if (streq(name, "blping")) {
if (blping != nullptr) {
AP_HAL::panic("Only one blping at a time");
}
blping = new SITL::RF_BLping();
return blping->fd();
return blping;
} else if (streq(name, "leddarone")) {
if (leddarone != nullptr) {
AP_HAL::panic("Only one leddarone at a time");
}
leddarone = new SITL::RF_LeddarOne();
return leddarone->fd();
return leddarone;
} else if (streq(name, "USD1_v0")) {
if (USD1_v0 != nullptr) {
AP_HAL::panic("Only one USD1_v0 at a time");
}
USD1_v0 = new SITL::RF_USD1_v0();
return USD1_v0->fd();
return USD1_v0;
} else if (streq(name, "USD1_v1")) {
if (USD1_v1 != nullptr) {
AP_HAL::panic("Only one USD1_v1 at a time");
}
USD1_v1 = new SITL::RF_USD1_v1();
return USD1_v1->fd();
return USD1_v1;
} else if (streq(name, "maxsonarseriallv")) {
if (maxsonarseriallv != nullptr) {
AP_HAL::panic("Only one maxsonarseriallv at a time");
}
maxsonarseriallv = new SITL::RF_MaxsonarSerialLV();
return maxsonarseriallv->fd();
return maxsonarseriallv;
} else if (streq(name, "wasp")) {
if (wasp != nullptr) {
AP_HAL::panic("Only one wasp at a time");
}
wasp = new SITL::RF_Wasp();
return wasp->fd();
return wasp;
} else if (streq(name, "nmea")) {
if (nmea != nullptr) {
AP_HAL::panic("Only one nmea at a time");
}
nmea = new SITL::RF_NMEA();
return nmea->fd();
return nmea;
} else if (streq(name, "rf_mavlink")) {
if (rf_mavlink != nullptr) {
AP_HAL::panic("Only one rf_mavlink at a time");
}
rf_mavlink = new SITL::RF_MAVLink();
return rf_mavlink->fd();
return rf_mavlink;
} else if (streq(name, "frsky-d")) {
if (frsky_d != nullptr) {
AP_HAL::panic("Only one frsky_d at a time");
}
frsky_d = new SITL::Frsky_D();
return frsky_d->fd();
return frsky_d;
// } else if (streq(name, "frsky-SPort")) {
// if (frsky_sport != nullptr) {
// AP_HAL::panic("Only one frsky_sport at a time");
// }
// frsky_sport = new SITL::Frsky_SPort();
// return frsky_sport->fd();
// return frsky_sport;
// } else if (streq(name, "frsky-SPortPassthrough")) {
// if (frsky_sport_passthrough != nullptr) {
// AP_HAL::panic("Only one frsky_sport passthrough at a time");
// }
// frsky_sport = new SITL::Frsky_SPortPassthrough();
// return frsky_sportpassthrough->fd();
// return frsky_sportpassthrough;
} else if (streq(name, "crsf")) {
if (crsf != nullptr) {
AP_HAL::panic("Only one crsf at a time");
}
crsf = new SITL::CRSF();
return crsf->fd();
return crsf;
} else if (streq(name, "rplidara2")) {
if (rplidara2 != nullptr) {
AP_HAL::panic("Only one rplidara2 at a time");
}
rplidara2 = new SITL::PS_RPLidarA2();
return rplidara2->fd();
return rplidara2;
} else if (streq(name, "terarangertower")) {
if (terarangertower != nullptr) {
AP_HAL::panic("Only one terarangertower at a time");
}
terarangertower = new SITL::PS_TeraRangerTower();
return terarangertower->fd();
return terarangertower;
} else if (streq(name, "sf45b")) {
if (sf45b != nullptr) {
AP_HAL::panic("Only one sf45b at a time");
}
sf45b = new SITL::PS_LightWare_SF45B();
return sf45b->fd();
return sf45b;
} else if (streq(name, "richenpower")) {
sitl_model->set_richenpower(&_sitl->richenpower_sim);
return _sitl->richenpower_sim.fd();
return &_sitl->richenpower_sim;
} else if (streq(name, "fetteconewireesc")) {
sitl_model->set_fetteconewireesc(&_sitl->fetteconewireesc_sim);
return _sitl->fetteconewireesc_sim.fd();
return &_sitl->fetteconewireesc_sim;
} else if (streq(name, "ie24")) {
sitl_model->set_ie24(&_sitl->ie24_sim);
return _sitl->ie24_sim.fd();
return &_sitl->ie24_sim;
} else if (streq(name, "gyus42v2")) {
if (gyus42v2 != nullptr) {
AP_HAL::panic("Only one gyus42v2 at a time");
}
gyus42v2 = new SITL::RF_GYUS42v2();
return gyus42v2->fd();
return gyus42v2;
} else if (streq(name, "megasquirt")) {
if (efi_ms != nullptr) {
AP_HAL::panic("Only one megasquirt at a time");
}
efi_ms = new SITL::EFI_MegaSquirt();
return efi_ms->fd();
return efi_ms;
} else if (streq(name, "VectorNav")) {
if (vectornav != nullptr) {
AP_HAL::panic("Only one VectorNav at a time");
}
vectornav = new SITL::VectorNav();
return vectornav->fd();
return vectornav;
} else if (streq(name, "LORD")) {
if (lord != nullptr) {
AP_HAL::panic("Only one LORD at a time");
}
lord = new SITL::LORD();
return lord->fd();
return lord;
} else if (streq(name, "AIS")) {
if (ais != nullptr) {
AP_HAL::panic("Only one AIS at a time");
}
ais = new SITL::AIS();
return ais->fd();
return ais;
} else if (strncmp(name, "gps", 3) == 0) {
const char *p = strchr(name, ':');
if (p == nullptr) {
@ -398,157 +398,11 @@ int SITL_State::sim_fd(const char *name, const char *arg) @@ -398,157 +398,11 @@ int SITL_State::sim_fd(const char *name, const char *arg)
AP_HAL::panic("Bad GPS number %u", x);
}
gps[x-1] = new SITL::GPS(x-1);
return gps[x-1]->fd();
return gps[x-1];
}
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();
} else if (streq(name, "benewake_tf02")) {
if (benewake_tf02 == nullptr) {
AP_HAL::panic("No benewake_tf02 created");
}
return benewake_tf02->write_fd();
} else if (streq(name, "benewake_tf03")) {
if (benewake_tf03 == nullptr) {
AP_HAL::panic("No benewake_tf03 created");
}
return benewake_tf03->write_fd();
} else if (streq(name, "benewake_tfmini")) {
if (benewake_tfmini == nullptr) {
AP_HAL::panic("No benewake_tfmini created");
}
return benewake_tfmini->write_fd();
} else if (streq(name, "lightwareserial")) {
if (lightwareserial == nullptr) {
AP_HAL::panic("No lightwareserial created");
}
return lightwareserial->write_fd();
} else if (streq(name, "lightwareserial-binary")) {
if (lightwareserial_binary == nullptr) {
AP_HAL::panic("No lightwareserial_binary created");
}
return lightwareserial_binary->write_fd();
} else if (streq(name, "lanbao")) {
if (lanbao == nullptr) {
AP_HAL::panic("No lanbao created");
}
return lanbao->write_fd();
} else if (streq(name, "blping")) {
if (blping == nullptr) {
AP_HAL::panic("No blping created");
}
return blping->write_fd();
} else if (streq(name, "leddarone")) {
if (leddarone == nullptr) {
AP_HAL::panic("No leddarone created");
}
return leddarone->write_fd();
} else if (streq(name, "USD1_v0")) {
if (USD1_v0 == nullptr) {
AP_HAL::panic("No USD1_v0 created");
}
return USD1_v0->write_fd();
} else if (streq(name, "USD1_v1")) {
if (USD1_v1 == nullptr) {
AP_HAL::panic("No USD1_v1 created");
}
return USD1_v1->write_fd();
} else if (streq(name, "maxsonarseriallv")) {
if (maxsonarseriallv == nullptr) {
AP_HAL::panic("No maxsonarseriallv created");
}
return maxsonarseriallv->write_fd();
} else if (streq(name, "wasp")) {
if (wasp == nullptr) {
AP_HAL::panic("No wasp created");
}
return wasp->write_fd();
} else if (streq(name, "nmea")) {
if (nmea == nullptr) {
AP_HAL::panic("No nmea created");
}
return nmea->write_fd();
} else if (streq(name, "rf_mavlink")) {
if (rf_mavlink == nullptr) {
AP_HAL::panic("No rf_mavlink created");
}
return rf_mavlink->write_fd();
} else if (streq(name, "frsky-d")) {
if (frsky_d == nullptr) {
AP_HAL::panic("No frsky-d created");
}
return frsky_d->write_fd();
} else if (streq(name, "crsf")) {
if (crsf == nullptr) {
AP_HAL::panic("No crsf created");
}
return crsf->write_fd();
} else if (streq(name, "rplidara2")) {
if (rplidara2 == nullptr) {
AP_HAL::panic("No rplidara2 created");
}
return rplidara2->write_fd();
} else if (streq(name, "terarangertower")) {
if (terarangertower == nullptr) {
AP_HAL::panic("No terarangertower created");
}
return terarangertower->write_fd();
} else if (streq(name, "sf45b")) {
if (sf45b == nullptr) {
AP_HAL::panic("No sf45b created");
}
return sf45b->write_fd();
} else if (streq(name, "richenpower")) {
return _sitl->richenpower_sim.write_fd();
} else if (streq(name, "fetteconewireesc")) {
return _sitl->fetteconewireesc_sim.write_fd();
} else if (streq(name, "ie24")) {
return _sitl->ie24_sim.write_fd();
} else if (streq(name, "gyus42v2")) {
if (gyus42v2 == nullptr) {
AP_HAL::panic("No gyus42v2 created");
}
return gyus42v2->write_fd();
} else if (streq(name, "megasquirt")) {
if (efi_ms == nullptr) {
AP_HAL::panic("No megasquirt created");
}
return efi_ms->write_fd();
} else if (streq(name, "VectorNav")) {
if (vectornav == nullptr) {
AP_HAL::panic("No VectorNav created");
}
return vectornav->write_fd();
} else if (streq(name, "LORD")) {
if (lord == nullptr) {
AP_HAL::panic("No LORD created");
}
return lord->write_fd();
} else if (streq(name, "AIS")) {
if (ais == nullptr) {
AP_HAL::panic("No AIS created");
}
return ais->write_fd();
} else if (strncmp(name, "gps", 3) == 0) {
const char *p = strchr(name, ':');
if (p == nullptr) {
AP_HAL::panic("Need a GPS number (e.g. sim:gps:1)");
}
const uint8_t x = atoi(p+1);
if (x <= 0 || x > ARRAY_SIZE(gps)) {
AP_HAL::panic("Bad GPS number %u", x);
}
return gps[x-1]->write_fd();
}
AP_HAL::panic("unknown simulated device: %s", name);
}
/*
check for a SITL RC input packet

8
libraries/AP_HAL_SITL/SITL_State.h
Unescape View File

@ -86,11 +86,9 @@ public: @@ -86,11 +86,9 @@ public:
return _base_port;
}
// 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);
// create a simulated serial device; type of device is given by
// name parameter
SITL::SerialDevice *create_serial_sim(const char *name, const char *arg);
bool use_rtscts(void) const {
return _use_rtscts;

36
libraries/AP_HAL_SITL/UARTDriver.cpp
Unescape View File

@ -66,13 +66,11 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) @@ -66,13 +66,11 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
if (strcmp(path, "GPS1") == 0) {
/* gps */
_connected = true;
_fd = _sitlState->sim_fd("gps:1", "");
_fd_write = _sitlState->sim_fd_write("gps:1");
_sim_serial_device = _sitlState->create_serial_sim("gps:1", "");
} else if (strcmp(path, "GPS2") == 0) {
/* 2nd gps */
_connected = true;
_fd = _sitlState->sim_fd("gps:2", "");
_fd_write = _sitlState->sim_fd_write("gps:2");
_sim_serial_device = _sitlState->create_serial_sim("gps:2", "");
} else {
/* parse type:args:flags string for path.
For example:
@ -122,8 +120,7 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) @@ -122,8 +120,7 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
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);
_sim_serial_device = _sitlState->create_serial_sim(args1, args2);
}
} else if (strcmp(devtype, "udpclient") == 0) {
// udp client connection
@ -248,17 +245,8 @@ size_t UARTDriver::write(const uint8_t *buffer, size_t size) @@ -248,17 +245,8 @@ size_t UARTDriver::write(const uint8_t *buffer, size_t size)
return 0;
}
if (_unbuffered_writes) {
// write buffer straight to the file descriptor
int fd = _fd_write;
if (fd == -1) {
fd = _fd;
}
const ssize_t nwritten = ::write(fd, buffer, size);
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;
@ -723,17 +711,11 @@ void UARTDriver::_timer_tick(void) @@ -723,17 +711,11 @@ void UARTDriver::_timer_tick(void)
const uint8_t *readptr = _writebuffer.readptr(navail);
if (readptr && navail > 0) {
navail = MIN(navail, max_bytes);
if (!_use_send_recv) {
int fd = _fd_write;
if (fd == -1) {
fd = _fd;
}
nwritten = ::write(fd, readptr, navail);
if (_sim_serial_device != nullptr) {
nwritten = _sim_serial_device->write_to_device((const char*)readptr, navail);
} else if (!_use_send_recv) {
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;
@ -778,6 +760,8 @@ void UARTDriver::_timer_tick(void) @@ -778,6 +760,8 @@ void UARTDriver::_timer_tick(void)
nread = 0;
}
}
} else if (_sim_serial_device != nullptr) {
nread = _sim_serial_device->read_from_device(buf, space);
} else if (!_use_send_recv) {
if (!_select_check(_fd)) {
return;

7
libraries/AP_HAL_SITL/UARTDriver.h
Unescape View File

@ -9,6 +9,8 @@ @@ -9,6 +9,8 @@
#include <AP_HAL/utility/Socket.h>
#include <AP_HAL/utility/RingBuffer.h>
#include <SITL/SIM_SerialDevice.h>
class HALSITL::UARTDriver : public AP_HAL::UARTDriver {
public:
friend class HALSITL::SITL_State;
@ -127,10 +129,7 @@ private: @@ -127,10 +129,7 @@ private:
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;
SITL::SerialDevice *_sim_serial_device;
};
#endif

Write Preview
Loading…
Cancel
Save