AP_HAL_SITL: add support for multiple periph instances

libraries/AP_HAL_SITL/SITL_Periph_State.cpp

@@ -18,13 +18,38 @@
 #include <AP_Param/AP_Param.h>
 #include <SITL/SIM_JSBSim.h>
 #include <AP_HAL/utility/Socket.h>
+#include <AP_HAL/utility/getopt_cpp.h>
 
 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) {

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

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)

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

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);
         }

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;

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);