Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Peter Barker 2021-10-26 23:31:22 +11:00 committed by Peter Barker
parent a0418eaab5
commit 9ebd0cdeed
6 changed files with 57 additions and 227 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_HAL_SITL/SITL_Periph_State.cpp
View File

@ -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) { ssize_t SITL::SerialDevice::write_to_device(char const*, unsigned int) const { return -1; }
return 0;
}
int SITL_State::sim_fd_write(const char *name) {
return 0;
}
#endif //CONFIG_HAL_BOARD == HAL_BOARD_SITL && defined(HAL_BUILD_AP_PERIPH) #endif //CONFIG_HAL_BOARD == HAL_BOARD_SITL && defined(HAL_BUILD_AP_PERIPH)

11
libraries/AP_HAL_SITL/SITL_Periph_State.h
View File

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

210
libraries/AP_HAL_SITL/SITL_State.cpp
View File

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

8
libraries/AP_HAL_SITL/SITL_State.h
View File

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

36
libraries/AP_HAL_SITL/UARTDriver.cpp
View File

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

7
libraries/AP_HAL_SITL/UARTDriver.h
View File

@ -9,6 +9,8 @@
#include <AP_HAL/utility/Socket.h> #include <AP_HAL/utility/Socket.h>
#include <AP_HAL/utility/RingBuffer.h> #include <AP_HAL/utility/RingBuffer.h>
#include <SITL/SIM_SerialDevice.h>
class HALSITL::UARTDriver : public AP_HAL::UARTDriver { class HALSITL::UARTDriver : public AP_HAL::UARTDriver {
public: public:
friend class HALSITL::SITL_State; friend class HALSITL::SITL_State;
@ -127,10 +129,7 @@ private:
uint16_t _mc_myport; uint16_t _mc_myport;
uint32_t last_tick_us; uint32_t last_tick_us;
// if this is not -1 then data should be written here instead of SITL::SerialDevice *_sim_serial_device;
// _fd. This is to support simulated serial devices, which use a
// pipe for read and a pipe for write
int _fd_write = -1;
}; };
#endif #endif