From 9ebd0cdeede1f89684ec9181ff8e650a09058ef1 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 26 Oct 2021 23:31:22 +1100 Subject: [PATCH] AP_HAL_SITL: use SITL::SerialDevice in place of pipe for communication --- libraries/AP_HAL_SITL/SITL_Periph_State.cpp | 12 +- libraries/AP_HAL_SITL/SITL_Periph_State.h | 11 +- libraries/AP_HAL_SITL/SITL_State.cpp | 210 +++----------------- libraries/AP_HAL_SITL/SITL_State.h | 8 +- libraries/AP_HAL_SITL/UARTDriver.cpp | 36 +--- libraries/AP_HAL_SITL/UARTDriver.h | 7 +- 6 files changed, 57 insertions(+), 227 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp index 67c9bc3bfa..521bf17b00 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp @@ -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) diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.h b/libraries/AP_HAL_SITL/SITL_Periph_State.h index 1549f79ac0..6f89a13456 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.h +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.h @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -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: 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); diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index a171361f58..e7ec21e1b4 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -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) 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 diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 8a2ab1ba4b..0eb5e7bb1f 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -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; diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 30f14ee5b9..2cf9cfb0b7 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -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) 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) 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) 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) 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; diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index 6ed8a88a48..392e9f1b0d 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -9,6 +9,8 @@ #include #include +#include + class HALSITL::UARTDriver : public AP_HAL::UARTDriver { public: friend class HALSITL::SITL_State; @@ -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