mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-12 19:10:27 +03:00
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)
This commit is contained in:
Jonathan Hudson
GitHub
parent
4fa7e508ed
commit
064a809ad2
9 changed files with 343 additions and 133 deletions
|
|
@ -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).
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
port->clientSocketFd = accept(port->socketFd, &port->clientAddress, &addrLen);
|
||||
socklen_t addrLen = sizeof(struct sockaddr_storage);
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -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 clientAddress;
|
||||
struct sockaddr_storage sockAddress;
|
||||
struct sockaddr_storage clientAddress;
|
||||
bool isClientConnected;
|
||||
} tcpPort_t;
|
||||
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -23,6 +23,8 @@
|
|||
*/
|
||||
|
||||
#include <arpa/inet.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netdb.h>
|
||||
|
||||
#define SOAP_REC_BUF_SIZE 256 * 1024
|
||||
|
||||
|
|
|
|||
|
|
@ -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,7 +156,7 @@ 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)
|
||||
|
|
@ -163,7 +167,7 @@ static void sendDref(char* dref, float value)
|
|||
memset(buf + 9, ' ', sizeof(buf) - 9);
|
||||
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;
|
||||
addr.sin_family = AF_INET;
|
||||
addr.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||
addr.sin_port = htons(0);
|
||||
sockFd = socket(((struct sockaddr*)&serverAddr)->sa_family, SOCK_DGRAM, IPPROTO_UDP);
|
||||
if (sockFd < 0) {
|
||||
return false;
|
||||
} 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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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'},
|
||||
{"useimu", optional_argument, 0, 'u'},
|
||||
{"chanmap", optional_argument, 0, 'c'},
|
||||
{"simip", optional_argument, 0, 'i'},
|
||||
{"simport", optional_argument, 0, 'p'},
|
||||
{"help", optional_argument, 0, 'h'},
|
||||
{"path", optional_argument, 0, 'e'},
|
||||
{"sim", required_argument, 0, 's'},
|
||||
{"useimu", no_argument, 0, 'u'},
|
||||
{"chanmap", required_argument, 0, 'c'},
|
||||
{"simip", required_argument, 0, 'i'},
|
||||
{"simport", required_argument, 0, 'p'},
|
||||
{"help", no_argument, 0, 'h'},
|
||||
{"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)
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue