diff --git a/docs/SITL/SITL.md b/docs/SITL/SITL.md index 0c127135781..df3279f6d31 100644 --- a/docs/SITL/SITL.md +++ b/docs/SITL/SITL.md @@ -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 SITL offers a built-in option for forwarding the host's serial port to the SITL UART. + +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. -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 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. +#### Example SBUS: +For this you need a USB-to-serial adapter, receiver with inverter, or receiver which can output inverted SBUS (normal UART). -### 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. +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. ![SITL-SBUS-FT232](assets/SITL-SBUS-FT232.png) -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``` +![SITL-SBUS](assets/serial_receiver_sbus.png) ### Telemetry +In the SITL configuration, enable serial receiver on some port and configure receiver type "Serial", "SBUS". + +#### 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). + +![SITL-SBUS](assets/serial_receiver_crsf.png) + +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. -LTM and MAVLink telemetry are supported, either as a discrete function or shared with MSP. +You also can use your plane/quad ( if receiver is powered from USB). + +![SITL-SBUS](assets/serial_receiver_proxy.png) + +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 - -### 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 +For INav-X-Plane-HITL plugin: +1. SITL (Run in configurator-only mode) +2. X-Plane -## Supported environments +# #Forwarding serial data for other UART -* 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``` diff --git a/docs/SITL/assets/serial_receiver_crsf.png b/docs/SITL/assets/serial_receiver_crsf.png new file mode 100644 index 00000000000..55ae6af98b6 Binary files /dev/null and b/docs/SITL/assets/serial_receiver_crsf.png differ diff --git a/docs/SITL/assets/serial_receiver_proxy.png b/docs/SITL/assets/serial_receiver_proxy.png new file mode 100644 index 00000000000..6f22f5dbac6 Binary files /dev/null and b/docs/SITL/assets/serial_receiver_proxy.png differ diff --git a/docs/SITL/assets/serial_receiver_sbus.png b/docs/SITL/assets/serial_receiver_sbus.png new file mode 100644 index 00000000000..ce4ebcce169 Binary files /dev/null and b/docs/SITL/assets/serial_receiver_sbus.png differ diff --git a/docs/development/Building SITL.md b/docs/development/Building SITL.md new file mode 100644 index 00000000000..33ba7ee3d5c --- /dev/null +++ b/docs/development/Building SITL.md @@ -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). diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c index 9d4c0262af2..3ca9b9d6fa8 100644 --- a/src/main/drivers/serial_tcp.c +++ b/src/main/drivers/serial_tcp.c @@ -44,6 +44,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]; @@ -119,6 +120,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]; @@ -163,22 +181,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; } @@ -241,9 +249,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) @@ -264,6 +284,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); @@ -273,7 +297,6 @@ uint32_t tcpTotalTxBytesFree(const serialPort_t *instance) bool isTcpTransmitBufferEmpty(const serialPort_t *instance) { UNUSED(instance); - return true; } diff --git a/src/main/drivers/serial_tcp.h b/src/main/drivers/serial_tcp.h index d27610eb805..7f394b8ccd3 100644 --- a/src/main/drivers/serial_tcp.h +++ b/src/main/drivers/serial_tcp.h @@ -26,6 +26,8 @@ #include #include +#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); \ No newline at end of file diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 56f8adcf862..1ffe4a2341f 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -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(); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 74be2f42258..9d3b01d3221 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -94,6 +94,10 @@ #include "config/feature.h" +#if defined(SITL_BUILD) +#include "target/SITL/serial_proxy.h" +#endif + void taskHandleSerial(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); @@ -423,8 +427,8 @@ void fcTasksInit(void) setTaskEnabled(TASK_SMARTPORT_MASTER, true); #endif -#ifdef USE_ADAPTIVE_FILTER - setTaskEnabled(TASK_ADAPTIVE_FILTER, true); +#if defined(SITL_BUILD) + serialProxyStart(); #endif } diff --git a/src/main/main.c b/src/main/main.c index 0488d88df52..aea224210b8 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -26,8 +26,13 @@ #include "fc/fc_init.h" #include "scheduler/scheduler.h" -/* EAL FHTW */ -#include "main.h" + +#include "main.h"//AURIX + +#if defined(SITL_BUILD) +#include "target/SITL/serial_proxy.h" +#endif + boolean INIT_INAV; #ifdef SOFTSERIAL_LOOPBACK @@ -72,6 +77,9 @@ int execute_inav(void) /* EAL FHTW */ while (true) { +#if defined(SITL_BUILD) + serialProxyProcess(); +#endif scheduler(); processLoopback(); } diff --git a/src/main/rx/sim.c b/src/main/rx/sim.c index a2a336f612e..62d9a9b864c 100644 --- a/src/main/rx/sim.c +++ b/src/main/rx/sim.c @@ -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 diff --git a/src/main/rx/sim.h b/src/main/rx/sim.h index 88342acad73..9c8ff36d1b0 100644 --- a/src/main/rx/sim.h +++ b/src/main/rx/sim.h @@ -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); diff --git a/src/main/target/SITL/serial_proxy.c b/src/main/target/SITL/serial_proxy.c new file mode 100644 index 00000000000..281fefd3ede --- /dev/null +++ b/src/main/target/SITL/serial_proxy.c @@ -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 +#include +#include +#include +#include +#include +#include +#include + +#include "platform.h" + +#include +#include +#include +#include +#include + +#include "drivers/time.h" +#include "msp/msp_serial.h" +#include "msp/msp_protocol.h" +#include "common/crc.h" +#include "rx/sim.h" + +#include + +#ifdef __FreeBSD__ +# define __BSD_VISIBLE 1 +#endif + +#ifdef __linux__ +#include +#ifndef TCGETS2 +#include +#endif +#else +#include +#endif + +#ifdef __APPLE__ +#include +#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 +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 diff --git a/src/main/target/SITL/serial_proxy.h b/src/main/target/SITL/serial_proxy.h new file mode 100644 index 00000000000..5cb97516399 --- /dev/null +++ b/src/main/target/SITL/serial_proxy.h @@ -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 +#include +#include + +#include + +#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 \ No newline at end of file diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index e8861eec56d..e473c636981 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -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)-,... 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)-,... 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 }