mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-12 19:10:27 +03:00
SITL: implemented built-in serial receivers support in SITL, implemented FC proxy mode, updated SITL docs (#9365)
* implemented built-in serial receivers support in SITL implemented FC Proxy for SITL update SITL.md * Update SITL.md * Update SITL.md * Update SITL.md * Update SITL.md * [SITL] update POSIX serial functions to support non-standard baud rates * disable xon/hof on serial port (SITL serial_proxy) --------- Co-authored-by: Jonathan Hudson <jh+github@daria.co.uk>
This commit is contained in:
Roman Lut
GitHub
parent
c5d1459e4e
commit
e92337a5fc
15 changed files with 1143 additions and 96 deletions
|
|
@ -16,6 +16,10 @@ Currently supported are
|
|||
|
||||
INAV SITL communicates for sensor data and control directly with the corresponding simulator, see the documentation of the individual simulators and the Configurator or the command line options.
|
||||
|
||||
AS SITL is still an inav software, but running on PC, it is possible to use HITL interface for communication.
|
||||
|
||||
INAV-X-Plane-HITL plugin https://github.com/RomanLut/INAV-X-Plane-HITL can be used with SITL.
|
||||
|
||||
## Sensors
|
||||
The following sensors are emulated:
|
||||
- IMU (Gyro, Accelerometer)
|
||||
|
|
@ -30,13 +34,18 @@ The following sensors are emulated:
|
|||
|
||||
Select "FAKE" as type for all mentioned, so that they receive the data from the simulator.
|
||||
|
||||
## Serial ports+
|
||||
UARTs are replaced by TCP starting with port 5760 ascending. UART 1 port 5760, UART2 5761, ...
|
||||
By default, UART1 and UART2 are available as MSP connections. Other UARTs will have TCP listeners if they have an INAV function assigned.
|
||||
To connect the Configurator to SITL: Select TCP and connect to ```localhost:5760``` (or ```127.0.0.1:5760``` if your OS doesn't understand `localhost`) (if SITL is running on the same machine).
|
||||
## Serial ports
|
||||
UARTs are replaced by TCP starting with port 5760 ascending. UART1 is mapped to port 5760, UART2 to 5761, etc.
|
||||
|
||||
By default, UART1 and UART2 are configured for MSP connections. Other UARTs will have TCP listeners if they have an INAV function assigned.
|
||||
|
||||
To connect the Configurator to SITL, select "SITL".
|
||||
|
||||
Alternativelly, select "TCP" and connect to ```localhost:5760``` (or ```127.0.0.1:5760``` if your OS doesn't understand `localhost`) (if SITL is running on the same machine).
|
||||
|
||||
IPv4 and IPv6 are supported, either raw addresses or host-name lookup.
|
||||
|
||||
The assignment and status of user UART/TCP connections is displayed on the console.
|
||||
The assignment and status of used UART/TCP connections is displayed on the console.
|
||||
|
||||
```
|
||||
INAV 6.1.0 SITL
|
||||
|
|
@ -51,39 +60,73 @@ INAV 6.1.0 SITL
|
|||
All other interfaces (I2C, SPI, etc.) are not emulated.
|
||||
|
||||
## Remote control
|
||||
MSP_RX (TCP/IP) or joystick (via simulator) or serial receiver via USB/Serial interface are supported.
|
||||
Multiple methods for connecting RC Controllers are available:
|
||||
- MSP_RX (TCP/IP)
|
||||
- joystick (via simulator)
|
||||
- serial receiver via USB to serial converter
|
||||
- any receiver with proxy flight controller
|
||||
|
||||
|
||||
### MSP_RX
|
||||
|
||||
MSP_RX is the default, 18 channels are supported over TCP/IP serial emulation.
|
||||
MSP_RX is the default, 18 channels are supported over TCP/IP connection.
|
||||
|
||||
### Joystick interface
|
||||
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.
|
||||
|
||||
*Not available with INAV-X-Plane-HITL plugin.*
|
||||
|
||||
### 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 to the PC via a USB-to-serial adapter
|
||||
- Configure the receiver in the SITL as usual
|
||||
- While starting SITL from configurator, enable "Serial receiver" option
|
||||
|
||||
The Configurator offers a built-in option for forwarding the serial data to the SITL TCP port, if SITL is started manually the following option can be used:
|
||||
The SITL offers a built-in option for forwarding the host's serial port to the SITL UART.
|
||||
|
||||
The connection can then be established with a programme that forwards the serial data unaltered to TCP, e.g. with the Python script tcp_serial_redirect.py (https://github.com/Scavanger/TCP-Serial-Redirect)
|
||||
If necessary, please download the required runtime environment from https://www.python.org/.
|
||||
Please use the linked version, which has a smaller buffer, otherwise the control response is relatively slow.
|
||||
Please note that 100000(SBUS) and 420000(CRSF) are non-standart baud rates which may not be supported by some USB-to-serial adapters. FDTI and CH340 should work. CP2102/9 does not work.
|
||||
|
||||
### Example SBUS:
|
||||
For this you need a FT232 module. With FT-Prog (https://ftdichip.com/utilities/) the signals can be inverted: Devices->Scan and Parse, then Hardware Specific -> Invert RS232 Signals -> Invert RXD.
|
||||
|
||||
#### Example SBUS:
|
||||
For this you need a USB-to-serial adapter, receiver with inverter, or receiver which can output inverted SBUS (normal UART).
|
||||
|
||||
SBUS protocol is inverted UART.
|
||||
|
||||
Receiver's SBUS output should be connected to the USB-to-serial adapter's RX pin (via inverter).
|
||||
|
||||
With FT-Prog (https://ftdichip.com/utilities/) the signal can be inverted by adapter: Devices->Scan and Parse, then Hardware Specific -> Invert RS232 Signals -> Invert RXD.
|
||||
|
||||
|
||||
|
||||
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```
|
||||
|
||||
|
||||
### Telemetry
|
||||
In the SITL configuration, enable serial receiver on some port and configure receiver type "Serial", "SBUS".
|
||||
|
||||
LTM and MAVLink telemetry are supported, either as a discrete function or shared with MSP.
|
||||
#### Example CRSF:
|
||||
|
||||
On receiver side, CRSF is normal UART.
|
||||
|
||||
Connect receiver's RX/TX pins (and GND, 5V of course) to USB-To-Serial adapter's TX/RX pins (RX to TX, TX to RX).
|
||||
|
||||
|
||||
|
||||
In the SITL configuration, enable serial receiver on some port and configure receiver type "Serial", "CRSF".
|
||||
|
||||
### Proxy Flight controller
|
||||
|
||||
The last, but probably the most easiest way to connect receiver to the SITL, is to use any inav/betaflight Flight controler as proxy.
|
||||
|
||||
Connect receiver of any type to FC and configure FC to the point where channels are correctly updated in the "Receiver" tab. Inav and Betaflight are supported.
|
||||
|
||||
You also can use your plane/quad ( if receiver is powered from USB).
|
||||
|
||||
|
||||
|
||||
In the SITL configuration, select "Receiver type: SIM" regardles of the kind of receiver used.
|
||||
|
||||
RX Telemetry via a return channel through the receiver is not yet supported by SITL.
|
||||
|
||||
## OSD
|
||||
For the OSD the program INAV-Sim-OSD is available: https://github.com/Scavanger/INAV-SIM-OSD.
|
||||
|
|
@ -91,6 +134,8 @@ For this, activate MSP-Displayport on a UART/TCP port and connect to the corresp
|
|||
|
||||
Note: INAV-Sim-OSD only works if the simulator is in window mode.
|
||||
|
||||
*With INAV-X-Plane-HITL plugin, OSD is supported natively.*
|
||||
|
||||
## Command line
|
||||
|
||||
The command line options are only necessary if the SITL executable is started by hand.
|
||||
|
|
@ -103,7 +148,7 @@ If SITL is started without command line options, only a serial MSP / CLI connect
|
|||
|
||||
```--path``` 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```, ```/home/user/sitl-eeproms/test-eeprom.bin```.
|
||||
|
||||
```--sim=[sim]``` Select the simulator. xp = X-Plane, rf = RealFlight. Example: ```--sim=xp```
|
||||
```--sim=[sim]``` Select the simulator. xp = X-Plane, rf = RealFlight. Example: ```--sim=xp```. If not specified, configurator-only mode is started. Omit for usage with INAV-X-Plane-HITL plugin.
|
||||
|
||||
```--simip=[ip]``` Hostname or IP address of the simulator, if you specify a simulator with "--sim" and omit this option IPv4 localhost (`127.0.0.1`) will be used. Example: ```--simip=172.65.21.15```, ```--simip acme-sims.org```, ```--sim ::1```.
|
||||
|
||||
|
|
@ -118,6 +163,18 @@ To assign motor1 to virtual receiver channel 1, servo 1 to channel 2, and servo2
|
|||
```--chanmap:M01-01,S01-02,S02-03```
|
||||
Please also read the documentation of the individual simulators.
|
||||
|
||||
```--serialport``` Use serial receiver or proxy FC connected to host's serial port, f.e. ```--serialportCOM5``` or ```--serialportdev/ttyACM3```
|
||||
|
||||
```--serialuart``` Map serial receiver to SITL UART, f.e. ```--serialuart=3``` for UART3. Omit if using ```--fcproxy```.
|
||||
|
||||
```--baudrate``` Serial receiver baudrate (default: 115200)
|
||||
|
||||
```--stopbits=[None|One|Two]``` Serial receiver stopbits (default: One)
|
||||
|
||||
```--parity=[Even|None|Odd]``` Serial receiver parity (default: None)
|
||||
|
||||
```--fcproxy``` Use inav/betaflight FC as a proxy for serial receiver.
|
||||
|
||||
```--help``` Displays help for the command line options.
|
||||
|
||||
For options that take an argument, either form `--flag=value` or `--flag value` may be used.
|
||||
|
|
@ -125,46 +182,13 @@ For options that take an argument, either form `--flag=value` or `--flag value`
|
|||
## Running SITL
|
||||
It is recommended to start the tools in the following order:
|
||||
1. Simulator, aircraft should be ready for take-off
|
||||
2. INAV-SITL
|
||||
2. SITL
|
||||
3. OSD
|
||||
4. serial redirect for RC input
|
||||
|
||||
## Compile
|
||||
For INav-X-Plane-HITL plugin:
|
||||
1. SITL (Run in configurator-only mode)
|
||||
2. X-Plane
|
||||
|
||||
### Linux and FreeBSD:
|
||||
Almost like normal, ruby, cmake and make are also required.
|
||||
With cmake, the option "-DSITL=ON" must be specified.
|
||||
# #Forwarding serial data for other UART
|
||||
|
||||
```
|
||||
mkdir build_SITL
|
||||
cd build_SITL
|
||||
cmake -DSITL=ON ..
|
||||
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.
|
||||
|
||||
If the build fails (segfault, possibly out of memory), adding `-DCMAKE_BUILD_TYPE=MinRelSize` to the `cmake` command may help.
|
||||
|
||||
#### 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. GCC 10 to GCC 13 are known to work.
|
||||
* Unix sockets networking. Cygwin is required on Windows (vice `winsock`).
|
||||
* Pthreads
|
||||
|
||||
## Supported environments
|
||||
|
||||
* Linux on x86_64, ia-32, Aarch64 (e.g. Rpi4), RISCV64 (e.g. VisionFive2)
|
||||
* Windows on x86_64
|
||||
* FreeBSD (x86_64 at least).
|
||||
Other UARTs can then be mapped to host's serial port using external tool, which can be found in directories ```inav-configurator\resources\sitl\linux\Ser2TCP```, ```inav-configurator\resources\sitl\windows\Ser2TCP.exe```
|
||||
|
|
|
|||
BIN
docs/SITL/assets/serial_receiver_crsf.png
Normal file
BIN
docs/SITL/assets/serial_receiver_crsf.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 6.8 KiB |
BIN
docs/SITL/assets/serial_receiver_proxy.png
Normal file
BIN
docs/SITL/assets/serial_receiver_proxy.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 6.9 KiB |
BIN
docs/SITL/assets/serial_receiver_sbus.png
Normal file
BIN
docs/SITL/assets/serial_receiver_sbus.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 6.6 KiB |
39
docs/development/Building SITL.md
Normal file
39
docs/development/Building SITL.md
Normal file
|
|
@ -0,0 +1,39 @@
|
|||
## Building SITL
|
||||
|
||||
### Linux and FreeBSD:
|
||||
Almost like normal, ruby, cmake and make are also required.
|
||||
With cmake, the option "-DSITL=ON" must be specified.
|
||||
|
||||
```
|
||||
mkdir build_SITL
|
||||
cd build_SITL
|
||||
cmake -DSITL=ON ..
|
||||
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.
|
||||
|
||||
If the build fails (segfault, possibly out of memory), adding `-DCMAKE_BUILD_TYPE=MinRelSize` to the `cmake` command may help.
|
||||
|
||||
#### 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. GCC 10 to GCC 13 are known to work.
|
||||
* Unix sockets networking. Cygwin is required on Windows (vice `winsock`).
|
||||
* Pthreads
|
||||
|
||||
## Supported environments
|
||||
|
||||
* Linux on x86_64, ia-32, Aarch64 (e.g. Rpi4), RISCV64 (e.g. VisionFive2)
|
||||
* Windows on x86_64
|
||||
* FreeBSD (x86_64 at least).
|
||||
|
|
@ -43,6 +43,7 @@
|
|||
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/serial_tcp.h"
|
||||
#include "target/SITL/serial_proxy.h"
|
||||
|
||||
static const struct serialPortVTable tcpVTable[];
|
||||
static tcpPort_t tcpPorts[SERIAL_PORT_COUNT];
|
||||
|
|
@ -118,6 +119,23 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
|
|||
return port;
|
||||
}
|
||||
|
||||
void tcpReceiveBytes( tcpPort_t *port, const uint8_t* buffer, ssize_t recvSize ) {
|
||||
for (ssize_t i = 0; i < recvSize; i++) {
|
||||
if (port->serialPort.rxCallback) {
|
||||
port->serialPort.rxCallback((uint16_t)buffer[i], port->serialPort.rxCallbackData);
|
||||
} else {
|
||||
pthread_mutex_lock(&port->receiveMutex);
|
||||
port->serialPort.rxBuffer[port->serialPort.rxBufferHead] = buffer[i];
|
||||
port->serialPort.rxBufferHead = (port->serialPort.rxBufferHead + 1) % port->serialPort.rxBufferSize;
|
||||
pthread_mutex_unlock(&port->receiveMutex);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void tcpReceiveBytesEx( int portIndex, const uint8_t* buffer, ssize_t recvSize ) {
|
||||
tcpReceiveBytes( &tcpPorts[portIndex], buffer, recvSize );
|
||||
}
|
||||
|
||||
int tcpReceive(tcpPort_t *port)
|
||||
{
|
||||
char addrbuf[IPADDRESS_PRINT_BUFLEN];
|
||||
|
|
@ -162,22 +180,12 @@ int tcpReceive(tcpPort_t *port)
|
|||
return 0;
|
||||
}
|
||||
|
||||
for (ssize_t i = 0; i < recvSize; i++) {
|
||||
|
||||
if (port->serialPort.rxCallback) {
|
||||
port->serialPort.rxCallback((uint16_t)buffer[i], port->serialPort.rxCallbackData);
|
||||
} else {
|
||||
pthread_mutex_lock(&port->receiveMutex);
|
||||
port->serialPort.rxBuffer[port->serialPort.rxBufferHead] = buffer[i];
|
||||
port->serialPort.rxBufferHead = (port->serialPort.rxBufferHead + 1) % port->serialPort.rxBufferSize;
|
||||
pthread_mutex_unlock(&port->receiveMutex);
|
||||
}
|
||||
}
|
||||
|
||||
if (recvSize < 0) {
|
||||
recvSize = 0;
|
||||
}
|
||||
|
||||
tcpReceiveBytes( port, buffer, recvSize );
|
||||
|
||||
return (int)recvSize;
|
||||
}
|
||||
|
||||
|
|
@ -240,9 +248,21 @@ void tcpWritBuf(serialPort_t *instance, const void *data, int count)
|
|||
send(port->clientSocketFd, data, count, 0);
|
||||
}
|
||||
|
||||
int getTcpPortIndex(const serialPort_t *instance) {
|
||||
for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
|
||||
if ( &(tcpPorts[i].serialPort) == instance) return i;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void tcpWrite(serialPort_t *instance, uint8_t ch)
|
||||
{
|
||||
tcpWritBuf(instance, (void*)&ch, 1);
|
||||
|
||||
int index = getTcpPortIndex(instance);
|
||||
if ( !serialFCProxy && serialProxyIsConnected() && (index == (serialUartIndex-1)) ) {
|
||||
serialProxyWriteData( (unsigned char *)&ch, 1);
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
|
||||
|
|
@ -263,6 +283,10 @@ uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
|
|||
return count;
|
||||
}
|
||||
|
||||
uint32_t tcpRXBytesFree(int portIndex) {
|
||||
return tcpPorts[portIndex].serialPort.rxBufferSize - tcpTotalRxBytesWaiting( &tcpPorts[portIndex].serialPort);
|
||||
}
|
||||
|
||||
uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
|
||||
{
|
||||
UNUSED(instance);
|
||||
|
|
@ -272,7 +296,6 @@ uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
|
|||
bool isTcpTransmitBufferEmpty(const serialPort_t *instance)
|
||||
{
|
||||
UNUSED(instance);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -26,6 +26,8 @@
|
|||
#include <netinet/in.h>
|
||||
#include <netdb.h>
|
||||
|
||||
#include "drivers/serial.h"
|
||||
|
||||
#define BASE_IP_ADDRESS 5760
|
||||
#define TCP_BUFFER_SIZE 2048
|
||||
#define TCP_MAX_PACKET_SIZE 65535
|
||||
|
|
@ -50,5 +52,7 @@ typedef struct
|
|||
|
||||
serialPort_t *tcpOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, void *rxCallbackData, uint32_t baudRate, portMode_t mode, portOptions_t options);
|
||||
|
||||
void tcpSend(tcpPort_t *port);
|
||||
int tcpReceive(tcpPort_t *port);
|
||||
extern void tcpSend(tcpPort_t *port);
|
||||
extern int tcpReceive(tcpPort_t *port);
|
||||
extern void tcpReceiveBytesEx( int portIndex, const uint8_t* buffer, ssize_t recvSize );
|
||||
extern uint32_t tcpRXBytesFree(int portIndex);
|
||||
|
|
@ -147,6 +147,10 @@
|
|||
|
||||
#include "telemetry/telemetry.h"
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
#include "target/SITL/serial_proxy.h"
|
||||
#endif
|
||||
|
||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||
#include "hardware_revision.h"
|
||||
#endif
|
||||
|
|
@ -223,6 +227,10 @@ void init(void)
|
|||
flashDeviceInitialized = flashInit();
|
||||
#endif
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
serialProxyInit();
|
||||
#endif
|
||||
|
||||
initEEPROM();
|
||||
ensureEEPROMContainsValidData();
|
||||
suspendRxSignal();
|
||||
|
|
|
|||
|
|
@ -93,6 +93,10 @@
|
|||
|
||||
#include "config/feature.h"
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
#include "target/SITL/serial_proxy.h"
|
||||
#endif
|
||||
|
||||
void taskHandleSerial(timeUs_t currentTimeUs)
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
|
|
@ -421,6 +425,10 @@ void fcTasksInit(void)
|
|||
#if defined(USE_SMARTPORT_MASTER)
|
||||
setTaskEnabled(TASK_SMARTPORT_MASTER, true);
|
||||
#endif
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
serialProxyStart();
|
||||
#endif
|
||||
}
|
||||
|
||||
cfTask_t cfTasks[TASK_COUNT] = {
|
||||
|
|
|
|||
|
|
@ -27,6 +27,11 @@
|
|||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
#include "target/SITL/serial_proxy.h"
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef SOFTSERIAL_LOOPBACK
|
||||
serialPort_t *loopbackPort;
|
||||
#endif
|
||||
|
|
@ -65,6 +70,9 @@ int main(void)
|
|||
loopbackInit();
|
||||
|
||||
while (true) {
|
||||
#if defined(SITL_BUILD)
|
||||
serialProxyProcess();
|
||||
#endif
|
||||
scheduler();
|
||||
processLoopback();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -31,15 +31,14 @@
|
|||
|
||||
static uint16_t channels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
static bool hasNewData = false;
|
||||
static uint16_t rssi = 0;
|
||||
|
||||
static uint16_t rxSimReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan)
|
||||
{
|
||||
static uint16_t rxSimReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) {
|
||||
UNUSED(rxRuntimeConfigPtr);
|
||||
return channels[chan];
|
||||
}
|
||||
|
||||
void rxSimSetChannelValue(uint16_t* values, uint8_t count)
|
||||
{
|
||||
void rxSimSetChannelValue(uint16_t* values, uint8_t count) {
|
||||
for (size_t i = 0; i < count; i++) {
|
||||
channels[i] = values[i];
|
||||
}
|
||||
|
|
@ -47,10 +46,11 @@ void rxSimSetChannelValue(uint16_t* values, uint8_t count)
|
|||
hasNewData = true;
|
||||
}
|
||||
|
||||
static uint8_t rxSimFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
static uint8_t rxSimFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) {
|
||||
UNUSED(rxRuntimeConfig);
|
||||
|
||||
lqTrackerSet(rxRuntimeConfig->lqTracker, rssi);
|
||||
|
||||
if (!hasNewData) {
|
||||
return RX_FRAME_PENDING;
|
||||
}
|
||||
|
|
@ -59,8 +59,7 @@ static uint8_t rxSimFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
|
|||
return RX_FRAME_COMPLETE;
|
||||
}
|
||||
|
||||
void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) {
|
||||
UNUSED(rxConfig);
|
||||
|
||||
rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_CHANNEL_COUNT;
|
||||
|
|
@ -68,4 +67,8 @@ void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
|||
rxRuntimeConfig->rcReadRawFn = rxSimReadRawRC;
|
||||
rxRuntimeConfig->rcFrameStatusFn = rxSimFrameStatus;
|
||||
}
|
||||
|
||||
void rxSimSetRssi(uint16_t value) {
|
||||
rssi = value;
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -20,4 +20,5 @@
|
|||
#include "rx/rx.h"
|
||||
|
||||
void rxSimSetChannelValue(uint16_t* values, uint8_t count);
|
||||
void rxSimSetRssi(uint16_t value);
|
||||
void rxSimInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
|
||||
|
|
|
|||
784
src/main/target/SITL/serial_proxy.c
Normal file
784
src/main/target/SITL/serial_proxy.c
Normal file
|
|
@ -0,0 +1,784 @@
|
|||
/*
|
||||
* This file is part of INAV Project.
|
||||
*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
|
||||
* You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*
|
||||
* Alternatively, the contents of this file may be used under the terms
|
||||
* of the GNU General Public License Version 3, as described below:
|
||||
*
|
||||
* This file is free software: you may copy, redistribute and/or modify
|
||||
* it under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or (at your
|
||||
* option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
|
||||
* Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see http://www.gnu.org/licenses/.
|
||||
*/
|
||||
|
||||
#include "serial_proxy.h"
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/select.h>
|
||||
#include <time.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <unistd.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "drivers/time.h"
|
||||
#include "msp/msp_serial.h"
|
||||
#include "msp/msp_protocol.h"
|
||||
#include "common/crc.h"
|
||||
#include "rx/sim.h"
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#ifdef __FreeBSD__
|
||||
# define __BSD_VISIBLE 1
|
||||
#endif
|
||||
|
||||
#ifdef __linux__
|
||||
#include <asm/termbits.h>
|
||||
#ifndef TCGETS2
|
||||
#include <asm-generic/ioctls.h>
|
||||
#endif
|
||||
#else
|
||||
#include <termios.h>
|
||||
#endif
|
||||
|
||||
#ifdef __APPLE__
|
||||
#include <IOKit/serial/ioss.h>
|
||||
#endif
|
||||
|
||||
#include "drivers/serial_tcp.h"
|
||||
|
||||
#define SYM_BEGIN '$'
|
||||
#define SYM_PROTO_V1 'M'
|
||||
#define SYM_PROTO_V2 'X'
|
||||
#define SYM_FROM_MWC '>'
|
||||
#define SYM_TO_MWC '<'
|
||||
#define SYM_UNSUPPORTED '!'
|
||||
|
||||
#define JUMBO_FRAME_MIN_SIZE 255
|
||||
#define MAX_MSP_MESSAGE 1024
|
||||
#define RX_CONFIG_SIZE 24
|
||||
|
||||
typedef enum {
|
||||
DS_IDLE,
|
||||
DS_PROTO_IDENTIFIER,
|
||||
DS_DIRECTION_V1,
|
||||
DS_DIRECTION_V2,
|
||||
DS_FLAG_V2,
|
||||
DS_PAYLOAD_LENGTH_V1,
|
||||
DS_PAYLOAD_LENGTH_JUMBO_LOW,
|
||||
DS_PAYLOAD_LENGTH_JUMBO_HIGH,
|
||||
DS_PAYLOAD_LENGTH_V2_LOW,
|
||||
DS_PAYLOAD_LENGTH_V2_HIGH,
|
||||
DS_CODE_V1,
|
||||
DS_CODE_JUMBO_V1,
|
||||
DS_CODE_V2_LOW,
|
||||
DS_CODE_V2_HIGH,
|
||||
DS_PAYLOAD_V1,
|
||||
DS_PAYLOAD_V2,
|
||||
DS_CHECKSUM_V1,
|
||||
DS_CHECKSUM_V2,
|
||||
} TDecoderState;
|
||||
|
||||
static TDecoderState decoderState = DS_IDLE;
|
||||
|
||||
typedef enum {
|
||||
RXC_IDLE = 0,
|
||||
RXC_RQ = 1,
|
||||
RXC_DONE = 2
|
||||
} TRXConfigState;
|
||||
|
||||
static TRXConfigState rxConfigState = RXC_IDLE;
|
||||
|
||||
static int message_length_expected;
|
||||
static unsigned char message_buffer[MAX_MSP_MESSAGE];
|
||||
static int message_length_received;
|
||||
static int unsupported;
|
||||
static int code;
|
||||
static int message_direction;
|
||||
static uint8_t message_checksum;
|
||||
static int reqCount = 0;
|
||||
static uint16_t rssi = 0;
|
||||
static uint8_t rxConfigBuffer[RX_CONFIG_SIZE];
|
||||
|
||||
static timeMs_t lastWarning = 0;
|
||||
|
||||
int serialUartIndex = -1;
|
||||
char serialPort[64] = "";
|
||||
int serialBaudRate = 115200;
|
||||
OptSerialStopBits_e serialStopBits = OPT_SERIAL_STOP_BITS_ONE; //0:None|1:One|2:OnePointFive|3:Two
|
||||
OptSerialParity_e serialParity = OPT_SERIAL_PARITY_NONE;
|
||||
bool serialFCProxy = false;
|
||||
|
||||
#define FC_PROXY_REQUEST_PERIOD_MIN_MS 20 //inav can handle 100 msp messages per second max
|
||||
#define FC_PROXY_REQUEST_PERIOD_TIMEOUT_MS 200
|
||||
#define FC_PROXY_REQUEST_PERIOD_RSSI_MS 300
|
||||
#define SERIAL_BUFFER_SIZE 256
|
||||
|
||||
#if defined(__CYGWIN__)
|
||||
#include <windows.h>
|
||||
static HANDLE hSerial;
|
||||
#else
|
||||
static int fd;
|
||||
#endif
|
||||
|
||||
static bool connected = false;
|
||||
static bool started = false;
|
||||
|
||||
static timeMs_t nextProxyRequestTimeout = 0;
|
||||
static timeMs_t nextProxyRequestMin = 0;
|
||||
static timeMs_t nextProxyRequestRssi = 0;
|
||||
|
||||
static timeMs_t lastVisit = 0;
|
||||
|
||||
#if !defined(__CYGWIN__)
|
||||
#if !defined( __linux__) && !defined(__APPLE__)
|
||||
static int rate_to_constant(int baudrate)
|
||||
{
|
||||
#ifdef __FreeBSD__
|
||||
return baudrate;
|
||||
#else
|
||||
#define B(x) case x: return B##x
|
||||
switch (baudrate) {
|
||||
B(50);
|
||||
B(75);
|
||||
B(110);
|
||||
B(134);
|
||||
B(150);
|
||||
B(200);
|
||||
B(300);
|
||||
B(600);
|
||||
B(1200);
|
||||
B(1800);
|
||||
B(2400);
|
||||
B(4800);
|
||||
B(9600);
|
||||
B(19200);
|
||||
B(38400);
|
||||
B(57600);
|
||||
B(115200);
|
||||
B(230400);
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
#undef B
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
static void close_serial(void)
|
||||
{
|
||||
#ifdef __linux__
|
||||
ioctl(fd, TCFLSH, TCIOFLUSH);
|
||||
#else
|
||||
tcflush(fd, TCIOFLUSH);
|
||||
#endif
|
||||
close(fd);
|
||||
}
|
||||
|
||||
static int set_fd_speed(void)
|
||||
{
|
||||
int res = -1;
|
||||
#ifdef __linux__
|
||||
// Just user BOTHER for everything (allows non-standard speeds)
|
||||
struct termios2 t;
|
||||
if ((res = ioctl(fd, TCGETS2, &t)) != -1) {
|
||||
t.c_cflag &= ~CBAUD;
|
||||
t.c_cflag |= BOTHER;
|
||||
t.c_ospeed = t.c_ispeed = serialBaudRate;
|
||||
res = ioctl(fd, TCSETS2, &t);
|
||||
}
|
||||
#elif __APPLE__
|
||||
speed_t speed = serialBaudRate;
|
||||
res = ioctl(fd, IOSSIOSPEED, &speed);
|
||||
#else
|
||||
int speed = rate_to_constant(serialBaudRate);
|
||||
struct termios term;
|
||||
if (tcgetattr(fd, &term)) return -1;
|
||||
if (speed == 0) {
|
||||
res = -1;
|
||||
} else {
|
||||
if (cfsetispeed(&term, speed) != -1) {
|
||||
cfsetospeed(&term, speed);
|
||||
res = tcsetattr(fd, TCSANOW, &term);
|
||||
}
|
||||
if (res != -1) {
|
||||
memset(&term, 0, sizeof(term));
|
||||
res = (tcgetattr(fd, &term));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
return res;
|
||||
}
|
||||
|
||||
static int set_attributes(void)
|
||||
{
|
||||
struct termios tio;
|
||||
memset (&tio, 0, sizeof(tio));
|
||||
int res = -1;
|
||||
#ifdef __linux__
|
||||
res = ioctl(fd, TCGETS, &tio);
|
||||
#else
|
||||
res = tcgetattr(fd, &tio);
|
||||
#endif
|
||||
if (res != -1) {
|
||||
// cfmakeraw ...
|
||||
tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
|
||||
tio.c_oflag &= ~OPOST;
|
||||
tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
|
||||
tio.c_cflag &= ~(CSIZE | PARENB);
|
||||
tio.c_cflag |= CS8 | CREAD | CLOCAL;
|
||||
tio.c_cc[VTIME] = 0;
|
||||
tio.c_cc[VMIN] = 0;
|
||||
|
||||
switch (serialStopBits) {
|
||||
case OPT_SERIAL_STOP_BITS_ONE:
|
||||
tio.c_cflag &= ~CSTOPB;
|
||||
break;
|
||||
case OPT_SERIAL_STOP_BITS_TWO:
|
||||
tio.c_cflag |= CSTOPB;
|
||||
break;
|
||||
case OPT_SERIAL_STOP_BITS_INVALID:
|
||||
break;
|
||||
}
|
||||
|
||||
switch (serialParity) {
|
||||
case OPT_SERIAL_PARITY_EVEN:
|
||||
tio.c_cflag |= PARENB;
|
||||
tio.c_cflag &= ~PARODD;
|
||||
break;
|
||||
case OPT_SERIAL_PARITY_NONE:
|
||||
tio.c_cflag &= ~PARENB;
|
||||
tio.c_cflag &= ~PARODD;
|
||||
break;
|
||||
case OPT_SERIAL_PARITY_ODD:
|
||||
tio.c_cflag |= PARENB;
|
||||
tio.c_cflag |= PARODD;
|
||||
break;
|
||||
case OPT_SERIAL_PARITY_INVALID:
|
||||
break;
|
||||
}
|
||||
#ifdef __linux__
|
||||
res = ioctl(fd, TCSETS, &tio);
|
||||
#else
|
||||
res = tcsetattr(fd, TCSANOW, &tio);
|
||||
#endif
|
||||
}
|
||||
if (res != -1) {
|
||||
res = set_fd_speed();
|
||||
}
|
||||
return res;
|
||||
}
|
||||
#endif
|
||||
|
||||
void serialProxyInit(void)
|
||||
{
|
||||
char portName[64 + 20];
|
||||
if ( strlen(serialPort) < 1) {
|
||||
return;
|
||||
}
|
||||
connected = false;
|
||||
|
||||
#if defined(__CYGWIN__)
|
||||
sprintf(portName, "\\\\.\\%s", serialPort);
|
||||
|
||||
hSerial = CreateFile(portName,
|
||||
GENERIC_READ | GENERIC_WRITE,
|
||||
0,
|
||||
NULL,
|
||||
OPEN_EXISTING,
|
||||
FILE_ATTRIBUTE_NORMAL,
|
||||
NULL);
|
||||
|
||||
if (hSerial == INVALID_HANDLE_VALUE) {
|
||||
if (GetLastError() == ERROR_FILE_NOT_FOUND) {
|
||||
fprintf(stderr, "[SERIALPROXY] ERROR: Sserial port was not attached. Reason: %s not available.\n", portName);
|
||||
} else {
|
||||
fprintf(stderr, "[SERIALPROXY] ERROR: Can not connect to serial port, unknown error.\n");
|
||||
}
|
||||
return;
|
||||
} else {
|
||||
DCB dcbSerialParams = { 0 };
|
||||
if (!GetCommState(hSerial, &dcbSerialParams)) {
|
||||
fprintf(stderr, "[SERIALPROXY] ALERT: failed to get current serial parameters!\n");
|
||||
} else {
|
||||
dcbSerialParams.BaudRate = serialBaudRate;
|
||||
dcbSerialParams.ByteSize = 8;
|
||||
|
||||
// Disable software flow control (XON/XOFF)
|
||||
dcbSerialParams.fOutX = FALSE;
|
||||
dcbSerialParams.fInX = FALSE;
|
||||
|
||||
// Disable hardware flow control (RTS/CTS)
|
||||
dcbSerialParams.fRtsControl = RTS_CONTROL_DISABLE;
|
||||
dcbSerialParams.fDtrControl = DTR_CONTROL_DISABLE;
|
||||
|
||||
// Disable any special processing of bytes
|
||||
dcbSerialParams.fBinary = TRUE;
|
||||
|
||||
switch (serialStopBits) {
|
||||
case OPT_SERIAL_STOP_BITS_ONE:
|
||||
dcbSerialParams.StopBits = ONESTOPBIT;
|
||||
break;
|
||||
case OPT_SERIAL_STOP_BITS_TWO:
|
||||
dcbSerialParams.StopBits = TWOSTOPBITS;
|
||||
break;
|
||||
case OPT_SERIAL_STOP_BITS_INVALID:
|
||||
break;
|
||||
}
|
||||
|
||||
switch (serialParity) {
|
||||
case OPT_SERIAL_PARITY_EVEN:
|
||||
dcbSerialParams.Parity = EVENPARITY;
|
||||
break;
|
||||
case OPT_SERIAL_PARITY_NONE:
|
||||
dcbSerialParams.Parity = NOPARITY;
|
||||
break;
|
||||
case OPT_SERIAL_PARITY_ODD:
|
||||
dcbSerialParams.Parity = ODDPARITY;
|
||||
break;
|
||||
case OPT_SERIAL_PARITY_INVALID:
|
||||
break;
|
||||
}
|
||||
|
||||
if (!SetCommState(hSerial, &dcbSerialParams)) {
|
||||
fprintf(stderr, "[SERIALPROXY] ALERT: Could not set Serial Port parameters\n");
|
||||
} else {
|
||||
COMMTIMEOUTS comTimeOut;
|
||||
comTimeOut.ReadIntervalTimeout = MAXDWORD;
|
||||
comTimeOut.ReadTotalTimeoutMultiplier = 0;
|
||||
comTimeOut.ReadTotalTimeoutConstant = 0;
|
||||
comTimeOut.WriteTotalTimeoutMultiplier = 0;
|
||||
comTimeOut.WriteTotalTimeoutConstant = 0;
|
||||
SetCommTimeouts(hSerial, &comTimeOut);
|
||||
}
|
||||
}
|
||||
}
|
||||
#else
|
||||
strcpy(portName, serialPort); // because, windows ...
|
||||
fd = open(serialPort, O_RDWR | O_NOCTTY);
|
||||
if (fd != -1) {
|
||||
int res = 1;
|
||||
res = set_attributes();
|
||||
if (res == -1) {
|
||||
fprintf(stderr, "[SERIALPROXY] ALERT: Failed to configure device: %s %s\n", portName, strerror(errno));
|
||||
close(fd);
|
||||
fd = -1;
|
||||
}
|
||||
} else {
|
||||
fprintf(stderr, "[SERIALPROXY] ERROR: Can not connect to serial port %s\n", portName);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
connected = true;
|
||||
|
||||
if ( !serialFCProxy ) {
|
||||
fprintf(stderr, "[SERIALPROXY] Connected %s to UART%d\n", portName, serialUartIndex);
|
||||
} else {
|
||||
fprintf(stderr, "[SERIALPROXY] Using proxy flight controller on %s\n", portName);
|
||||
}
|
||||
}
|
||||
|
||||
void serialProxyStart(void)
|
||||
{
|
||||
started = true;
|
||||
}
|
||||
|
||||
void serialProxyClose(void)
|
||||
{
|
||||
if (connected) {
|
||||
connected = false;
|
||||
#if defined(__CYGWIN__)
|
||||
CloseHandle(hSerial);
|
||||
#else
|
||||
close_serial();
|
||||
#endif
|
||||
started = false;
|
||||
nextProxyRequestTimeout = 0;
|
||||
nextProxyRequestMin = 0;
|
||||
nextProxyRequestRssi = 0;
|
||||
lastWarning = 0;
|
||||
lastVisit = 0;
|
||||
}
|
||||
}
|
||||
|
||||
static bool canOutputWarning(void)
|
||||
{
|
||||
if ( millis() > lastWarning ) {
|
||||
lastWarning = millis() + 5000;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
int serialProxyReadData(unsigned char *buffer, unsigned int nbChar)
|
||||
{
|
||||
if (!connected) return 0;
|
||||
|
||||
#if defined(__CYGWIN__)
|
||||
COMSTAT status;
|
||||
DWORD errors;
|
||||
DWORD bytesRead;
|
||||
|
||||
ClearCommError(hSerial, &errors, &status);
|
||||
if (status.cbInQue > 0) {
|
||||
unsigned int toRead = (status.cbInQue > nbChar) ? nbChar : status.cbInQue;
|
||||
if (ReadFile(hSerial, buffer, toRead, &bytesRead, NULL) && (bytesRead != 0)) {
|
||||
return bytesRead;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
#else
|
||||
if (nbChar == 0) return 0;
|
||||
int bytesRead = read(fd, buffer, nbChar);
|
||||
return bytesRead;
|
||||
#endif
|
||||
}
|
||||
|
||||
bool serialProxyWriteData(unsigned char *buffer, unsigned int nbChar)
|
||||
{
|
||||
if (!connected) return false;
|
||||
|
||||
#if defined(__CYGWIN__)
|
||||
COMSTAT status;
|
||||
DWORD errors;
|
||||
DWORD bytesSent;
|
||||
if (!WriteFile(hSerial, (void *)buffer, nbChar, &bytesSent, 0)) {
|
||||
ClearCommError(hSerial, &errors, &status);
|
||||
if ( canOutputWarning() ) {
|
||||
fprintf(stderr, "[SERIALPROXY] ERROR: Can not write to serial port\n");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
if ( bytesSent != nbChar ) {
|
||||
if ( canOutputWarning() ) {
|
||||
fprintf(stderr, "[SERIALPROXY] ERROR: Can not write to serial port\n");
|
||||
}
|
||||
}
|
||||
#else
|
||||
ssize_t l = write(fd, buffer, nbChar);
|
||||
if ( l != (ssize_t)nbChar ) {
|
||||
if ( canOutputWarning() ) {
|
||||
fprintf(stderr, "[SERIALPROXY] ERROR: Can not write to serial port\n");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
return true;
|
||||
}
|
||||
|
||||
bool serialProxyIsConnected(void)
|
||||
{
|
||||
return connected;
|
||||
}
|
||||
|
||||
static void mspSendCommand(int cmd, unsigned char *data, int dataLen)
|
||||
{
|
||||
uint8_t msgBuf[MAX_MSP_MESSAGE] = { '$', 'X', '<' };
|
||||
int msgLen = 3;
|
||||
|
||||
mspHeaderV2_t *hdrV2 = (mspHeaderV2_t *)&msgBuf[msgLen];
|
||||
hdrV2->flags = 0;
|
||||
hdrV2->cmd = cmd;
|
||||
hdrV2->size = dataLen;
|
||||
msgLen += sizeof(mspHeaderV2_t);
|
||||
|
||||
for ( int i = 0; i < dataLen; i++ ) {
|
||||
msgBuf[msgLen++] = data[i];
|
||||
}
|
||||
|
||||
uint8_t crc = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
|
||||
crc = crc8_dvb_s2_update(crc, data, dataLen);
|
||||
msgBuf[msgLen] = crc;
|
||||
msgLen++;
|
||||
|
||||
serialProxyWriteData((unsigned char *)&msgBuf, msgLen);
|
||||
}
|
||||
|
||||
static void mspRequestChannels(void)
|
||||
{
|
||||
mspSendCommand(MSP_RC, NULL, 0);
|
||||
}
|
||||
|
||||
static void mspRequestRssi(void)
|
||||
{
|
||||
mspSendCommand(MSP_ANALOG, NULL, 0);
|
||||
}
|
||||
|
||||
static void requestRXConfigState(void)
|
||||
{
|
||||
mspSendCommand(MSP_RX_CONFIG, NULL, 0);
|
||||
rxConfigState = RXC_RQ;
|
||||
fprintf(stderr, "[SERIALPROXY] Requesting RX config from proxy FC...\n");
|
||||
}
|
||||
|
||||
static void processMessage(void)
|
||||
{
|
||||
if ( code == MSP_RC ) {
|
||||
if ( reqCount > 0 ) reqCount--;
|
||||
int count = message_length_received / 2;
|
||||
if ( count <= MAX_SUPPORTED_RC_CHANNEL_COUNT) {
|
||||
uint16_t *channels = (uint16_t *)(&message_buffer[0]);
|
||||
//AETR => AERT
|
||||
uint v = channels[2];
|
||||
channels[2] = channels[3];
|
||||
channels[3] = v;
|
||||
if ( rssi > 0 ) {
|
||||
rxSimSetChannelValue(channels, count);
|
||||
}
|
||||
}
|
||||
} else if ( code == MSP_ANALOG ) {
|
||||
if ( reqCount > 0 ) reqCount--;
|
||||
if ( message_length_received >= 7 ) {
|
||||
rssi = *((uint16_t *)(&message_buffer[3]));
|
||||
rxSimSetRssi( rssi );
|
||||
}
|
||||
} else if ( code == MSP_RX_CONFIG ) {
|
||||
memcpy( &(rxConfigBuffer[0]), &(message_buffer[0]), RX_CONFIG_SIZE );
|
||||
*((uint16_t *) & (rxConfigBuffer[1])) = 2500; //maxcheck
|
||||
*((uint16_t *) & (rxConfigBuffer[5])) = 500; //mincheck
|
||||
|
||||
mspSendCommand( MSP_SET_RX_CONFIG, rxConfigBuffer, RX_CONFIG_SIZE );
|
||||
rxConfigState = RXC_DONE;
|
||||
fprintf(stderr, "[SERIALPROXY] Setting RX config on proxy FC...\n");
|
||||
}
|
||||
}
|
||||
|
||||
static void decodeProxyMessages(unsigned char *data, int len)
|
||||
{
|
||||
for (int i = 0; i < len; i++) {
|
||||
switch (decoderState) {
|
||||
case DS_IDLE: // sync char 1
|
||||
if (data[i] == SYM_BEGIN) {
|
||||
decoderState = DS_PROTO_IDENTIFIER;
|
||||
}
|
||||
break;
|
||||
|
||||
case DS_PROTO_IDENTIFIER: // sync char 2
|
||||
switch (data[i]) {
|
||||
case SYM_PROTO_V1:
|
||||
decoderState = DS_DIRECTION_V1;
|
||||
break;
|
||||
case SYM_PROTO_V2:
|
||||
decoderState = DS_DIRECTION_V2;
|
||||
break;
|
||||
default:
|
||||
//unknown protocol
|
||||
decoderState = DS_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case DS_DIRECTION_V1: // direction (should be >)
|
||||
|
||||
case DS_DIRECTION_V2:
|
||||
unsupported = 0;
|
||||
switch (data[i]) {
|
||||
case SYM_FROM_MWC:
|
||||
message_direction = 1;
|
||||
break;
|
||||
case SYM_TO_MWC:
|
||||
message_direction = 0;
|
||||
break;
|
||||
case SYM_UNSUPPORTED:
|
||||
unsupported = 1;
|
||||
break;
|
||||
}
|
||||
decoderState = decoderState == DS_DIRECTION_V1 ? DS_PAYLOAD_LENGTH_V1 : DS_FLAG_V2;
|
||||
break;
|
||||
|
||||
case DS_FLAG_V2:
|
||||
// Ignored for now
|
||||
decoderState = DS_CODE_V2_LOW;
|
||||
break;
|
||||
|
||||
case DS_PAYLOAD_LENGTH_V1:
|
||||
message_length_expected = data[i];
|
||||
|
||||
if (message_length_expected == JUMBO_FRAME_MIN_SIZE) {
|
||||
decoderState = DS_CODE_JUMBO_V1;
|
||||
} else {
|
||||
message_length_received = 0;
|
||||
decoderState = DS_CODE_V1;
|
||||
}
|
||||
break;
|
||||
|
||||
case DS_PAYLOAD_LENGTH_V2_LOW:
|
||||
message_length_expected = data[i];
|
||||
decoderState = DS_PAYLOAD_LENGTH_V2_HIGH;
|
||||
break;
|
||||
|
||||
case DS_PAYLOAD_LENGTH_V2_HIGH:
|
||||
message_length_expected |= data[i] << 8;
|
||||
message_length_received = 0;
|
||||
if (message_length_expected <= MAX_MSP_MESSAGE) {
|
||||
decoderState = message_length_expected > 0 ? DS_PAYLOAD_V2 : DS_CHECKSUM_V2;
|
||||
} else {
|
||||
//too large payload
|
||||
decoderState = DS_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
case DS_CODE_V1:
|
||||
case DS_CODE_JUMBO_V1:
|
||||
code = data[i];
|
||||
if (message_length_expected > 0) {
|
||||
// process payload
|
||||
if (decoderState == DS_CODE_JUMBO_V1) {
|
||||
decoderState = DS_PAYLOAD_LENGTH_JUMBO_LOW;
|
||||
} else {
|
||||
decoderState = DS_PAYLOAD_V1;
|
||||
}
|
||||
} else {
|
||||
// no payload
|
||||
decoderState = DS_CHECKSUM_V1;
|
||||
}
|
||||
break;
|
||||
|
||||
case DS_CODE_V2_LOW:
|
||||
code = data[i];
|
||||
decoderState = DS_CODE_V2_HIGH;
|
||||
break;
|
||||
|
||||
case DS_CODE_V2_HIGH:
|
||||
code |= data[i] << 8;
|
||||
decoderState = DS_PAYLOAD_LENGTH_V2_LOW;
|
||||
break;
|
||||
|
||||
case DS_PAYLOAD_LENGTH_JUMBO_LOW:
|
||||
message_length_expected = data[i];
|
||||
decoderState = DS_PAYLOAD_LENGTH_JUMBO_HIGH;
|
||||
break;
|
||||
|
||||
case DS_PAYLOAD_LENGTH_JUMBO_HIGH:
|
||||
message_length_expected |= data[i] << 8;
|
||||
message_length_received = 0;
|
||||
decoderState = DS_PAYLOAD_V1;
|
||||
break;
|
||||
|
||||
case DS_PAYLOAD_V1:
|
||||
case DS_PAYLOAD_V2:
|
||||
message_buffer[message_length_received] = data[i];
|
||||
message_length_received++;
|
||||
|
||||
if (message_length_received >= message_length_expected) {
|
||||
decoderState = decoderState == DS_PAYLOAD_V1 ? DS_CHECKSUM_V1 : DS_CHECKSUM_V2;
|
||||
}
|
||||
break;
|
||||
|
||||
case DS_CHECKSUM_V1:
|
||||
if (message_length_expected >= JUMBO_FRAME_MIN_SIZE) {
|
||||
message_checksum = JUMBO_FRAME_MIN_SIZE;
|
||||
} else {
|
||||
message_checksum = message_length_expected;
|
||||
}
|
||||
message_checksum ^= code;
|
||||
if (message_length_expected >= JUMBO_FRAME_MIN_SIZE) {
|
||||
message_checksum ^= message_length_expected & 0xFF;
|
||||
message_checksum ^= (message_length_expected & 0xFF00) >> 8;
|
||||
}
|
||||
for (int ii = 0; ii < message_length_received; ii++) {
|
||||
message_checksum ^= message_buffer[ii];
|
||||
}
|
||||
if (message_checksum == data[i]) {
|
||||
processMessage();
|
||||
}
|
||||
decoderState = DS_IDLE;
|
||||
break;
|
||||
|
||||
case DS_CHECKSUM_V2:
|
||||
message_checksum = 0;
|
||||
message_checksum = crc8_dvb_s2(message_checksum, 0); // flag
|
||||
message_checksum = crc8_dvb_s2(message_checksum, code & 0xFF);
|
||||
message_checksum = crc8_dvb_s2(message_checksum, (code & 0xFF00) >> 8);
|
||||
message_checksum = crc8_dvb_s2(message_checksum, message_length_expected & 0xFF);
|
||||
message_checksum = crc8_dvb_s2(message_checksum, (message_length_expected & 0xFF00) >> 8);
|
||||
for (int ii = 0; ii < message_length_received; ii++) {
|
||||
message_checksum = crc8_dvb_s2(message_checksum, message_buffer[ii]);
|
||||
}
|
||||
if (message_checksum == data[i]) {
|
||||
processMessage();
|
||||
}
|
||||
decoderState = DS_IDLE;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void serialProxyProcess(void)
|
||||
{
|
||||
|
||||
if (!started) return;
|
||||
if (!connected) return;
|
||||
|
||||
if ((millis() - lastVisit) > 500) {
|
||||
if ( lastVisit > 0 ) {
|
||||
fprintf(stderr, "timeout=%dms\n", millis() - lastVisit);
|
||||
}
|
||||
}
|
||||
lastVisit = millis();
|
||||
|
||||
if ( serialFCProxy ) {
|
||||
//first we have to change FC min_check and max_check thresholds to avoid activating stick commands on proxy FC
|
||||
if ( rxConfigState == RXC_IDLE ) {
|
||||
requestRXConfigState();
|
||||
} else if ( rxConfigState == RXC_DONE ) {
|
||||
if ( (nextProxyRequestTimeout <= millis()) || ((reqCount == 0) && (nextProxyRequestMin <= millis()))) {
|
||||
nextProxyRequestTimeout = millis() + FC_PROXY_REQUEST_PERIOD_TIMEOUT_MS;
|
||||
nextProxyRequestMin = millis() + FC_PROXY_REQUEST_PERIOD_MIN_MS;
|
||||
mspRequestChannels();
|
||||
reqCount++;
|
||||
}
|
||||
|
||||
if ( nextProxyRequestRssi <= millis()) {
|
||||
nextProxyRequestRssi = millis() + FC_PROXY_REQUEST_PERIOD_RSSI_MS;
|
||||
mspRequestRssi();
|
||||
reqCount++;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned char buf[SERIAL_BUFFER_SIZE];
|
||||
int count = serialProxyReadData(buf, SERIAL_BUFFER_SIZE);
|
||||
if (count == 0) return;
|
||||
decodeProxyMessages(buf, count);
|
||||
|
||||
} else {
|
||||
|
||||
if ( serialUartIndex == -1 ) return;
|
||||
unsigned char buf[SERIAL_BUFFER_SIZE];
|
||||
uint32_t avail = tcpRXBytesFree(serialUartIndex - 1);
|
||||
if ( avail == 0 ) return;
|
||||
if (avail > SERIAL_BUFFER_SIZE) avail = SERIAL_BUFFER_SIZE;
|
||||
|
||||
int count = serialProxyReadData(buf, avail);
|
||||
if (count == 0) return;
|
||||
|
||||
tcpReceiveBytesEx( serialUartIndex - 1, buf, count);
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
64
src/main/target/SITL/serial_proxy.h
Normal file
64
src/main/target/SITL/serial_proxy.h
Normal file
|
|
@ -0,0 +1,64 @@
|
|||
/*
|
||||
* This file is part of INAV Project.
|
||||
*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
|
||||
* You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*
|
||||
* Alternatively, the contents of this file may be used under the terms
|
||||
* of the GNU General Public License Version 3, as described below:
|
||||
*
|
||||
* This file is free software: you may copy, redistribute and/or modify
|
||||
* it under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or (at your
|
||||
* option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
|
||||
* Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see http://www.gnu.org/licenses/.
|
||||
*/
|
||||
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
|
||||
#include <platform.h>
|
||||
|
||||
#if defined(SITL_BUILD)
|
||||
|
||||
typedef enum
|
||||
{
|
||||
OPT_SERIAL_STOP_BITS_ONE,
|
||||
OPT_SERIAL_STOP_BITS_TWO,
|
||||
OPT_SERIAL_STOP_BITS_INVALID
|
||||
} OptSerialStopBits_e;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
OPT_SERIAL_PARITY_EVEN,
|
||||
OPT_SERIAL_PARITY_NONE,
|
||||
OPT_SERIAL_PARITY_ODD,
|
||||
OPT_SERIAL_PARITY_INVALID
|
||||
} OptSerialParity_e;
|
||||
|
||||
|
||||
extern int serialUartIndex; ///1 for UART1
|
||||
extern char serialPort[64];
|
||||
extern int serialBaudRate;
|
||||
extern OptSerialStopBits_e serialStopBits;
|
||||
extern OptSerialParity_e serialParity;
|
||||
extern bool serialFCProxy;
|
||||
|
||||
extern void serialProxyInit(void);
|
||||
extern void serialProxyStart(void);
|
||||
extern void serialProxyProcess(void);
|
||||
extern void serialProxyClose(void);
|
||||
extern bool serialProxyIsConnected(void);
|
||||
extern bool serialProxyWriteData(unsigned char *buffer, unsigned int nbChar);
|
||||
|
||||
#endif
|
||||
|
|
@ -56,6 +56,8 @@
|
|||
#include "target/SITL/sim/realFlight.h"
|
||||
#include "target/SITL/sim/xplane.h"
|
||||
|
||||
#include "target/SITL/serial_proxy.h"
|
||||
|
||||
// More dummys
|
||||
const int timerHardwareCount = 0;
|
||||
timerHardware_t timerHardware[1];
|
||||
|
|
@ -170,19 +172,47 @@ bool parseMapping(char* mapStr)
|
|||
return true;
|
||||
}
|
||||
|
||||
OptSerialStopBits_e parseStopBits(const char* optarg){
|
||||
if ( strcmp(optarg, "One") == 0 ) {
|
||||
return OPT_SERIAL_STOP_BITS_ONE;
|
||||
} else if ( strcmp(optarg, "Two") == 0 ) {
|
||||
return OPT_SERIAL_STOP_BITS_TWO;
|
||||
} else {
|
||||
return OPT_SERIAL_STOP_BITS_INVALID;
|
||||
}
|
||||
}
|
||||
|
||||
OptSerialParity_e parseParity(const char* optarg){
|
||||
if ( strcmp(optarg, "Even") == 0 ) {
|
||||
return OPT_SERIAL_PARITY_EVEN;
|
||||
} else if ( strcmp(optarg, "None") == 0 ) {
|
||||
return OPT_SERIAL_PARITY_NONE;
|
||||
} else if ( strcmp(optarg, "Odd") == 0 ) {
|
||||
return OPT_SERIAL_PARITY_ODD;
|
||||
} else {
|
||||
return OPT_SERIAL_PARITY_INVALID;
|
||||
}
|
||||
}
|
||||
|
||||
void printCmdLineOptions(void)
|
||||
{
|
||||
printVersion();
|
||||
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, "--sim=[rf|xp] Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n");
|
||||
fprintf(stderr, "--simip=[ip] IP-Address oft the simulator host. If not specified localhost (127.0.0.1) is used.\n");
|
||||
fprintf(stderr, "--simport=[port] Port oft the simulator host.\n");
|
||||
fprintf(stderr, "--useimu Use IMU sensor data from the simulator instead of using attitude data from the simulator directly (experimental, not recommended).\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, " 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, "--path=[path] Path and filename of eeprom.bin. If not specified 'eeprom.bin' in program directory is used.\n");
|
||||
fprintf(stderr, "--sim=[rf|xp] Simulator interface: rf = RealFligt, xp = XPlane. Example: --sim=rf\n");
|
||||
fprintf(stderr, "--simip=[ip] IP-Address oft the simulator host. If not specified localhost (127.0.0.1) is used.\n");
|
||||
fprintf(stderr, "--simport=[port] Port oft the simulator host.\n");
|
||||
fprintf(stderr, "--useimu Use IMU sensor data from the simulator instead of using attitude data from the simulator directly (experimental, not recommended).\n");
|
||||
fprintf(stderr, "--serialuart=[uart] UART number on which serial receiver is configured in SITL, f.e. 3 for UART3\n");
|
||||
fprintf(stderr, "--serialport=[serialport] Host's serial port to which serial receiver/proxy FC is connected, f.e. COM3, /dev/ttyACM3\n");
|
||||
fprintf(stderr, "--baudrate=[baudrate] Serial receiver baudrate (default: 115200).\n");
|
||||
fprintf(stderr, "--stopbits=[None|One|Two] Serial receiver stopbits (default: One).\n");
|
||||
fprintf(stderr, "--parity=[Even|None|Odd] Serial receiver parity (default: None).\n");
|
||||
fprintf(stderr, "--fcproxy Use inav/betaflight FC as a proxy for serial receiver.\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, " 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");
|
||||
}
|
||||
|
||||
void parseArguments(int argc, char *argv[])
|
||||
|
|
@ -202,7 +232,13 @@ void parseArguments(int argc, char *argv[])
|
|||
{"simport", required_argument, 0, 'p'},
|
||||
{"help", no_argument, 0, 'h'},
|
||||
{"path", required_argument, 0, 'e'},
|
||||
{"version", no_argument, 0, 'v'},
|
||||
{"version", no_argument, 0, 'v'},
|
||||
{"serialuart", required_argument, 0, '0'},
|
||||
{"serialport", required_argument, 0, '1'},
|
||||
{"baudrate", required_argument, 0, '2'},
|
||||
{"stopbits", required_argument, 0, '3'},
|
||||
{"parity", required_argument, 0, '4'},
|
||||
{"fcproxy", no_argument, 0, '5'},
|
||||
{NULL, 0, NULL, 0}
|
||||
};
|
||||
|
||||
|
|
@ -242,10 +278,54 @@ void parseArguments(int argc, char *argv[])
|
|||
fprintf(stderr, "[EEPROM] Invalid path, using eeprom file in program directory\n.");
|
||||
}
|
||||
break;
|
||||
case 'v':
|
||||
printVersion();
|
||||
exit(0);
|
||||
default:
|
||||
case 'v':
|
||||
printVersion();
|
||||
exit(0);
|
||||
case '0':
|
||||
serialUartIndex = atoi(optarg);
|
||||
if ( (serialUartIndex<1) || (serialUartIndex>8) ) {
|
||||
fprintf(stderr, "[serialuart] Invalid argument\n.");
|
||||
exit(0);
|
||||
}
|
||||
break;
|
||||
case '1':
|
||||
if ( (strlen(optarg)<1) || (strlen(optarg)>63) ) {
|
||||
fprintf(stderr, "[serialport] Invalid argument\n.");
|
||||
exit(0);
|
||||
} else {
|
||||
strcpy( serialPort, optarg );
|
||||
}
|
||||
break;
|
||||
case '2':
|
||||
serialBaudRate = atoi(optarg);
|
||||
if ( serialBaudRate < 1200 )
|
||||
{
|
||||
fprintf(stderr, "[baudrate] Invalid argument\n.");
|
||||
exit(0);
|
||||
}
|
||||
break;
|
||||
case '3':
|
||||
serialStopBits = parseStopBits(optarg);
|
||||
if ( serialStopBits == OPT_SERIAL_STOP_BITS_INVALID )
|
||||
{
|
||||
fprintf(stderr, "[stopbits] Invalid argument\n.");
|
||||
exit(0);
|
||||
}
|
||||
break;
|
||||
|
||||
case '4':
|
||||
serialParity = parseParity(optarg);
|
||||
if ( serialParity== OPT_SERIAL_PARITY_INVALID )
|
||||
{
|
||||
fprintf(stderr, "[parity] Invalid argument\n.");
|
||||
exit(0);
|
||||
}
|
||||
break;
|
||||
case '5':
|
||||
serialFCProxy = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
printCmdLineOptions();
|
||||
exit(0);
|
||||
}
|
||||
|
|
@ -304,6 +384,7 @@ void systemReset(void)
|
|||
#else
|
||||
closefrom(3);
|
||||
#endif
|
||||
serialProxyClose();
|
||||
execvp(c_argv[0], c_argv); // restart
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue