Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 03:19:58 +03:00
Code Issues Wiki Activity

6.1.0 sitl interop (#8913)

Browse source 
* 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 2023-03-26 13:39:44 +01:00 • committed by GitHub
parent 4fa7e508ed
commit 064a809ad2
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
9 changed files with 343 additions and 133 deletions
Download patch file Download diff file Expand all files Collapse all files

48
docs/SITL/SITL.md
View file

@ -27,12 +27,13 @@ The following sensors are emulated:
![SITL-Fake-Sensors](assets/SITL-Fake-Sensors.png) ![SITL-Fake-Sensors](assets/SITL-Fake-Sensors.png)
Select "FAKE" as type for all mentioned, so that they receive the data from the simulator. Select "FAKE" as type for all mentioned, so that they receive the data from the simulator.
## Serial ports+ ## Serial ports+
UARTs are replaced by TCP starting with port 5760 ascending. UART 1 port 5760, UART2 6761, ... 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. 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). 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. The assignment and status of user UART/TCP connections is displayed on the console.
@ -41,11 +42,11 @@ The assignment and status of user UART/TCP connections is displayed on the conso
All other interfaces (I2C, SPI, etc.) are not emulated. All other interfaces (I2C, SPI, etc.) are not emulated.
## Remote control ## Remote control
Joystick (via simulator) or serial receiver via USB/Serial interface are supported. Joystick (via simulator) or serial receiver via USB/Serial interface are supported.
### Joystick interface ### Joystick interface
Only 8 channels are supported. Only 8 channels are supported.
Select "SIM (SITL)" as the receiver and set up a joystick in the simulator, details of which can be found in the documentation for the individual simulators. Select "SIM (SITL)" as the receiver and set up a joystick in the simulator, details of which can be found in the documentation for the individual simulators.
### Serial Receiver via USB ### Serial Receiver via USB
Connect a serial receiver (e.g. SBUS) to the PC via a UART/USB adapter. Configure the receiver in the Configurator as usual. Connect a serial receiver (e.g. SBUS) to the PC via a UART/USB adapter. Configure the receiver in the Configurator as usual.
@ -61,8 +62,8 @@ For this you need a FT232 module. With FT-Prog (https://ftdichip.com/utilities/)
![SITL-SBUS-FT232](assets/SITL-SBUS-FT232.png) ![SITL-SBUS-FT232](assets/SITL-SBUS-FT232.png)
For SBUS, the command line arguments of the python script are: For SBUS, the command line arguments of the python script are:
```python tcp_serial_redirect.py --parity E --stopbits 2 -c 127.0.0.1:[INAV-UART-PORT] COMXX 100000``` ```python tcp_serial_redirect.py --parity E --stopbits 2 -c 127.0.0.1:[INAV-UART-PORT] COMXX 100000```
Note: Telemetry via return channel through the receiver is not supported by SITL (yet). Note: Telemetry via return channel through the receiver is not supported by SITL (yet).
@ -70,15 +71,15 @@ Note: Telemetry via return channel through the receiver is not supported by SITL
For the OSD the program INAV-Sim-OSD is available: https://github.com/Scavanger/INAV-SIM-OSD. For the OSD the program INAV-Sim-OSD is available: https://github.com/Scavanger/INAV-SIM-OSD.
For this, activate MSP-Displayport on a UART/TCP port and connect to the corresponding port. For this, activate MSP-Displayport on a UART/TCP port and connect to the corresponding port.
Note: INAV-Sim-OSD only works if the simulator is in window mode. Note: INAV-Sim-OSD only works if the simulator is in window mode.
## Command line ## Command line
The command line options are only necessary if the SITL executable is started by hand, e.g. when debugging. The command line options are only necessary if the SITL executable is started by hand, e.g. when debugging.
For normal use, please use the SITL tab in the configurator. For normal use, please use the SITL tab in the configurator.
The following SITL specific command line options are available: 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``` ```--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,9 +109,9 @@ It is recommended to start the tools in the following order:
## Compile ## Compile
### Linux: ### Linux and FreeBSD:
Almost like normal, ruby, cmake and make are also required. Almost like normal, ruby, cmake and make are also required.
With cmake, the option "-DSITL=ON" must be specified. With cmake, the option "-DSITL=ON" must be specified.
``` ```
mkdir build_SITL mkdir build_SITL
@ -120,5 +121,26 @@ make
``` ```
### Windows: ### Windows:
Compile under cygwin, then as in Linux. Compile under cygwin, then as in Linux.
Copy cygwin1.dll into the directory, or include cygwin's /bin/ directory in the environment variable PATH. 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).

20
docs/SITL/X-Plane.md
View file

@ -1,4 +1,4 @@
# X-Plane # X-Plane
Tested on X-Plane 11, 12 should(!) work but not tested. Tested on X-Plane 11, 12 should(!) work but not tested.
@ -20,21 +20,25 @@ In the settings, calibrate the joystick, set it up and assign the axes as follow
| Roll | Roll | | Roll | Roll |
| Pitch | Pitch | | Pitch | Pitch |
| Throttle | Cowl Flap 1 | | Throttle | Cowl Flap 1 |
| Yaw | Yaw | | Yaw | Yaw |
| Channel 5 | Cowl Flap 2 | | Channel 5 | Cowl Flap 2 |
| Channel 6 | Cowl Flap 3 | | Channel 6 | Cowl Flap 3 |
| Channel 7 | Cowl Flap 4 | | Channel 7 | Cowl Flap 4 |
| Channel 8 | Cowl Flap 5 | | Channel 8 | Cowl Flap 5 |
Reverse axis in X-Plane if necessary. Reverse axis in X-Plane if necessary.
## Channelmap: ## Channelmap:
The assignment of the "virtual receiver" is fixed: The assignment of the "virtual receiver" is fixed:
1 - Throttle 1 - Throttle
2 - Roll 2 - Roll
3 - Pitch 3 - Pitch
4 - Yaw 4 - Yaw
The internal mixer (e.g. for flying wings) cannot be deactivated without further ado, therefore always select "Aircraft with tail" in INAV. 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: For the standard Aircraft preset the channelmap is:
```--chanmap=M01-01,S01-03,S03-02,S04-04``` ```--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.

136
src/main/drivers/serial_tcp.c
View file

@ -33,6 +33,10 @@
#include <sys/socket.h> #include <sys/socket.h>
#include <fcntl.h> #include <fcntl.h>
#include <netinet/in.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <errno.h>
#include "common/utils.h" #include "common/utils.h"
@ -43,6 +47,68 @@ static const struct serialPortVTable tcpVTable[];
static tcpPort_t tcpPorts[SERIAL_PORT_COUNT]; static tcpPort_t tcpPorts[SERIAL_PORT_COUNT];
static bool tcpThreadRunning = false; 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) static void *tcpReceiveThread(void* arg)
{ {
tcpPort_t *port = (tcpPort_t*)arg; tcpPort_t *port = (tcpPort_t*)arg;
@ -50,7 +116,7 @@ static void *tcpReceiveThread(void* arg)
while(tcpThreadRunning) { while(tcpThreadRunning) {
if (tcpReceive(port) < 0) { if (tcpReceive(port) < 0) {
break; break;
} }
} }
return NULL; return NULL;
@ -58,6 +124,7 @@ static void *tcpReceiveThread(void* arg)
static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id) static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
{ {
socklen_t sockaddrlen;
if (port->isInitalized){ if (port->isInitalized){
return port; return port;
} }
@ -66,14 +133,29 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
return NULL; 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) { if (port->socketFd < 0) {
fprintf(stderr, "[SOCKET] Unable to create tcp socket\n"); fprintf(stderr, "[SOCKET] Unable to create tcp socket\n");
return NULL; return NULL;
} }
int one = 1;
int err = 0; 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;
err = setsockopt(port->socketFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); 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); 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; 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->isClientConnected = false;
port->id = id; 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"); fprintf(stderr, "[SOCKET] Unable to bind socket\n");
return NULL; return NULL;
} }
@ -99,57 +177,53 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
return NULL; 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; return port;
}
static char *tcpGetAddressString(struct sockaddr *addr)
{
return inet_ntoa(((struct sockaddr_in *)addr)->sin_addr);
} }
int tcpReceive(tcpPort_t *port) int tcpReceive(tcpPort_t *port)
{ {
if (!port->isClientConnected) { if (!port->isClientConnected) {
fd_set fds; fd_set fds;
FD_ZERO(&fds); FD_ZERO(&fds);
FD_SET(port->socketFd, &fds); FD_SET(port->socketFd, &fds);
if (select(port->socketFd + 1, &fds, NULL, NULL, NULL) < 0) { if (select(port->socketFd + 1, &fds, NULL, NULL, NULL) < 0) {
fprintf(stderr, "[SOCKET] Unable to wait for connection.\n"); fprintf(stderr, "[SOCKET] Unable to wait for connection.\n");
return -1; 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) { if (port->clientSocketFd < 1) {
fprintf(stderr, "[SOCKET] Can't accept connection.\n"); fprintf(stderr, "[SOCKET] Can't accept connection.\n");
return -1; 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; port->isClientConnected = true;
} }
uint8_t buffer[TCP_BUFFER_SIZE]; uint8_t buffer[TCP_BUFFER_SIZE];
ssize_t recvSize = recv(port->clientSocketFd, buffer, TCP_BUFFER_SIZE, 0); ssize_t recvSize = recv(port->clientSocketFd, buffer, TCP_BUFFER_SIZE, 0);
// Disconnect // Disconnect
if (port->isClientConnected && recvSize == 0) 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); close(port->clientSocketFd);
memset(&port->clientAddress, 0, sizeof(port->clientAddress)); memset(&port->clientAddress, 0, sizeof(port->clientAddress));
port->isClientConnected = false; port->isClientConnected = false;
return 0; return 0;
} }
for (ssize_t i = 0; i < recvSize; i++) { for (ssize_t i = 0; i < recvSize; i++) {
if (port->serialPort.rxCallback) { if (port->serialPort.rxCallback) {
port->serialPort.rxCallback((uint16_t)buffer[i], port->serialPort.rxCallbackData); port->serialPort.rxCallback((uint16_t)buffer[i], port->serialPort.rxCallbackData);
} else { } else {
@ -159,7 +233,7 @@ int tcpReceive(tcpPort_t *port)
pthread_mutex_unlock(&port->receiveMutex); pthread_mutex_unlock(&port->receiveMutex);
} }
} }
if (recvSize < 0) { if (recvSize < 0) {
recvSize = 0; recvSize = 0;
} }
@ -199,7 +273,7 @@ serialPort_t *tcpOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,
return NULL; return NULL;
} }
tcpThreadRunning = true; tcpThreadRunning = true;
return (serialPort_t*)port; return (serialPort_t*)port;
} }
@ -254,7 +328,7 @@ uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
uint32_t tcpTotalTxBytesFree(const serialPort_t *instance) uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
{ {
tcpPort_t *port = (tcpPort_t*)instance; tcpPort_t *port = (tcpPort_t*)instance;
if (port->isClientConnected) { if (port->isClientConnected) {
return TCP_MAX_PACKET_SIZE; return TCP_MAX_PACKET_SIZE;
} else { } else {

9
src/main/drivers/serial_tcp.h
View file

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

6
src/main/io/osd.c
View file

@ -429,7 +429,7 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
} }
osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3); osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3);
if (!isValid && ((millis() / 1000) % 4 < 2)) if (!isValid && ((millis() / 1000) % 4 < 2))
suffix = '*'; suffix = '*';
@ -444,7 +444,7 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid)
*/ */
void osdSimpleAltitudeSymbol(char *buff, int32_t alt) { void osdSimpleAltitudeSymbol(char *buff, int32_t alt) {
int32_t convertedAltutude; int32_t convertedAltutude = 0;
char suffix = '\0'; char suffix = '\0';
switch ((osd_unit_e)osdConfig()->units) { switch ((osd_unit_e)osdConfig()->units) {
@ -2471,7 +2471,7 @@ static bool osdDrawSingleElement(uint8_t item)
displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_L, elemAttr); displayWriteCharWithAttr(osdDisplayPort, elemPosX, elemPosY, SYM_SERVO_PAN_IS_OFFSET_L, elemAttr);
} else { } else {
panServoTimeOffCentre = 0; panServoTimeOffCentre = 0;
if (osdConfig()->pan_servo_indicator_show_degrees) { if (osdConfig()->pan_servo_indicator_show_degrees) {
tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES); tfp_sprintf(buff, "%3d%c", panOffset, SYM_DEGREES);
displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX+1, elemPosY, buff, elemAttr);

15
src/main/target/SITL/sim/simple_soap_client.c
View file

@ -31,6 +31,8 @@
#include <unistd.h> #include <unistd.h>
#include <string.h> #include <string.h>
#include <sys/socket.h> #include <sys/socket.h>
# include <netinet/in.h>
# include <netdb.h>
#include <fcntl.h> #include <fcntl.h>
#include <sys/select.h> #include <sys/select.h>
@ -50,7 +52,7 @@ bool soapClientConnect(soap_client_t *client, const char *address, int port)
if (setsockopt(client->sockedFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) < 0) { if (setsockopt(client->sockedFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) < 0) {
return false; return false;
} }
client->socketAddr.sin_family = AF_INET; client->socketAddr.sin_family = AF_INET;
client->socketAddr.sin_port = htons(port); client->socketAddr.sin_port = htons(port);
client->socketAddr.sin_addr.s_addr = inet_addr(address); client->socketAddr.sin_addr.s_addr = inet_addr(address);
@ -79,7 +81,7 @@ void soapClientSendRequestVa(soap_client_t *client, const char* action, const ch
return; return;
} }
char* requestBody; char* requestBody;
if (vasprintf(&requestBody, fmt, va) < 0) { if (vasprintf(&requestBody, fmt, va) < 0) {
return; return;
} }
@ -89,16 +91,16 @@ void soapClientSendRequestVa(soap_client_t *client, const char* action, const ch
action, (unsigned)strlen(requestBody), requestBody) < 0) { action, (unsigned)strlen(requestBody), requestBody) < 0) {
return; return;
} }
send(client->sockedFd, request, strlen(request), 0); send(client->sockedFd, request, strlen(request), 0);
} }
void soapClientSendRequest(soap_client_t *client, const char* action, const char *fmt, ...) void soapClientSendRequest(soap_client_t *client, const char* action, const char *fmt, ...)
{ {
va_list va; va_list va;
va_start(va, fmt); va_start(va, fmt);
soapClientSendRequestVa(client, action, fmt, va); soapClientSendRequestVa(client, action, fmt, va);
va_end(va); va_end(va);
} }
@ -159,10 +161,9 @@ char* soapClientReceive(soap_client_t *client)
if (size2 <= 0) { if (size2 <= 0) {
return NULL; return NULL;
} }
size += size2; size += size2;
} }
recBuffer[size] = '\0'; recBuffer[size] = '\0';
return strdup(body); return strdup(body);
} }

2
src/main/target/SITL/sim/simple_soap_client.h
View file

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

183
src/main/target/SITL/sim/xplane.c
View file

@ -29,6 +29,9 @@
#include <stdarg.h> #include <stdarg.h>
#include <sys/socket.h> #include <sys/socket.h>
#include <arpa/inet.h> #include <arpa/inet.h>
#include <netdb.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <pthread.h> #include <pthread.h>
#include <errno.h> #include <errno.h>
#include <math.h> #include <math.h>
@ -63,13 +66,14 @@
static uint8_t pwmMapping[XP_MAX_PWM_OUTS]; static uint8_t pwmMapping[XP_MAX_PWM_OUTS];
static uint8_t mappingCount; static uint8_t mappingCount;
static struct sockaddr_in serverAddr; static struct sockaddr_storage serverAddr;
static socklen_t serverAddrLen;
static int sockFd; static int sockFd;
static pthread_t listenThread; static pthread_t listenThread;
static bool initalized = false; static bool initalized = false;
static bool useImu = false; static bool useImu = false;
static float lattitude = 0; static float lattitude = 0;
static float longitude = 0; static float longitude = 0;
static float elevation = 0; static float elevation = 0;
static float agl = 0; static float agl = 0;
@ -92,7 +96,7 @@ static float barometer = 0;
static bool hasJoystick = false; static bool hasJoystick = false;
static float joystickRaw[XPLANE_JOYSTICK_AXIS_COUNT]; static float joystickRaw[XPLANE_JOYSTICK_AXIS_COUNT];
typedef enum typedef enum
{ {
DREF_LATITUDE, DREF_LATITUDE,
DREF_LONGITUDE, DREF_LONGITUDE,
@ -126,12 +130,12 @@ typedef enum
DREF_JOYSTICK_VALUES_CH8, DREF_JOYSTICK_VALUES_CH8,
} dref_t; } dref_t;
uint32_t xint2uint32 (uint8_t * buf) uint32_t xint2uint32 (uint8_t * buf)
{ {
return buf[3] << 24 | buf [2] << 16 | buf [1] << 8 | buf [0]; return buf[3] << 24 | buf [2] << 16 | buf [1] << 8 | buf [0];
} }
float xflt2float (uint8_t * buf) float xflt2float (uint8_t * buf)
{ {
union { union {
float f; float f;
@ -152,18 +156,18 @@ static void registerDref(dref_t id, char* dref, uint32_t freq)
memcpy(buf + 9, &id, 4); memcpy(buf + 9, &id, 4);
memcpy(buf + 13, dref, strlen(dref) + 1); 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) static void sendDref(char* dref, float value)
{ {
char buf[509]; char buf[509];
strcpy(buf, "DREF"); 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) static void* listenWorker(void* arg)
@ -171,13 +175,13 @@ static void* listenWorker(void* arg)
UNUSED(arg); UNUSED(arg);
uint8_t buf[1024]; uint8_t buf[1024];
struct sockaddr remoteAddr; struct sockaddr_storage remoteAddr;
socklen_t slen = sizeof(remoteAddr); socklen_t slen = sizeof(remoteAddr);
int recvLen; int recvLen;
while (true) while (true)
{ {
float motorValue = 0; float motorValue = 0;
float yokeValues[3] = { 0 }; float yokeValues[3] = { 0 };
int y = 0; int y = 0;
@ -189,10 +193,10 @@ static void* listenWorker(void* arg)
motorValue = PWM_TO_FLOAT_0_1(motor[pwmMapping[i] & 0x7f]); motorValue = PWM_TO_FLOAT_0_1(motor[pwmMapping[i] & 0x7f]);
} else { } else {
yokeValues[y] = PWM_TO_FLOAT_MINUS_1_1(servo[pwmMapping[i]]); yokeValues[y] = PWM_TO_FLOAT_MINUS_1_1(servo[pwmMapping[i]]);
y++; y++;
} }
} }
sendDref("sim/operation/override/override_joystick", 1); sendDref("sim/operation/override/override_joystick", 1);
sendDref("sim/cockpit2/engine/actuators/throttle_ratio_all", motorValue); sendDref("sim/cockpit2/engine/actuators/throttle_ratio_all", motorValue);
sendDref("sim/joystick/yoke_roll_ratio", yokeValues[0]); sendDref("sim/joystick/yoke_roll_ratio", yokeValues[0]);
@ -203,11 +207,11 @@ static void* listenWorker(void* arg)
sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[2]", 0); sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[2]", 0);
sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[3]", 0); sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[3]", 0);
sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[4]", 0); sendDref("sim/cockpit2/engine/actuators/cowl_flap_ratio[4]", 0);
recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, (struct sockaddr*)&remoteAddr, &slen); recvLen = recvfrom(sockFd, buf, sizeof(buf), 0, (struct sockaddr*)&remoteAddr, &slen);
if (recvLen < 0 && errno != EWOULDBLOCK) { if (recvLen < 0 && errno != EWOULDBLOCK) {
continue; continue;
} }
if (strncmp((char*)buf, "RREF", 4) != 0) { if (strncmp((char*)buf, "RREF", 4) != 0) {
continue; continue;
@ -216,7 +220,7 @@ static void* listenWorker(void* arg)
for (int i = 5; i < recvLen; i += 8) { for (int i = 5; i < recvLen; i += 8) {
dref_t dref = (dref_t)xint2uint32(&buf[i]); dref_t dref = (dref_t)xint2uint32(&buf[i]);
float value = xflt2float(&(buf[i + 4])); float value = xflt2float(&(buf[i + 4]));
switch (dref) switch (dref)
{ {
case DREF_LATITUDE: case DREF_LATITUDE:
@ -230,7 +234,7 @@ static void* listenWorker(void* arg)
case DREF_ELEVATION: case DREF_ELEVATION:
elevation = value; elevation = value;
break; break;
case DREF_AGL: case DREF_AGL:
agl = value; agl = value;
break; break;
@ -274,7 +278,7 @@ static void* listenWorker(void* arg)
case DREF_FORCE_G_AXI1: case DREF_FORCE_G_AXI1:
accel_x = value; accel_x = value;
break; break;
case DREF_FORCE_G_SIDE: case DREF_FORCE_G_SIDE:
accel_y = value; accel_y = value;
break; break;
@ -290,7 +294,7 @@ static void* listenWorker(void* arg)
case DREF_POS_Q: case DREF_POS_Q:
gyro_y = value; gyro_y = value;
break; break;
case DREF_POS_R: case DREF_POS_R:
gyro_z = value; gyro_z = value;
break; break;
@ -302,36 +306,36 @@ static void* listenWorker(void* arg)
case DREF_HAS_JOYSTICK: case DREF_HAS_JOYSTICK:
hasJoystick = value >= 1 ? true : false; hasJoystick = value >= 1 ? true : false;
break; break;
case DREF_JOYSTICK_VALUES_ROll: case DREF_JOYSTICK_VALUES_ROll:
joystickRaw[0] = value; joystickRaw[0] = value;
break; break;
case DREF_JOYSTICK_VALUES_PITCH: case DREF_JOYSTICK_VALUES_PITCH:
joystickRaw[1] = value; joystickRaw[1] = value;
break; break;
case DREF_JOYSTICK_VALUES_THROTTLE: case DREF_JOYSTICK_VALUES_THROTTLE:
joystickRaw[2] = value; joystickRaw[2] = value;
break; break;
case DREF_JOYSTICK_VALUES_YAW: case DREF_JOYSTICK_VALUES_YAW:
joystickRaw[3] = value; joystickRaw[3] = value;
break; break;
case DREF_JOYSTICK_VALUES_CH5: case DREF_JOYSTICK_VALUES_CH5:
joystickRaw[4] = value; joystickRaw[4] = value;
break; break;
case DREF_JOYSTICK_VALUES_CH6: case DREF_JOYSTICK_VALUES_CH6:
joystickRaw[5] = value; joystickRaw[5] = value;
break; break;
case DREF_JOYSTICK_VALUES_CH7: case DREF_JOYSTICK_VALUES_CH7:
joystickRaw[6] = value; joystickRaw[6] = value;
break; break;
case DREF_JOYSTICK_VALUES_CH8: case DREF_JOYSTICK_VALUES_CH8:
joystickRaw[7] = value; joystickRaw[7] = value;
break; break;
@ -375,8 +379,8 @@ static void* listenWorker(void* arg)
0, //(int16_t)round(-local_vy * 100), 0, //(int16_t)round(-local_vy * 100),
0 0
); );
const int32_t altitideOverGround = (int32_t)round(agl * 100); const int32_t altitideOverGround = (int32_t)round(agl * 100);
if (altitideOverGround > 0 && altitideOverGround <= RANGEFINDER_VIRTUAL_MAX_RANGE_CM) { if (altitideOverGround > 0 && altitideOverGround <= RANGEFINDER_VIRTUAL_MAX_RANGE_CM) {
fakeRangefindersSetData(altitideOverGround); fakeRangefindersSetData(altitideOverGround);
} else { } else {
@ -391,7 +395,7 @@ static void* listenWorker(void* arg)
imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav); imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav);
imuUpdateAttitude(micros()); imuUpdateAttitude(micros());
} }
fakeAccSet( fakeAccSet(
constrainToInt16(-accel_x * GRAVITY_MSS * 1000), constrainToInt16(-accel_x * GRAVITY_MSS * 1000),
constrainToInt16(accel_y * GRAVITY_MSS * 1000), constrainToInt16(accel_y * GRAVITY_MSS * 1000),
@ -425,7 +429,7 @@ static void* listenWorker(void* arg)
if (!initalized) { if (!initalized) {
ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL);
// Aircraft can wobble on the runway and prevents calibration of the accelerometer // Aircraft can wobble on the runway and prevents calibration of the accelerometer
ENABLE_STATE(ACCELEROMETER_CALIBRATED); ENABLE_STATE(ACCELEROMETER_CALIBRATED);
initalized = true; initalized = true;
} }
@ -435,22 +439,113 @@ static void* listenWorker(void* arg)
return NULL; 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) bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool imu)
{ {
memcpy(pwmMapping, mapping, mapCount); memcpy(pwmMapping, mapping, mapCount);
mappingCount = mapCount; mappingCount = mapCount;
useImu = imu; useImu = imu;
sockFd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (sockFd < 0) { if (port == 0) {
port = XP_PORT; // use default port
}
if(lookup_address(ip, port, SOCK_DGRAM, (struct sockaddr*)&serverAddr, &serverAddrLen) != 0) {
return false; 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; struct timeval tv;
tv.tv_sec = 1; tv.tv_sec = 1;
@ -462,13 +557,7 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
if (setsockopt(sockFd, SOL_SOCKET, SO_SNDTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) { if (setsockopt(sockFd, SOL_SOCKET, SO_SNDTIMEO, (struct timeval *) &tv,sizeof(struct timeval))) {
return false; 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) { if (pthread_create(&listenThread, NULL, listenWorker, NULL) < 0) {
return false; return false;
} }
@ -508,4 +597,4 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool
} }
return true; return true;
} }

57
src/main/target/SITL/target.c
View file

@ -66,20 +66,23 @@ static bool useImu = false;
static char *simIp = NULL; static char *simIp = NULL;
static int simPort = 0; static int simPort = 0;
static char **c_argv;
void systemInit(void) { void systemInit(void) {
fprintf(stderr, "INAV %d.%d.%d SITL\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL); fprintf(stderr, "INAV %d.%d.%d SITL\n", FC_VERSION_MAJOR, FC_VERSION_MINOR, FC_VERSION_PATCH_LEVEL);
clock_gettime(CLOCK_MONOTONIC, &start_time); clock_gettime(CLOCK_MONOTONIC, &start_time);
fprintf(stderr, "[SYSTEM] Init...\n"); fprintf(stderr, "[SYSTEM] Init...\n");
#if !defined(__FreeBSD__) // maybe also || !defined(__APPLE__)
pthread_attr_t thAttr; pthread_attr_t thAttr;
int policy = 0; int policy = 0;
pthread_attr_init(&thAttr); pthread_attr_init(&thAttr);
pthread_attr_getschedpolicy(&thAttr, &policy); pthread_attr_getschedpolicy(&thAttr, &policy);
pthread_setschedprio(pthread_self(), sched_get_priority_min(policy)); pthread_setschedprio(pthread_self(), sched_get_priority_min(policy));
pthread_attr_destroy(&thAttr); pthread_attr_destroy(&thAttr);
#endif
if (pthread_mutex_init(&mainLoopLock, NULL) != 0) { if (pthread_mutex_init(&mainLoopLock, NULL) != 0) {
fprintf(stderr, "[SYSTEM] Unable to create mainLoop lock.\n"); fprintf(stderr, "[SYSTEM] Unable to create mainLoop lock.\n");
@ -89,7 +92,7 @@ void systemInit(void) {
if (sitlSim != SITL_SIM_NONE) { if (sitlSim != SITL_SIM_NONE) {
fprintf(stderr, "[SIM] Waiting for connection...\n"); fprintf(stderr, "[SIM] Waiting for connection...\n");
} }
switch (sitlSim) { switch (sitlSim) {
case SITL_SIM_REALFLIGHT: case SITL_SIM_REALFLIGHT:
if (mappingCount > RF_MAX_PWM_OUTS) { if (mappingCount > RF_MAX_PWM_OUTS) {
@ -108,7 +111,7 @@ void systemInit(void) {
fprintf(stderr, "[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", XP_MAX_PWM_OUTS); fprintf(stderr, "[SIM] Mapping error. RealFligt supports a maximum of %i PWM outputs.", XP_MAX_PWM_OUTS);
sitlSim = SITL_SIM_NONE; sitlSim = SITL_SIM_NONE;
break; break;
} }
if (simXPlaneInit(simIp, simPort, pwmMapping, mappingCount, useImu)) { if (simXPlaneInit(simIp, simPort, pwmMapping, mappingCount, useImu)) {
fprintf(stderr, "[SIM] Connection with X-Plane successfully established.\n"); fprintf(stderr, "[SIM] Connection with X-Plane successfully established.\n");
} else { } else {
@ -119,13 +122,13 @@ void systemInit(void) {
fprintf(stderr, "[SIM] No interface specified. Configurator only.\n"); fprintf(stderr, "[SIM] No interface specified. Configurator only.\n");
break; break;
} }
rescheduleTask(TASK_SERIAL, 1); rescheduleTask(TASK_SERIAL, 1);
} }
bool parseMapping(char* mapStr) bool parseMapping(char* mapStr)
{ {
char *split = strtok(mapStr, ","); char *split = strtok(mapStr, ",");
char numBuf[2]; char numBuf[2];
while(split) while(split)
{ {
@ -158,7 +161,7 @@ bool parseMapping(char* mapStr)
return true; return true;
} }
void printCmdLineOptions(void) void printCmdLineOptions(void)
{ {
fprintf(stderr, "Avaiable options:\n"); fprintf(stderr, "Avaiable options:\n");
fprintf(stderr, "--path=[path] Path and filename of eeprom.bin. If not specified 'eeprom.bin' in program directory is used.\n"); fprintf(stderr, "--path=[path] Path and filename of eeprom.bin. If not specified 'eeprom.bin' in program directory is used.\n");
@ -169,21 +172,26 @@ void printCmdLineOptions(void)
fprintf(stderr, "--chanmap=[mapstring] Channel mapping. Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n"); fprintf(stderr, "--chanmap=[mapstring] Channel mapping. Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n");
fprintf(stderr, " The mapstring has the following format: M(otor)|S(servo)<INAV-OUT>-<RECEIVER-OUT>,... All numbers must have two digits\n"); fprintf(stderr, " The mapstring has the following format: M(otor)|S(servo)<INAV-OUT>-<RECEIVER-OUT>,... All numbers must have two digits\n");
fprintf(stderr, " For example: Map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n"); fprintf(stderr, " For example: Map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n");
fprintf(stderr, " --chanmap=M01-01,S01-02,S02-03\n"); fprintf(stderr, " --chanmap=M01-01,S01-02,S02-03\n");
} }
void parseArguments(int argc, char *argv[]) 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; int c;
while(true) { while(true) {
static struct option longOpt[] = { 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} {NULL, 0, NULL, 0}
}; };
@ -192,7 +200,7 @@ void parseArguments(int argc, char *argv[])
break; break;
switch (c) { switch (c) {
case 's': case 's':
if (strcmp(optarg, "rf") == 0) { if (strcmp(optarg, "rf") == 0) {
sitlSim = SITL_SIM_REALFLIGHT; sitlSim = SITL_SIM_REALFLIGHT;
} else if (strcmp(optarg, "xp") == 0){ } else if (strcmp(optarg, "xp") == 0){
@ -210,7 +218,7 @@ void parseArguments(int argc, char *argv[])
} }
break; break;
case 'p': case 'p':
simPort = atoi(optarg); simPort = atoi(optarg);
break; break;
case 'u': case 'u':
useImu = true; useImu = true;
@ -227,12 +235,12 @@ void parseArguments(int argc, char *argv[])
printCmdLineOptions(); printCmdLineOptions();
exit(0); exit(0);
break; break;
} }
} }
if (simIp == NULL) { if (simIp == NULL) {
simIp = malloc(10); simIp = malloc(10);
strcpy(simIp, "127.0.0.1"); strcpy(simIp, "127.0.0.1");
} }
} }
@ -261,7 +269,7 @@ uint64_t microsISR(void)
uint32_t millis(void) { uint32_t millis(void) {
return (uint32_t)(micros() / 1000); return (uint32_t)(micros() / 1000);
} }
void delayMicroseconds(timeUs_t us) void delayMicroseconds(timeUs_t us)
{ {
@ -273,10 +281,17 @@ void delay(timeMs_t ms)
delayMicroseconds(ms * 1000UL); delayMicroseconds(ms * 1000UL);
} }
void systemReset(void) void systemReset(void)
{ {
fprintf(stderr, "[SYSTEM] Reset\n"); 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) void systemResetToBootloader(void)