From 49d934eb42f9300060e249e00614d2a3f0f3204b Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Mon, 28 Dec 2020 20:03:56 +0530 Subject: [PATCH] AP_HAL_SITL: add support for multiple periph instances --- libraries/AP_HAL_SITL/SITL_Periph_State.cpp | 25 +++++++++++++++++++++ libraries/AP_HAL_SITL/SITL_Periph_State.h | 19 ++++++++++------ libraries/AP_HAL_SITL/SITL_State.h | 3 +++ libraries/AP_HAL_SITL/Storage.cpp | 2 ++ libraries/AP_HAL_SITL/UARTDriver.cpp | 6 +++++ libraries/AP_HAL_SITL/Util.cpp | 6 ++++- libraries/AP_HAL_SITL/sitl_gps.cpp | 12 +++++++--- 7 files changed, 62 insertions(+), 11 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp index 62ab0d6485..711c26e182 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp @@ -18,13 +18,38 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; using namespace HALSITL; void SITL_State::init(int argc, char * const argv[]) { + int opt; + const struct GetOptLong::option options[] = { + {"help", false, 0, 'h'}, + {"instance", true, 0, 'I'}, + }; + setvbuf(stdout, (char *)0, _IONBF, 0); + setvbuf(stderr, (char *)0, _IONBF, 0); + + GetOptLong gopt(argc, argv, "hI:", + options); + + while((opt = gopt.getoption()) != -1) { + switch (opt) { + case 'I': + _instance = atoi(gopt.optarg); + break; + default: + printf("Options:\n" + "\t--help|-h display this help information\n" + "\t--instance|-I N set instance of SITL Periph\n"); + exit(1); + } + } + printf("Running Instance: %d\n", _instance); } void SITL_State::wait_clock(uint64_t wait_time_usec) { diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.h b/libraries/AP_HAL_SITL/SITL_Periph_State.h index 2f4c37ef19..77626f21ea 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.h +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.h @@ -55,14 +55,17 @@ public: uint16_t current2_pin_value; // pin 14 // paths for UART devices const char *_uart_path[7] { - "tcp:5860", - "fifo:/tmp/ap_gps0", - "tcp:5861", - "tcp:5862", - "fifo:/tmp/ap_gps0", - "tcp:5863", - "tcp:5864", + "none:0", + "fifo:gps", + "none:1", + "none:2", + "none:3", + "none:4", + "none:5", }; + + uint8_t get_instance() const { return _instance; } + private: void wait_clock(uint64_t wait_time_usec); @@ -70,6 +73,8 @@ private: uint16_t _base_port; const char *defaults_path = HAL_PARAM_DEFAULTS_PATH; + + uint8_t _instance; }; #endif diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index d1be45927a..fa153061d6 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -124,6 +124,8 @@ public: Location &loc, float &yaw_degrees); + uint8_t get_instance() const { return _instance; } + private: void _parse_command_line(int argc, char * const argv[]); void _set_param_default(const char *parm); @@ -310,6 +312,7 @@ private: const char *defaults_path = HAL_PARAM_DEFAULTS_PATH; const char *_home_str; + char *_gps_fifo[2]; }; #endif // defined(HAL_BUILD_AP_PERIPH) diff --git a/libraries/AP_HAL_SITL/Storage.cpp b/libraries/AP_HAL_SITL/Storage.cpp index fcc744c5b1..3e971cdceb 100644 --- a/libraries/AP_HAL_SITL/Storage.cpp +++ b/libraries/AP_HAL_SITL/Storage.cpp @@ -13,6 +13,8 @@ #ifndef HAL_STORAGE_FILE #if APM_BUILD_TYPE(APM_BUILD_Replay) #define HAL_STORAGE_FILE "eeprom-replay.bin" +#elif APM_BUILD_TYPE(APM_BUILD_AP_Periph) +#define HAL_STORAGE_FILE "eeprom-periph.bin" #else #define HAL_STORAGE_FILE "eeprom.bin" #endif diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 2ca3c5a3eb..f4d38db8fa 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -104,6 +104,9 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) _uart_baudrate = baudrate; _uart_start_connection(); } else if (strcmp(devtype, "fifo") == 0) { + if(strcmp(args1, "gps") == 0) { + UNUSED_RESULT(asprintf(&args1, "/tmp/gps_fifo%d", (int)_sitlState->get_instance())); + } ::printf("Reading FIFO file @ %s\n", args1); _fd = ::open(args1, O_RDONLY | O_NONBLOCK); if (_fd >= 0) { @@ -134,6 +137,9 @@ void UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) ::printf("UDP multicast connection %s:%u\n", ip, port); _udp_start_multicast(ip, port); } + } else if (strcmp(devtype,"none") == 0) { + // skipping port + ::printf("Skipping port %s\n", args1); } else { AP_HAL::panic("Invalid device path: %s", path); } diff --git a/libraries/AP_HAL_SITL/Util.cpp b/libraries/AP_HAL_SITL/Util.cpp index 1717ad0d42..c1f9570dc7 100644 --- a/libraries/AP_HAL_SITL/Util.cpp +++ b/libraries/AP_HAL_SITL/Util.cpp @@ -51,6 +51,7 @@ bool HALSITL::Util::get_system_id_unformatted(uint8_t buf[], uint8_t &len) *p = 0; } len = strnlen(cbuf, len); + buf[0] += sitlState->get_instance(); return true; } @@ -58,7 +59,10 @@ bool HALSITL::Util::get_system_id_unformatted(uint8_t buf[], uint8_t &len) if (gethostname(cbuf, len) != 0) { // use a default name so this always succeeds. Without it we can't // implement some features (such as UAVCAN) - strncpy(cbuf, "sitl-unknown", len); + snprintf(cbuf, len, "sitl-unknown-%d", sitlState->get_instance()); + } else { + // To ensure separate ids for each instance + cbuf[0] += sitlState->get_instance(); } len = strnlen(cbuf, len); return true; diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 4043132379..4978a54db1 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -66,15 +66,17 @@ ssize_t SITL_State::gps_read(int fd, void *buf, size_t count) /* setup GPS input pipe */ -const char * gps_fifo[2] = {"/tmp/ap_gps0", "/tmp/ap_gps1"}; int SITL_State::gps_pipe(uint8_t idx) { int fd[2]; + if (_gps_fifo[idx] == nullptr) { + UNUSED_RESULT(asprintf(&_gps_fifo[idx], "/tmp/gps_fifo%d", (int)(ARRAY_SIZE(_gps_fifo)*_instance + idx))); + } if (gps_state[idx].client_fd != 0) { return gps_state[idx].client_fd; } pipe(fd); - if (mkfifo(gps_fifo[idx], 0666) < 0) { + if (mkfifo(_gps_fifo[idx], 0666) < 0) { printf("MKFIFO failed with %s\n", strerror(errno)); } @@ -96,8 +98,12 @@ void SITL_State::_gps_write(const uint8_t *p, uint16_t size, uint8_t instance) if (instance == 1 && _sitl->gps_disable[instance]) { return; } + if (_gps_fifo[instance] == nullptr) { + printf("GPS FIFO path not set\n"); + return; + } // also write to external fifo - int fd = open(gps_fifo[instance], O_WRONLY | O_NONBLOCK); + int fd = open(_gps_fifo[instance], O_WRONLY | O_NONBLOCK); if (fd >= 0) { write(fd, p, size); close(fd);