6.1.0 sitl interop (#8913)

* fix getopts 'has_arg' usage (iaw getopt_long(3))

* Rexec the SITL on reboot

* Allow hostnames, facilitate compilation on non-GNU OS (e.g. *BSD), add IPV6 [xplane.c]

* add required interop header files [simple_soap_client.c]

* add required interop header files [simple_soap_client.h]

* update serial_tcp (headers, IPv6, lookup etc)

* conditional for  pthread_attr_getschedpolicy availability

* fix error in xplane socket familiy

* remove unnecessary added headers [xplane.c]

* fix gcc 12  warning is osd.c

* update docs

* fix for older gcc without closefrom(3)

* add AI_V4MAPPED|AI_ADDRCONFIG to ai_flags (to support V4 only hosts)

docs/SITL/SITL.md

@@ -33,6 +33,7 @@ Select "FAKE" as type for all mentioned, so that they receive the data from the
 UARTs are replaced by TCP starting with port 5760 ascending. UART 1 port 5760, UART2 6761, ...
 By default, UART1 and UART2 are available as MSP connections.
 To connect the Configurator to SITL: Select TCP and connect to ```127.0.0.1:5760``` (if SITL is running on the same machine).
+IPv4 and IPv6 are supported, either raw addresses of hostname lookup.
 
 The assignment and status of user UART/TCP connections is displayed on the console.
 
@@ -78,7 +79,7 @@ For normal use, please use the SITL tab in the configurator.
 
 The following SITL specific command line options are available:
 
-If SITL is started without command line options, only the Configurator can be used.
+If SITL is started without command line options, only a serial MSP / CLI connection can be used (e.g. Configurator or other application) can be used.
 
 ```--path``` Full path and file name to config file, if not present, eeprom.bin in the current directory is used. Example: ```C:\INAV_SITL\flying-wing.bin```
 
@@ -108,7 +109,7 @@ It is recommended to start the tools in the following order:
 
 ## Compile
 
-### Linux:
+### Linux and FreeBSD:
 Almost like normal, ruby, cmake and make are also required.
 With cmake, the option "-DSITL=ON" must be specified.
 
@@ -122,3 +123,24 @@ make
 ### Windows:
 Compile under cygwin, then as in Linux.
 Copy cygwin1.dll into the directory, or include cygwin's /bin/ directory in the environment variable PATH.
+
+#### Build manager
+
+`ninja` may also be used (parallel builds without `-j $(nproc)`):
+
+```
+cmake -GNinja -DSITL=ON ..
+ninja
+```
+
+### Compiler requirements
+
+* Modern GCC. Must be a *real* GCC, macOS faking it with clang will not work.
+* Unix sockets networking. Cygwin is required on Windows (vice `winsock`).
+* Pthreads
+
+## Supported environments
+
+* Linux on x86_64, Aarch64 (e.g. Rpi4), RISC-V (e.g. VisionFive2)
+* Windows on x86_64
+* FreeBSD (x86_64 at least).

docs/SITL/X-Plane.md

@@ -38,3 +38,7 @@ The assignment of the "virtual receiver" is fixed:
 The internal mixer (e.g. for flying wings) cannot be deactivated without further ado, therefore always select "Aircraft with tail" in INAV.
 For the standard Aircraft preset the channelmap is:
 ```--chanmap=M01-01,S01-03,S03-02,S04-04```
+
+## Other applications
+
+[fl2sitl](https://github.com/stronnag/bbl2kml/wiki/fl2sitl) is an open source application to replay an INAV Blackbox log through the INAV SITL via `blackbox_decode`. The output may be visualised in any MSP capable application, such as the INAV Configurator or [mwp](https://github.com/stronnag/mwptools). fl2sitl uses the X-plane protocol.

src/main/drivers/serial_tcp.c

@@ -33,6 +33,10 @@
 
 #include <sys/socket.h>
 #include <fcntl.h>
+#include <netinet/in.h>
+#include <netdb.h>
+#include <arpa/inet.h>
+#include <errno.h>
 
 #include "common/utils.h"
 
@@ -43,6 +47,68 @@ static const struct serialPortVTable tcpVTable[];
 static tcpPort_t tcpPorts[SERIAL_PORT_COUNT];
 static bool tcpThreadRunning = false;
 
+static int lookup_address (char *name, int port, int type, struct sockaddr *addr, socklen_t* len )
+{
+    struct addrinfo *servinfo, *p;
+    struct addrinfo hints = {.ai_family = AF_UNSPEC, .ai_socktype = type, .ai_flags = AI_V4MAPPED|AI_ADDRCONFIG};
+    if (name == NULL) {
+	hints.ai_flags |= AI_PASSIVE;
+    }
+    /*
+      This nonsense is to uniformly deliver the same sa_family regardless of whether
+      name is NULL or non-NULL
+      Otherwise, at least on Linux, we get
+      - V6,V4 for the non-null case and
+      - V4,V6 for the null case, regardless of gai.conf
+      Which may confuse consumers.
+      FreeBSD and Windows behave consistently, giving V6 for Ipv6 enabled stacks in all cases
+      unless a quad dotted address is specificied
+    */
+    struct addrinfo *p4 = NULL;
+    struct addrinfo *p6 = NULL;
+    int result;
+    char aport[16];
+    snprintf(aport, sizeof(aport), "%d", port);
+    if ((result = getaddrinfo(name, aport, &hints, &servinfo)) != 0) {
+        fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(result));
+        return result;
+    } else {
+	int j = 0;
+	for(p = servinfo; p != NULL; p = p->ai_next) {
+	    if(p->ai_family == AF_INET6)
+		p6 = p;
+	    else if(p->ai_family == AF_INET)
+        p4 = p;
+	    j++;
+	}
+	if (p6 != NULL)
+	    p = p6;
+	else if (p4 != NULL)
+	    p = p4;
+	else
+	    return -1;
+	memcpy(addr, p->ai_addr, p->ai_addrlen);
+	*len = p->ai_addrlen;
+	freeaddrinfo(servinfo);
+    }
+    return 0;
+}
+
+static char *tcpGetAddressString(struct sockaddr *addr)
+{
+    static char straddr[INET6_ADDRSTRLEN];
+    void *ipaddr;
+    if (addr->sa_family == AF_INET6) {
+	struct sockaddr_in6 * ip = (struct sockaddr_in6*)addr;
+	ipaddr = &ip->sin6_addr;
+    } else {
+	struct sockaddr_in * ip = (struct sockaddr_in*)addr;
+	ipaddr = &ip->sin_addr;
+    }
+    const char *res = inet_ntop(addr->sa_family, ipaddr, straddr, sizeof straddr);
+    return (char *)res;
+}
+
 static void *tcpReceiveThread(void* arg)
 {
     tcpPort_t *port = (tcpPort_t*)arg;
@@ -58,6 +124,7 @@ static void *tcpReceiveThread(void* arg)
 
 static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
 {
+    socklen_t sockaddrlen;
     if (port->isInitalized){
         return port;
     }
@@ -66,14 +133,29 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
         return NULL;
     }
 
-    port->socketFd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
+    uint16_t tcpPort = BASE_IP_ADDRESS + id - 1;
+    if (lookup_address(NULL, tcpPort, SOCK_STREAM, (struct sockaddr*)&port->sockAddress, &sockaddrlen) != 0) {
+	return NULL;
+    }
+    port->socketFd = socket(((struct sockaddr*)&port->sockAddress)->sa_family, SOCK_STREAM, IPPROTO_TCP);
+
     if (port->socketFd < 0) {
         fprintf(stderr, "[SOCKET] Unable to create tcp socket\n");
         return NULL;
     }
+    int err = 0;
+#ifdef __CYGWIN__
+    // Sadly necesary to enforce dual-stack behaviour on Windows networking ,,,
+    if (((struct sockaddr*)&port->sockAddress)->sa_family == AF_INET6) {
+	int v6only=0;
+	err = setsockopt(port->socketFd, IPPROTO_IPV6, IPV6_V6ONLY, &v6only, sizeof(v6only));
+	if (err != 0) {
+	    fprintf(stderr,"[SOCKET] setting V6ONLY=false: %s\n", strerror(errno));
+	}
+    }
+#endif
 
     int one = 1;
-    int err = 0;
     err = setsockopt(port->socketFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
     err = fcntl(port->socketFd, F_SETFL, fcntl(port->socketFd, F_GETFL, 0) | O_NONBLOCK);
 
@@ -82,14 +164,10 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
         return NULL;
     }
 
-    uint16_t tcpPort = BASE_IP_ADDRESS + id - 1; 
-    port->sockAddress.sin_family = AF_INET;
-    port->sockAddress.sin_port = htons(tcpPort);
-    port->sockAddress.sin_addr.s_addr = INADDR_ANY;
     port->isClientConnected = false;
     port->id = id;
 
-    if (bind(port->socketFd, (struct sockaddr*)&port->sockAddress, sizeof(port->sockAddress)) < 0) {
+    if (bind(port->socketFd, (struct sockaddr*)&port->sockAddress, sockaddrlen) < 0) {
         fprintf(stderr, "[SOCKET] Unable to bind socket\n");
         return NULL;
     }
@@ -99,16 +177,12 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
         return NULL;
     }
 
-    fprintf(stderr, "[SOCKET] Bind TCP port %d to UART%d\n", tcpPort, id);
+    fprintf(stderr, "[SOCKET] Bind TCP %s port %d to UART%d\n",
+	    tcpGetAddressString((struct sockaddr*)&port->sockAddress), tcpPort, id);
 
     return port;
 }
 
-static char *tcpGetAddressString(struct sockaddr *addr)
-{
-    return inet_ntoa(((struct sockaddr_in *)addr)->sin_addr);
-}
-
 int tcpReceive(tcpPort_t *port)
 {
     if (!port->isClientConnected) {
@@ -123,14 +197,14 @@ int tcpReceive(tcpPort_t *port)
             return -1;
         }
 
-        socklen_t addrLen = sizeof(port->sockAddress);
+	socklen_t addrLen = sizeof(struct sockaddr_storage);
-        port->clientSocketFd = accept(port->socketFd, &port->clientAddress, &addrLen);
+        port->clientSocketFd = accept(port->socketFd,(struct sockaddr*)&port->clientAddress, &addrLen);
         if (port->clientSocketFd < 1) {
             fprintf(stderr, "[SOCKET] Can't accept connection.\n");
             return -1;
         }
 
-        fprintf(stderr, "[SOCKET] %s connected to UART%d\n", tcpGetAddressString(&port->clientAddress), port->id);
+       fprintf(stderr, "[SOCKET] %s connected to UART%d\n", tcpGetAddressString((struct sockaddr *)&port->clientAddress), port->id);
         port->isClientConnected = true;
     }
 
@@ -140,7 +214,7 @@ int tcpReceive(tcpPort_t *port)
     // Disconnect
     if (port->isClientConnected && recvSize == 0)
     {
-       fprintf(stderr, "[SOCKET] %s disconnected from UART%d\n", tcpGetAddressString(&port->clientAddress), port->id);
+       fprintf(stderr, "[SOCKET] %s disconnected from UART%d\n", tcpGetAddressString((struct sockaddr *)&port->clientAddress), port->id);
        close(port->clientSocketFd);
        memset(&port->clientAddress, 0, sizeof(port->clientAddress));
        port->isClientConnected = false;

src/main/drivers/serial_tcp.h

@@ -22,6 +22,9 @@
 
 #include <pthread.h>
 #include <arpa/inet.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <netdb.h>
 
 #define BASE_IP_ADDRESS 5760
 #define TCP_BUFFER_SIZE 2048
@@ -39,8 +42,8 @@ typedef struct
     pthread_t receiveThread;
     int socketFd;
     int clientSocketFd;
-    struct sockaddr_in sockAddress;
+    struct sockaddr_storage sockAddress;
-    struct sockaddr clientAddress;
+    struct sockaddr_storage clientAddress;
     bool isClientConnected;
 } tcpPort_t;
 

src/main/io/osd.c

@@ -444,7 +444,7 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
  */
 void osdSimpleAltitudeSymbol(char *buff, int32_t alt) {
 
-    int32_t convertedAltutude;
+    int32_t convertedAltutude = 0;
     char suffix = '\0';
 
     switch ((osd_unit_e)osdConfig()->units) {

src/main/target/SITL/sim/simple_soap_client.c

@@ -31,6 +31,8 @@
 #include <unistd.h>
 #include <string.h>
 #include <sys/socket.h>
+# include <netinet/in.h>
+# include <netdb.h>
 #include <fcntl.h>
 #include <sys/select.h>
 
@@ -165,4 +167,3 @@ char* soapClientReceive(soap_client_t *client)
     recBuffer[size] = '\0';
     return strdup(body);
 }
-

src/main/target/SITL/sim/simple_soap_client.h

@@ -23,6 +23,8 @@
  */
 
 #include <arpa/inet.h>
+#include <netinet/in.h>
+#include <netdb.h>
 
 #define SOAP_REC_BUF_SIZE 256 * 1024
 

src/main/target/SITL/sim/xplane.c

@@ -29,6 +29,9 @@
 #include <stdarg.h>
 #include <sys/socket.h>
 #include <arpa/inet.h>
+#include <netdb.h>
+#include <sys/types.h>
+#include <netinet/in.h>
 #include <pthread.h>
 #include <errno.h>
 #include <math.h>
@@ -63,7 +66,8 @@
 static uint8_t pwmMapping[XP_MAX_PWM_OUTS];
 static uint8_t mappingCount;
 
-static struct sockaddr_in serverAddr;
+static struct sockaddr_storage serverAddr;
+static socklen_t serverAddrLen;
 static int sockFd;
 static pthread_t listenThread;
 static bool initalized = false;
@@ -152,18 +156,18 @@ static void registerDref(dref_t id, char* dref, uint32_t freq)
     memcpy(buf + 9, &id, 4);
     memcpy(buf + 13, dref, strlen(dref) + 1);
 
-    sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, sizeof(serverAddr));
+    sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, serverAddrLen);
 }
 
 static void sendDref(char* dref, float value)
 {
     char buf[509];
     strcpy(buf, "DREF");
-	memcpy(buf + 5, &value, 4);
+    memcpy(buf + 5, &value, 4);
-	memset(buf + 9, ' ', sizeof(buf) - 9);
+    memset(buf + 9, ' ', sizeof(buf) - 9);
-	strcpy(buf + 9, dref);
+    strcpy(buf + 9, dref);
 
-    sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, sizeof(serverAddr));
+    sendto(sockFd, (void*)buf, sizeof(buf), 0, (struct sockaddr*)&serverAddr, serverAddrLen);
 }
 
 static void* listenWorker(void* arg)
@@ -171,7 +175,7 @@ static void* listenWorker(void* arg)
     UNUSED(arg);
 
     uint8_t buf[1024];
-    struct sockaddr remoteAddr;
+    struct sockaddr_storage remoteAddr;
     socklen_t slen = sizeof(remoteAddr);
     int recvLen;
 
@@ -435,22 +439,113 @@ static void* listenWorker(void* arg)
     return NULL;
 }
 
+static int lookup_address (char *name, int port, int type, struct sockaddr *addr, socklen_t* len )
+{
+    struct addrinfo *servinfo, *p;
+    struct addrinfo hints = {.ai_family = AF_UNSPEC, .ai_socktype = type, .ai_flags = AI_V4MAPPED|AI_ADDRCONFIG};
+    if (name == NULL) {
+	hints.ai_flags |= AI_PASSIVE;
+    }
+  /*
+    This nonsense is to uniformly deliver the same sa_family regardless of whether
+    name is NULL or non-NULL ** ON LINUX **
+    Otherwise, at least on Linux, we get
+    - V6,V4 for the non-null case and
+    - V4,V6 for the null case, regardless of gai.conf
+    Which may confuse consumers
+    FreeBSD and Windows behave consistently, giving V6 for Ipv6 enabled stacks
+    unless a quad dotted address is specified (or a name resolveds to V4,
+    or system policy enforces IPv4 over V6
+  */
+    struct addrinfo *p4 = NULL;
+    struct addrinfo *p6 = NULL;
+
+    int result;
+    char aport[16];
+    snprintf(aport, sizeof(aport), "%d", port);
+
+    if ((result = getaddrinfo(name, aport, &hints, &servinfo)) != 0) {
+        fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(result));
+        return result;
+    } else {
+	int j = 0;
+	for(p = servinfo; p != NULL; p = p->ai_next) {
+	    if(p->ai_family == AF_INET6)
+		p6 = p;
+	    else if(p->ai_family == AF_INET)
+		p4 = p;
+	    j++;
+	}
+
+	if (p6 != NULL)
+	    p = p6;
+	else if (p4 != NULL)
+	    p = p4;
+	else
+	    return -1;
+	memcpy(addr, p->ai_addr, p->ai_addrlen);
+	*len = p->ai_addrlen;
+	freeaddrinfo(servinfo);
+    }
+    return 0;
+}
+
+static char * pretty_print_address(struct sockaddr* p)
+{
+    char straddr[INET6_ADDRSTRLEN];
+    void *addr;
+    uint16_t port;
+    if (p->sa_family == AF_INET6) {
+	struct sockaddr_in6 * ip = (struct sockaddr_in6*)p;
+	addr = &ip->sin6_addr;
+	port = ntohs(ip->sin6_port);
+    } else {
+	struct sockaddr_in * ip = (struct sockaddr_in*)p;
+	port = ntohs(ip->sin_port);
+	addr = &ip->sin_addr;
+    }
+    const char *res = inet_ntop(p->sa_family, addr, straddr, sizeof straddr);
+    if (res != NULL) {
+	int nb = strlen(res)+16;
+	char *buf = calloc(nb,1);
+	char *ptr = buf;
+	if (p->sa_family == AF_INET6) {
+	    *ptr++='[';
+	}
+	ptr = stpcpy(ptr, res);
+	if (p->sa_family == AF_INET6) {
+	    *ptr++=']';
+	}
+	sprintf(ptr, ":%d", port);
+	return buf;
+    }
+    return NULL;
+}
+
 bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool imu)
 {
     memcpy(pwmMapping, mapping, mapCount);
     mappingCount = mapCount;
     useImu = imu;
 
-    sockFd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+    if (port == 0) {
+	port = XP_PORT; // use default port
+    }
 
-    if (sockFd < 0) {
+    if(lookup_address(ip, port, SOCK_DGRAM, (struct sockaddr*)&serverAddr, &serverAddrLen) != 0) {
         return false;
     }
 
-    struct sockaddr_in addr;
+    sockFd = socket(((struct sockaddr*)&serverAddr)->sa_family, SOCK_DGRAM, IPPROTO_UDP);
-    addr.sin_family = AF_INET;
+    if (sockFd < 0) {
-    addr.sin_addr.s_addr = htonl(INADDR_ANY);
+        return false;
-    addr.sin_port = htons(0);
+    } else {
+	char *nptr = pretty_print_address((struct sockaddr *)&serverAddr);
+	if (nptr != NULL) {
+	    fprintf(stderr, "[SOCKET] xplane address = %s, fd=%d\n", nptr, sockFd);
+	    free(nptr);
+	}
+    }
 
     struct timeval tv;
 	tv.tv_sec = 1;
@@ -463,12 +558,6 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
         return false;
     }
 
-    bind(sockFd, (struct sockaddr *) &addr, sizeof(addr));
-
-    serverAddr.sin_family = AF_INET;
-	serverAddr.sin_addr.s_addr = inet_addr(ip);
-	serverAddr.sin_port = htons(port);
-    
     if (pthread_create(&listenThread, NULL, listenWorker, NULL) < 0) {
         return false;
     }

src/main/target/SITL/target.c

@@ -66,20 +66,23 @@ static bool useImu = false;
 static char *simIp = NULL;
 static int simPort = 0;
 
+static char **c_argv;
+
 void systemInit(void) {
 
     fprintf(stderr, "INAV %d.%d.%d SITL\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL);
     clock_gettime(CLOCK_MONOTONIC, &start_time);
     fprintf(stderr, "[SYSTEM] Init...\n");
 
+#if !defined(__FreeBSD__)   // maybe also || !defined(__APPLE__)
     pthread_attr_t thAttr;
     int policy = 0;
 
     pthread_attr_init(&thAttr);
     pthread_attr_getschedpolicy(&thAttr, &policy);
-
     pthread_setschedprio(pthread_self(), sched_get_priority_min(policy));
     pthread_attr_destroy(&thAttr);
+#endif
 
     if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
         fprintf(stderr, "[SYSTEM] Unable to create mainLoop lock.\n");
@@ -174,16 +177,21 @@ void printCmdLineOptions(void)
 
 void parseArguments(int argc, char *argv[])
 {
+    // Stash these so we can rexec on reboot, just like a FC does
+    c_argv = calloc(argc+1, sizeof(char *));
+    for (int i = 0; i < argc; i++) {
+        c_argv[i] = strdup(argv[i]);
+    }
     int c;
     while(true) {
         static struct option longOpt[] = {
-            {"sim", optional_argument, 0, 's'},
+            {"sim", required_argument, 0, 's'},
-            {"useimu", optional_argument, 0, 'u'},
+            {"useimu", no_argument, 0, 'u'},
-            {"chanmap", optional_argument, 0, 'c'},
+            {"chanmap", required_argument, 0, 'c'},
-            {"simip", optional_argument, 0, 'i'},
+            {"simip", required_argument, 0, 'i'},
-            {"simport", optional_argument, 0, 'p'},
+            {"simport", required_argument, 0, 'p'},
-            {"help", optional_argument, 0, 'h'},
+            {"help", no_argument, 0, 'h'},
-            {"path", optional_argument, 0, 'e'},
+            {"path", required_argument, 0, 'e'},
             {NULL, 0, NULL, 0}
         };
 
@@ -276,7 +284,14 @@ void delay(timeMs_t ms)
 void systemReset(void)
 {
     fprintf(stderr, "[SYSTEM] Reset\n");
-    exit(0);
+#if defined(__CYGWIN__) || defined(__APPLE__) || GCC_MAJOR < 12
+    for(int j = 3; j < 1024; j++) {
+        close(j);
+    }
+#else
+    closefrom(3);
+#endif
+    execvp(c_argv[0], c_argv); // restart
 }
 
 void systemResetToBootloader(void)