From b3438919f5fddbfac221affcc48cadbc87464ac9 Mon Sep 17 00:00:00 2001 From: error414 Date: Sat, 16 Mar 2024 10:31:06 +0100 Subject: [PATCH 01/14] new NRA protocol fro radars from nanoradar new rangerfinder NRA15/NRA24 from nanoradar --- docs/Rangefinder.md | 16 +++ src/main/CMakeLists.txt | 3 + src/main/fc/settings.yaml | 2 +- src/main/io/rangefinder.h | 2 + src/main/io/rangefinder_nanoradar.c | 167 ++++++++++++++++++++++++++++ src/main/io/rangefinder_usd1_v0.c | 136 ++++++++++++++++++++++ src/main/sensors/rangefinder.c | 16 +++ src/main/sensors/rangefinder.h | 2 + src/main/target/common.h | 2 + 9 files changed, 345 insertions(+), 1 deletion(-) create mode 100644 src/main/io/rangefinder_nanoradar.c create mode 100644 src/main/io/rangefinder_usd1_v0.c diff --git a/docs/Rangefinder.md b/docs/Rangefinder.md index 095a6f0df33..813184f8374 100644 --- a/docs/Rangefinder.md +++ b/docs/Rangefinder.md @@ -21,6 +21,22 @@ Following rangefinders are supported: * MSP - experimental * TOF10120 - small & lightweight laser range sensor, usable up to 200cm * TERARANGER EVO - 30cm to 600cm, depends on version https://www.terabee.com/sensors-modules/lidar-tof-range-finders/#individual-distance-measurement-sensors +* NRA15/NRA24 - experimental, UART version + +#### NRA15/NRA24 +NRA15/NRA24 from nanoradar use US-D1_V0 or NRA protocol, it depends which firmware you use. Radar can be set by firmware +to two different resolutions. See table below. + +| Radar | Protocol | Resolution | Name in configurator | +|-------|----------|-----------------|-----------------------| +| NRA15 | US-D1_V0 | 0-30m (+-4cm) | USD1_V0 | +| NRA15 | NRA | 0-30m (+-4cm) | NRA15/NRA24 | +| NRA15 | NRA | 0-100m (+-10cm) | NRA15/NRA24 | +| NRA24 | US-D1_V0 | 0-50m (+-4cm) | USD1_V0 | +| NRA24 | US-D1_V0 | 0-200m (+-10cm) | USD1_V0 | +| NRA24 | NRA | 0-50m (+-4cm) | NRA15/NRA24 | +| NRA24 | NRA | 0-200m (+-10cm) | NRA15/NRA24 | + ## Connections diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index cba13db687e..28c068966f8 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -1,3 +1,4 @@ + main_sources(COMMON_SRC main.c @@ -485,6 +486,8 @@ main_sources(COMMON_SRC io/rangefinder.h io/rangefinder_msp.c io/rangefinder_benewake.c + io/rangefinder_usd1_v0.c + io/rangefinder_nanoradar.c io/rangefinder_fake.c io/opflow.h io/opflow_cxof.c diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index cb586eacf06..b2be345391d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -5,7 +5,7 @@ tables: values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware - values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO"] + values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA15/NRA24"] enum: rangefinderType_e - name: mag_hardware values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"] diff --git a/src/main/io/rangefinder.h b/src/main/io/rangefinder.h index 6768c7d75d8..fd51aeb44aa 100644 --- a/src/main/io/rangefinder.h +++ b/src/main/io/rangefinder.h @@ -31,6 +31,8 @@ extern virtualRangefinderVTable_t rangefinderMSPVtable; extern virtualRangefinderVTable_t rangefinderBenewakeVtable; +extern virtualRangefinderVTable_t rangefinderUSD1Vtable; +extern virtualRangefinderVTable_t rangefinderNanoradarVtable; //NRA15/NRA24 extern virtualRangefinderVTable_t rangefinderFakeVtable; void mspRangefinderReceiveNewData(uint8_t * bufferPtr); diff --git a/src/main/io/rangefinder_nanoradar.c b/src/main/io/rangefinder_nanoradar.c new file mode 100644 index 00000000000..f6978448ea4 --- /dev/null +++ b/src/main/io/rangefinder_nanoradar.c @@ -0,0 +1,167 @@ +/* + * 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 "platform.h" +#include "io/serial.h" +#include "drivers/time.h" + +#if defined(USE_RANGEFINDER_NANORADAR) +#include "drivers/rangefinder/rangefinder_virtual.h" + +#define NANORADAR_HDR 0xAA // Header Byte +#define NANORADAR_END 0x55 + +#define NANORADAR_COMMAND_TARGET_INFO 0x70C + +typedef struct __attribute__((packed)) { + uint8_t header0; + uint8_t header1; + uint8_t commandH; + uint8_t commandL; + uint8_t index; // Target ID + uint8_t rcs; // The section of radar reflection + uint8_t rangeH; // Target distance high 8 bi + uint8_t rangeL; // Target distance low 8 bit + uint8_t rsvd1; + uint8_t info; // VrelH Rsvd1 RollCount + uint8_t vrelL; + uint8_t SNR; // Signal-Noise Ratio + uint8_t end0; + uint8_t end1; +} nanoradarPacket_t; + +#define NANORADAR_PACKET_SIZE sizeof(nanoradarPacket_t) +#define NANORADAR_TIMEOUT_MS 2000 // 2s + +static bool hasNewData = false; +static bool hasEverData = false; +static serialPort_t * serialPort = NULL; +static serialPortConfig_t * portConfig; +static uint8_t buffer[NANORADAR_PACKET_SIZE]; +static unsigned bufferPtr; + +static int32_t sensorData = RANGEFINDER_NO_NEW_DATA; +static timeMs_t lastProtocolActivityMs; + +static bool nanoradarRangefinderDetect(void) +{ + portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER); + if (!portConfig) { + return false; + } + + return true; +} + +static void nanoradarRangefinderInit(void) +{ + if (!portConfig) { + return; + } + + serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED); + if (!serialPort) { + return; + } + + lastProtocolActivityMs = 0; +} + +static void nanoradarRangefinderUpdate(void) +{ + + nanoradarPacket_t *nanoradarPacket = (nanoradarPacket_t *)buffer; + while (serialRxBytesWaiting(serialPort) > 0) { + uint8_t c = serialRead(serialPort); + + if (bufferPtr < NANORADAR_PACKET_SIZE) { + buffer[bufferPtr++] = c; + } + + if ((bufferPtr == 1) && (nanoradarPacket->header0 != NANORADAR_HDR)) { + bufferPtr = 0; + continue; + } + + if ((bufferPtr == 2) && (nanoradarPacket->header1 != NANORADAR_HDR)) { + bufferPtr = 0; + continue; + } + + //only target info packet we are interested + if (bufferPtr == 4) { + uint16_t command = nanoradarPacket->commandH + (nanoradarPacket->commandL * 0x100); + + if (command != NANORADAR_COMMAND_TARGET_INFO) { + bufferPtr = 0; + continue; + } + } + + // Check for complete packet + if (bufferPtr == NANORADAR_PACKET_SIZE) { + if (nanoradarPacket->end0 == NANORADAR_END && nanoradarPacket->end1 == NANORADAR_END) { + // Valid packet + hasNewData = true; + hasEverData = true; + lastProtocolActivityMs = millis(); + + sensorData = ((nanoradarPacket->rangeH * 0x100) + nanoradarPacket->rangeL); + } + + // Prepare for new packet + bufferPtr = 0; + } + } +} + +static int32_t nanoradarRangefinderGetDistance(void) +{ + int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE; + + if (hasNewData) { + hasNewData = false; + return altitude; + } + else { + //keep last value for timeout, because radar sends data only if change + if ((millis() - lastProtocolActivityMs) < NANORADAR_TIMEOUT_MS) { + return altitude; + } + + return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA; + } +} + +virtualRangefinderVTable_t rangefinderNanoradarVtable = { + .detect = nanoradarRangefinderDetect, + .init = nanoradarRangefinderInit, + .update = nanoradarRangefinderUpdate, + .read = nanoradarRangefinderGetDistance +}; + +#endif diff --git a/src/main/io/rangefinder_usd1_v0.c b/src/main/io/rangefinder_usd1_v0.c new file mode 100644 index 00000000000..a509d4096c2 --- /dev/null +++ b/src/main/io/rangefinder_usd1_v0.c @@ -0,0 +1,136 @@ +/* + * 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 "platform.h" +#include "io/serial.h" +#include "drivers/time.h" + +#if defined(USE_RANGEFINDER_USD1_V0) +#include "drivers/rangefinder/rangefinder_virtual.h" + +#define USD1_HDR_V0 72 // Header Byte for beta V0 of USD1_Serial (0x48) + +#define USD1_PACKET_SIZE 3 +#define USD1_KEEP_DATA_TIMEOUT 2000 // 2s + + +static serialPort_t * serialPort = NULL; +static serialPortConfig_t * portConfig; + +static bool hasNewData = false; +static bool hasEverData = false; +static uint8_t lineBuf[USD1_PACKET_SIZE]; +static int32_t sensorData = RANGEFINDER_NO_NEW_DATA; +static timeMs_t lastProtocolActivityMs; + +static bool usd1RangefinderDetect(void) +{ + portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER); + if (!portConfig) { + return false; + } + + return true; +} + +static void usd1RangefinderInit(void) +{ + if (!portConfig) { + return; + } + + serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED); + if (!serialPort) { + return; + } + + lastProtocolActivityMs = 0; +} + +static void usd1RangefinderUpdate(void) +{ + float sum = 0; + uint16_t count = 0; + uint8_t index = 0; + + while (serialRxBytesWaiting(serialPort) > 0) { + uint8_t c = serialRead(serialPort); + + if (c == USD1_HDR_V0 && index == 0) { + lineBuf[index] = c; + index = 1; + continue; + } + + if (index > 0) { + lineBuf[index] = c; + index++; + if (index == 3) { + index = 0; + sum += (float)((lineBuf[2]&0x7F) * 128 + (lineBuf[1]&0x7F)); + count++; + } + } + } + + if (count == 0) { + return; + } + + hasNewData = true; + hasEverData = true; + lastProtocolActivityMs = millis(); + sensorData = (int32_t)(2.5f * sum / (float)count); +} + +static int32_t usd1RangefinderGetDistance(void) +{ + int32_t altitude = (sensorData > 0) ? (sensorData) : RANGEFINDER_OUT_OF_RANGE; + + if (hasNewData) { + hasNewData = false; + return altitude; + } + else { + //keep last value for timeout, because radar sends data only if change + if ((millis() - lastProtocolActivityMs) < USD1_KEEP_DATA_TIMEOUT) { + return altitude; + } + + return hasEverData ? RANGEFINDER_OUT_OF_RANGE : RANGEFINDER_NO_NEW_DATA; + } +} + +virtualRangefinderVTable_t rangefinderUSD1Vtable = { + .detect = usd1RangefinderDetect, + .init = usd1RangefinderInit, + .update = usd1RangefinderUpdate, + .read = usd1RangefinderGetDistance +}; + +#endif diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 88f027f1545..a721bb98634 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -130,6 +130,22 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar rangefinderHardware = RANGEFINDER_BENEWAKE; rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS)); } +#endif + break; + case RANGEFINDER_USD1_V0: +#if defined(USE_RANGEFINDER_USD1_V0) + if (virtualRangefinderDetect(dev, &rangefinderUSD1Vtable)) { + rangefinderHardware = RANGEFINDER_USD1_V0; + rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS)); + } +#endif + break; + case RANGEFINDER_NANORADAR: +#if defined(USE_RANGEFINDER_NANORADAR) + if (virtualRangefinderDetect(dev, &rangefinderNanoradarVtable)) { + rangefinderHardware = RANGEFINDER_NANORADAR; + rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VIRTUAL_TASK_PERIOD_MS)); + } #endif break; diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index 2888e7838af..4c2a4e629aa 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -32,6 +32,8 @@ typedef enum { RANGEFINDER_TOF10102I2C = 7, RANGEFINDER_FAKE = 8, RANGEFINDER_TERARANGER_EVO = 9, + RANGEFINDER_USD1_V0 = 10, + RANGEFINDER_NANORADAR = 11, } rangefinderType_e; typedef struct rangefinderConfig_s { diff --git a/src/main/target/common.h b/src/main/target/common.h index d146ebb9790..b550df42d0f 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -84,6 +84,8 @@ #define USE_RANGEFINDER_US42 #define USE_RANGEFINDER_TOF10120_I2C #define USE_RANGEFINDER_TERARANGER_EVO_I2C +#define USE_RANGEFINDER_USD1_V0 +#define USE_RANGEFINDER_NANORADAR // Allow default optic flow boards #define USE_OPFLOW From 81b1f03325698313be0f167318b406d144a3ffc6 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 13 Apr 2024 23:48:29 +0200 Subject: [PATCH 02/14] Use 2 bits for msp displayport font page --- src/main/io/displayport_msp_bf_compat.c | 2 +- src/main/io/displayport_msp_osd.c | 19 +++++++++---------- 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index 7067ac140f5..4219d2b2db8 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -27,7 +27,7 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) { - uint16_t ech = ch | (page << 8); + uint16_t ech = ch | ((page & 0x3)<< 8) ; if (ech >= 0x20 && ech <= 0x5F) { // ASCII range return ch; diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index e1e6cb29676..56dbc723439 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -100,7 +100,7 @@ static timeMs_t sendSubFrameMs = 0; static uint8_t currentOsdMode; // HDZero screen mode can change across layouts static uint8_t screen[SCREENSIZE]; -static BITARRAY_DECLARE(fontPage, SCREENSIZE); // font page for each character on the screen +static uint8_t fontPage[SCREENSIZE]; // font page for each character on the screen static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character on the screen static BITARRAY_DECLARE(blinkChar, SCREENSIZE); // Does the character blink? static bool screenCleared; @@ -171,7 +171,7 @@ static int setDisplayMode(displayPort_t *displayPort) static void init(void) { memset(screen, SYM_BLANK, sizeof(screen)); - BITARRAY_CLR_ALL(fontPage); + memset(fontPage, 0, sizeof(fontPage)); BITARRAY_CLR_ALL(dirty); BITARRAY_CLR_ALL(blinkChar); } @@ -204,9 +204,8 @@ static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint1 } *c = screen[pos]; - if (bitArrayGet(fontPage, pos)) { - *c |= 0x100; - } + uint8_t page = fontPage[pos]; + *c |= (page & 0x3) << 8; if (attr) { *attr = TEXT_ATTRIBUTES_NONE; @@ -219,10 +218,10 @@ static int setChar(const uint16_t pos, const uint16_t c, textAttributes_t attr) { if (pos < SCREENSIZE) { uint8_t ch = c & 0xFF; - bool page = (c >> 8); - if (screen[pos] != ch || bitArrayGet(fontPage, pos) != page) { + uint8_t page = (c >> 8) & 0x3; + if (screen[pos] != ch || fontPage[pos] != page) { screen[pos] = ch; - (page) ? bitArraySet(fontPage, pos) : bitArrayClr(fontPage, pos); + fontPage[pos] = page & 0x3; (TEXT_ATTRIBUTES_HAVE_BLINK(attr)) ? bitArraySet(blinkChar, pos) : bitArrayClr(blinkChar, pos); bitArraySet(dirty, pos); } @@ -287,7 +286,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz uint8_t col = pos % COLS; uint8_t attributes = 0; int endOfLine = row * COLS + screenCols; - bool page = bitArrayGet(fontPage, pos); + uint8_t page = fontPage[pos]; bool blink = bitArrayGet(blinkChar, pos); uint8_t len = 4; @@ -299,7 +298,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz if (bitArrayGet(dirty, pos)) { next = pos; } - } while (next == pos && next < endOfLine && bitArrayGet(fontPage, next) == page && bitArrayGet(blinkChar, next) == blink); + } while (next == pos && next < endOfLine && fontPage[next] == page && bitArrayGet(blinkChar, next) == blink); if (!isBfCompatibleVideoSystem(osdConfig())) { attributes |= (page << DISPLAYPORT_MSP_ATTR_FONTPAGE); From 9da168d5dcc6339eb8cc105b93ec2c6e46c3c6ea Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Apr 2024 16:12:38 +0200 Subject: [PATCH 03/14] Merge blink bitarry into attr array --- src/main/io/displayport_msp_osd.c | 41 +++++++++++++++++++++---------- src/main/io/displayport_msp_osd.h | 20 ++++++++++++--- 2 files changed, 44 insertions(+), 17 deletions(-) diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index 56dbc723439..ab88ec661fb 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -100,9 +100,8 @@ static timeMs_t sendSubFrameMs = 0; static uint8_t currentOsdMode; // HDZero screen mode can change across layouts static uint8_t screen[SCREENSIZE]; -static uint8_t fontPage[SCREENSIZE]; // font page for each character on the screen +static uint8_t attrs[SCREENSIZE]; // font page for each character on the screen static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character on the screen -static BITARRAY_DECLARE(blinkChar, SCREENSIZE); // Does the character blink? static bool screenCleared; static uint8_t screenRows, screenCols; static videoSystem_e osdVideoSystem; @@ -158,6 +157,22 @@ static uint8_t determineHDZeroOsdMode(void) return HD_3016; } + +uint8_t setAttrPage(uint8_t origAttr, uint8_t page) +{ + return (origAttr & ~DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK) | (page & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK); +} + +uint8_t setAttrBlink(uint8_t origAttr, uint8_t blink) +{ + return (origAttr & ~DISPLAYPORT_MSP_ATTR_BLINK_MASK) | ((blink << DISPLAYPORT_MSP_ATTR_BLINK) & DISPLAYPORT_MSP_ATTR_BLINK_MASK); +} + +uint8_t setAttrVersion(uint8_t origAttr, uint8_t version) +{ + return (origAttr & ~DISPLAYPORT_MSP_ATTR_VERSION_MASK) | ((version << DISPLAYPORT_MSP_ATTR_VERSION) & DISPLAYPORT_MSP_ATTR_VERSION_MASK); +} + static int setDisplayMode(displayPort_t *displayPort) { if (osdVideoSystem == VIDEO_SYSTEM_HDZERO) { @@ -171,9 +186,8 @@ static int setDisplayMode(displayPort_t *displayPort) static void init(void) { memset(screen, SYM_BLANK, sizeof(screen)); - memset(fontPage, 0, sizeof(fontPage)); + memset(attrs, 0, sizeof(attrs)); BITARRAY_CLR_ALL(dirty); - BITARRAY_CLR_ALL(blinkChar); } static int clearScreen(displayPort_t *displayPort) @@ -204,8 +218,8 @@ static bool readChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint1 } *c = screen[pos]; - uint8_t page = fontPage[pos]; - *c |= (page & 0x3) << 8; + uint8_t page = getAttrPage(attrs[pos]); + *c |= page << 8; if (attr) { *attr = TEXT_ATTRIBUTES_NONE; @@ -218,11 +232,12 @@ static int setChar(const uint16_t pos, const uint16_t c, textAttributes_t attr) { if (pos < SCREENSIZE) { uint8_t ch = c & 0xFF; - uint8_t page = (c >> 8) & 0x3; - if (screen[pos] != ch || fontPage[pos] != page) { + uint8_t page = (c >> 8) & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK; + if (screen[pos] != ch || getAttrPage(attrs[pos]) != page) { screen[pos] = ch; - fontPage[pos] = page & 0x3; - (TEXT_ATTRIBUTES_HAVE_BLINK(attr)) ? bitArraySet(blinkChar, pos) : bitArrayClr(blinkChar, pos); + attrs[pos] = setAttrPage(attrs[pos], page); + uint8_t blink = (TEXT_ATTRIBUTES_HAVE_BLINK(attr)) ? 1 : 0; + attrs[pos] = setAttrBlink(attrs[pos], blink); bitArraySet(dirty, pos); } } @@ -286,8 +301,8 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz uint8_t col = pos % COLS; uint8_t attributes = 0; int endOfLine = row * COLS + screenCols; - uint8_t page = fontPage[pos]; - bool blink = bitArrayGet(blinkChar, pos); + uint8_t page = getAttrPage(attrs[pos]); + uint8_t blink = getAttrBlink(attrs[pos]); uint8_t len = 4; do { @@ -298,7 +313,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz if (bitArrayGet(dirty, pos)) { next = pos; } - } while (next == pos && next < endOfLine && fontPage[next] == page && bitArrayGet(blinkChar, next) == blink); + } while (next == pos && next < endOfLine && getAttrPage(attrs[next]) == page && getAttrBlink(attrs[next]) == blink); if (!isBfCompatibleVideoSystem(osdConfig())) { attributes |= (page << DISPLAYPORT_MSP_ATTR_FONTPAGE); diff --git a/src/main/io/displayport_msp_osd.h b/src/main/io/displayport_msp_osd.h index 0a2f64c48a2..affd5b39be5 100644 --- a/src/main/io/displayport_msp_osd.h +++ b/src/main/io/displayport_msp_osd.h @@ -27,13 +27,25 @@ #include "drivers/osd.h" #include "msp/msp_serial.h" +// MSP displayport V2 attribute byte bit functions +#define DISPLAYPORT_MSP_ATTR_FONTPAGE 0 // Select bank of 256 characters as per displayPortSeverity_e +#define DISPLAYPORT_MSP_ATTR_BLINK 6 // Device local blink +#define DISPLAYPORT_MSP_ATTR_VERSION 7 // Format indicator; must be zero for V2 (and V1) + +#define DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK 0x3 +#define DISPLAYPORT_MSP_ATTR_BLINK_MASK (1 << DISPLAYPORT_MSP_ATTR_BLINK) +#define DISPLAYPORT_MSP_ATTR_VERSION_MASK (1 << DISPLAYPORT_MSP_ATTR_VERSION) + typedef struct displayPort_s displayPort_t; displayPort_t *mspOsdDisplayPortInit(const videoSystem_e videoSystem); void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn); mspPort_t *getMspOsdPort(void); -// MSP displayport V2 attribute byte bit functions -#define DISPLAYPORT_MSP_ATTR_FONTPAGE 0 // Select bank of 256 characters as per displayPortSeverity_e -#define DISPLAYPORT_MSP_ATTR_BLINK 6 // Device local blink -#define DISPLAYPORT_MSP_ATTR_VERSION 7 // Format indicator; must be zero for V2 (and V1) \ No newline at end of file +#define getAttrPage(attr) (attr & DISPLAYPORT_MSP_ATTR_FONTPAGE_MASK) +#define getAttrBlink(attr) ((attr & DISPLAYPORT_MSP_ATTR_BLINK_MASK) >> DISPLAYPORT_MSP_ATTR_BLINK) +#define getAttrVersion(attr) ((attr & DISPLAYPORT_MSP_ATTR_VERSION_MASK) >> DISPLAYPORT_MSP_ATTR_VERSION) + +uint8_t setAttrPage(uint8_t origAttr, uint8_t page); +uint8_t setAttrBlink(uint8_t origAttr, uint8_t page); +uint8_t setAttrVersion(uint8_t origAttr, uint8_t page); \ No newline at end of file From 1ee021b5a8cf3dc83afe52347ea6ef8ec71922a6 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 14 Apr 2024 16:16:58 +0200 Subject: [PATCH 04/14] Formating changes --- src/main/io/displayport_msp_osd.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index ab88ec661fb..c9372d9fbf9 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -59,7 +59,7 @@ typedef enum { // defines are from hdzero code SD_3016, HD_5018, HD_3016, // Special HDZERO mode that just sends the centre 30x16 of the 50x18 canvas to the VRX - HD_6022, // added to support DJI wtfos 60x22 grid + HD_6022, // added to support DJI wtfos 60x22 grid HD_5320 // added to support Avatar and BetaflightHD } resolutionType_e; @@ -97,11 +97,11 @@ static timeMs_t sendSubFrameMs = 0; // set screen size #define SCREENSIZE (ROWS*COLS) -static uint8_t currentOsdMode; // HDZero screen mode can change across layouts +static uint8_t currentOsdMode; // HDZero screen mode can change across layouts static uint8_t screen[SCREENSIZE]; -static uint8_t attrs[SCREENSIZE]; // font page for each character on the screen -static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character on the screen +static uint8_t attrs[SCREENSIZE]; // font page, blink and other attributes +static BITARRAY_DECLARE(dirty, SCREENSIZE); // change status for each character on the screen static bool screenCleared; static uint8_t screenRows, screenCols; static videoSystem_e osdVideoSystem; From 09ed11ca35a7cd09741c17723287348005eb0bf5 Mon Sep 17 00:00:00 2001 From: error414 Date: Wed, 17 Apr 2024 21:34:31 +0200 Subject: [PATCH 05/14] rename NRA15/NRA24 only to NRA --- docs/Rangefinder.md | 18 +++++++++--------- src/main/fc/settings.yaml | 2 +- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/docs/Rangefinder.md b/docs/Rangefinder.md index 813184f8374..b53ae439b7e 100644 --- a/docs/Rangefinder.md +++ b/docs/Rangefinder.md @@ -27,15 +27,15 @@ Following rangefinders are supported: NRA15/NRA24 from nanoradar use US-D1_V0 or NRA protocol, it depends which firmware you use. Radar can be set by firmware to two different resolutions. See table below. -| Radar | Protocol | Resolution | Name in configurator | -|-------|----------|-----------------|-----------------------| -| NRA15 | US-D1_V0 | 0-30m (+-4cm) | USD1_V0 | -| NRA15 | NRA | 0-30m (+-4cm) | NRA15/NRA24 | -| NRA15 | NRA | 0-100m (+-10cm) | NRA15/NRA24 | -| NRA24 | US-D1_V0 | 0-50m (+-4cm) | USD1_V0 | -| NRA24 | US-D1_V0 | 0-200m (+-10cm) | USD1_V0 | -| NRA24 | NRA | 0-50m (+-4cm) | NRA15/NRA24 | -| NRA24 | NRA | 0-200m (+-10cm) | NRA15/NRA24 | +| Radar | Protocol | Resolution | Name in configurator | +|-------|----------|-----------------|----------------------| +| NRA15 | US-D1_V0 | 0-30m (+-4cm) | USD1_V0 | +| NRA15 | NRA | 0-30m (+-4cm) | NRA | +| NRA15 | NRA | 0-100m (+-10cm) | NRA | +| NRA24 | US-D1_V0 | 0-50m (+-4cm) | USD1_V0 | +| NRA24 | US-D1_V0 | 0-200m (+-10cm) | USD1_V0 | +| NRA24 | NRA | 0-50m (+-4cm) | NRA | +| NRA24 | NRA | 0-200m (+-10cm) | NRA | ## Connections diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b2be345391d..11658135417 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -5,7 +5,7 @@ tables: values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware - values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA15/NRA24"] + values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"] enum: rangefinderType_e - name: mag_hardware values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"] From a53296abb7d9aa10f850b34aae11cc142db6aab3 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 27 Apr 2024 13:11:40 +0200 Subject: [PATCH 06/14] Fix function prototypes and macos warnings --- src/main/flight/mixer.c | 2 +- src/main/flight/mixer_profile.c | 2 +- src/main/io/displayport_msp_osd.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d2a073f26ee..716b48d7aa4 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -728,7 +728,7 @@ bool areMotorsRunning(void) return false; } -uint16_t getMaxThrottle() { +uint16_t getMaxThrottle(void) { static uint16_t throttle = 0; diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 7b2590ff70b..5769b56fbdb 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -79,7 +79,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) } } -void activateMixerConfig(){ +void activateMixerConfig(void){ currentMixerProfileIndex = getConfigMixerProfile(); currentMixerConfig = *mixerConfig(); nextMixerProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index e1e6cb29676..0d797e1f756 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -525,7 +525,7 @@ void mspOsdSerialProcess(mspProcessCommandFnPtr mspProcessCommandFn) } } -mspPort_t *getMspOsdPort() +mspPort_t *getMspOsdPort(void) { if (mspPort.port) { return &mspPort; From 30d96107bc6a506e4816375a0e08e68936feee67 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 27 Apr 2024 13:19:12 +0200 Subject: [PATCH 07/14] macos-latest got bumped to macos-14 and introduces some new failures on sitl Move it back to macos-13 until it can be updated back to latest. --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 09e27045ca6..6fddc02df85 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -74,7 +74,7 @@ jobs: path: ./build_SITL/*_SITL build-SITL-Mac: - runs-on: macos-latest + runs-on: macos-13 steps: - uses: actions/checkout@v3 - name: Install dependencies From cc276acef9070b2a19a64c75dd7e35098374022b Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 27 Apr 2024 13:56:54 +0200 Subject: [PATCH 08/14] More warnings, last of the alignment errors --- src/main/build/build_config.h | 2 +- src/main/cms/cms_types.h | 8 +++++++- src/main/config/parameter_group.h | 2 +- src/main/fc/cli.c | 2 +- src/main/io/osd_hud.c | 4 ++-- 5 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/build/build_config.h b/src/main/build/build_config.h index d97e6d300ce..75bb46668d7 100644 --- a/src/main/build/build_config.h +++ b/src/main/build/build_config.h @@ -40,7 +40,7 @@ #endif #ifdef __APPLE__ -#define FASTRAM __attribute__ ((section("__DATA,__.fastram_bss"), aligned(4))) +#define FASTRAM __attribute__ ((section("__DATA,__.fastram_bss"), aligned(8))) #else #define FASTRAM __attribute__ ((section(".fastram_bss"), aligned(4))) #endif diff --git a/src/main/cms/cms_types.h b/src/main/cms/cms_types.h index 9b2b06646c5..a07e55eaaa4 100644 --- a/src/main/cms/cms_types.h +++ b/src/main/cms/cms_types.h @@ -26,6 +26,12 @@ //type of elements +#ifndef __APPLE__ +#define OSD_ENTRY_ATTR __attribute__((packed)) +#else +#define OSD_ENTRY_ATTR +#endif + typedef enum { OME_Label, @@ -71,7 +77,7 @@ typedef struct const void * const data; const uint8_t type; // from OSD_MenuElement uint8_t flags; -} __attribute__((packed)) OSD_Entry; +} OSD_ENTRY_ATTR OSD_Entry; // Bits in flags #define PRINT_VALUE (1 << 0) // Value has been changed, need to redraw diff --git a/src/main/config/parameter_group.h b/src/main/config/parameter_group.h index 9594ad96c41..28a647ecb96 100644 --- a/src/main/config/parameter_group.h +++ b/src/main/config/parameter_group.h @@ -64,7 +64,7 @@ static inline uint16_t pgIsProfile(const pgRegistry_t* reg) {return (reg->size & #ifdef __APPLE__ extern const pgRegistry_t __pg_registry_start[] __asm("section$start$__DATA$__pg_registry"); extern const pgRegistry_t __pg_registry_end[] __asm("section$end$__DATA$__pg_registry"); -#define PG_REGISTER_ATTRIBUTES __attribute__ ((section("__DATA,__pg_registry"), used, aligned(4))) +#define PG_REGISTER_ATTRIBUTES __attribute__ ((section("__DATA,__pg_registry"), used, aligned(8))) extern const uint8_t __pg_resetdata_start[] __asm("section$start$__DATA$__pg_resetdata"); extern const uint8_t __pg_resetdata_end[] __asm("section$end$__DATA$__pg_resetdata"); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 48345583412..97de1bd6ed1 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1171,7 +1171,7 @@ static void cliRxRange(char *cmdline) ptr = cmdline; i = fastA2I(ptr); if (i >= 0 && i < NON_AUX_CHANNEL_COUNT) { - int rangeMin, rangeMax; + int rangeMin = 0, rangeMax = 0; ptr = nextArg(ptr); if (ptr) { diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c index 47cc96f834b..07bc181d1c1 100644 --- a/src/main/io/osd_hud.c +++ b/src/main/io/osd_hud.c @@ -122,8 +122,8 @@ int8_t radarGetNearestPOI(void) */ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitude, uint8_t poiType, uint16_t poiSymbol, int16_t poiP1, int16_t poiP2) { - int poi_x; - int poi_y; + int poi_x = -1; + int poi_y = -1; uint8_t center_x; uint8_t center_y; bool poi_is_oos = 0; From 8256875af9e297df0dd0895b73c5d0243bb44383 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 27 Apr 2024 14:01:53 +0200 Subject: [PATCH 09/14] Back to latest macos --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 6fddc02df85..09e27045ca6 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -74,7 +74,7 @@ jobs: path: ./build_SITL/*_SITL build-SITL-Mac: - runs-on: macos-13 + runs-on: macos-latest steps: - uses: actions/checkout@v3 - name: Install dependencies From a030a2d06787ef81c601e9f940f3d78ab7633743 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 27 Apr 2024 15:42:07 +0200 Subject: [PATCH 10/14] Add separate x64 build --- .github/workflows/ci.yml | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 09e27045ca6..4ff42bfa935 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -109,6 +109,42 @@ jobs: name: ${{ env.BUILD_NAME }}_SITL-MacOS path: ./build_SITL/*_SITL + build-SITL-Mac-large: + runs-on: macos-latest-large + steps: + - uses: actions/checkout@v3 + - name: Install dependencies + run: | + brew install cmake ninja ruby + + - name: Setup environment + env: + ACTIONS_ALLOW_UNSECURE_COMMANDS: true + run: | + # This is the hash of the commit for the PR + # when the action is triggered by PR, empty otherwise + COMMIT_ID=${{ github.event.pull_request.head.sha }} + # This is the hash of the commit when triggered by push + # but the hash of refs/pull//merge, which is different + # from the hash of the latest commit in the PR, that's + # why we try github.event.pull_request.head.sha first + COMMIT_ID=${COMMIT_ID:-${{ github.sha }}} + BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID}) + VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }') + echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV + echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV + - name: Build SITL + run: | + mkdir -p build_SITL && cd build_SITL + cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. + ninja + + - name: Upload artifacts + uses: actions/upload-artifact@v3 + with: + name: ${{ env.BUILD_NAME }}_SITL-MacOS-x64 + path: ./build_SITL/*_SITL + build-SITL-Windows: runs-on: windows-latest defaults: From f6a86f80f978c24c7e5bcca5c21c63091d3166ef Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 27 Apr 2024 15:47:06 +0200 Subject: [PATCH 11/14] Build MacOSX sitl as universal binary --- .github/workflows/ci.yml | 38 +------------------------------------- 1 file changed, 1 insertion(+), 37 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4ff42bfa935..111a0cc35c0 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -100,7 +100,7 @@ jobs: - name: Build SITL run: | mkdir -p build_SITL && cd build_SITL - cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. + cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -G Ninja .. ninja - name: Upload artifacts @@ -109,42 +109,6 @@ jobs: name: ${{ env.BUILD_NAME }}_SITL-MacOS path: ./build_SITL/*_SITL - build-SITL-Mac-large: - runs-on: macos-latest-large - steps: - - uses: actions/checkout@v3 - - name: Install dependencies - run: | - brew install cmake ninja ruby - - - name: Setup environment - env: - ACTIONS_ALLOW_UNSECURE_COMMANDS: true - run: | - # This is the hash of the commit for the PR - # when the action is triggered by PR, empty otherwise - COMMIT_ID=${{ github.event.pull_request.head.sha }} - # This is the hash of the commit when triggered by push - # but the hash of refs/pull//merge, which is different - # from the hash of the latest commit in the PR, that's - # why we try github.event.pull_request.head.sha first - COMMIT_ID=${COMMIT_ID:-${{ github.sha }}} - BUILD_SUFFIX=ci-$(date '+%Y%m%d')-$(git rev-parse --short ${COMMIT_ID}) - VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }') - echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV - echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV - - name: Build SITL - run: | - mkdir -p build_SITL && cd build_SITL - cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. - ninja - - - name: Upload artifacts - uses: actions/upload-artifact@v3 - with: - name: ${{ env.BUILD_NAME }}_SITL-MacOS-x64 - path: ./build_SITL/*_SITL - build-SITL-Windows: runs-on: windows-latest defaults: From e92337a5fce818c08a68ed632b46ef32f7740680 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sat, 27 Apr 2024 17:32:00 +0200 Subject: [PATCH 12/14] 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 --- docs/SITL/SITL.md | 142 ++-- docs/SITL/assets/serial_receiver_crsf.png | Bin 0 -> 6993 bytes docs/SITL/assets/serial_receiver_proxy.png | Bin 0 -> 7107 bytes docs/SITL/assets/serial_receiver_sbus.png | Bin 0 -> 6803 bytes docs/development/Building SITL.md | 39 + src/main/drivers/serial_tcp.c | 49 +- src/main/drivers/serial_tcp.h | 8 +- src/main/fc/fc_init.c | 8 + src/main/fc/fc_tasks.c | 8 + src/main/main.c | 8 + src/main/rx/sim.c | 19 +- src/main/rx/sim.h | 1 + src/main/target/SITL/serial_proxy.c | 784 +++++++++++++++++++++ src/main/target/SITL/serial_proxy.h | 64 ++ src/main/target/SITL/target.c | 109 ++- 15 files changed, 1143 insertions(+), 96 deletions(-) create mode 100644 docs/SITL/assets/serial_receiver_crsf.png create mode 100644 docs/SITL/assets/serial_receiver_proxy.png create mode 100644 docs/SITL/assets/serial_receiver_sbus.png create mode 100644 docs/development/Building SITL.md create mode 100644 src/main/target/SITL/serial_proxy.c create mode 100644 src/main/target/SITL/serial_proxy.h 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 0000000000000000000000000000000000000000..55ae6af98b62a85984c02020ab6280473c3c3848 GIT binary patch literal 6993 zcmZvBc{G&&`#vdz3>8_@V<}3MExQ@J%9fB2vW4tRvJ69F5T=rSH})(cS+Z|q%f9c+ z*rORUm>J9b^!~m-pWpBN&iAk9Joh>G{oLn%uIt>_>v}~z(b2d>$3aIyL2>EPL)E7g z6z2d63d*~*RA-S#LChUz4|XrL=U)1*ue{z`de~AZ*|=KSazApmw6lF`YiZ-_-fb&? zmJ#$s%RsFsf;UnuvIKoZVVJW{A{`tYW&!6ae`GZ&F&Ki z;-1cJG#0!z2ab-X{5(+Xx=$-cyU0b<#58jv=s%n7B)wT zZ0(ytkq-)RLlGO4EE*qgq#ha!l_O@xa1{H-e%GLm~T8S7U9`YW6492gDqKNmWrI9aIo z;;<+o%VrDy^wYaC2^r)3>3jTq>112{=KV^2e&D^H(*`c3uXO~8ZXb~%I|fDq*tq#V zEp|^{72PKn-wdEK+mYu0UZv>upG3%UN|HgKA6g|{`Q7{h8&r!~WeIwE(q+edFG~0O z-uLGA)DX$S-I;3* zfAE$(go`Uc*snHjPyccmW-@$0%gFa3VP%z|Z+grGeLuFv8(5%w6>^%$HOVlurub&Q zY1LiD{N~+-rht)&?iF~~9cmnzXx|Ph+D7K2A?h>MSzP`01j!=cj>3fN#(jTtgnR>> z@Yg3$^ESN6lBJ}ZDj(F3yHLE-KE*+x$A&&AwPMb3NQAxn)vTRj&rCno(lV9R6t-h9 zjhVFT1%QbV)J*e#p;H|RqE=En_y33gB7})R@xO;_!abCwVbMLP3A93kj)2Aoz!;f73;=x zH>?Ht0Rd}(Ns%K8@px&aa(^l;gISRy8u3^m!RQ&-Jer5fK?E$g_(`^W&;2v$3N#2& zNhF{|35rKnt9+KO7R@w!`YEg>Q27WxvLbn$71H|SbO&XK8F~&ipyMvA=|FOQn2t z?jL3@Qq?Iz>s`L4nIjopIDj&X$`O+YZ|XTA@bqgq4NcCaIJXgkbV=@6jT$z9X6})* z{gAA{Nv3gw=Y^ht6!G}d4rFp(^gzPQs6v|&JGNq9OLYb_bzrRU=8GV%uWaJE?G6;Z zMn{?ISNu_bk8Nb^39lGFoQ#BK zq-Ntt;6i8V(W#A%fQ;+0y~cjeIQPm+jZ36j64dpLy+yc|dF*fZCz2n9!QYq83$b*I zGSBGwY-yhtM@;trT;Ol`-c;P8K~BuQH+LE?dGERLn%rT}CE~l?aLG@(R7k{YBc;aO z=Da*%iLwFv>u%*2EmY_WtmzvE!%q3ZjCpsSV3F&g;dD1Jj(9bKH@>xL{iAO2@DA-O z31O{=Sl+O;V!L)vlp}->WW_AYU`QK)1$dKL>y*LCyZ*WgJMD@ibFUaLWc&s>OL_86 zUjVev#m5{Rjji!15K}u})V7*#$C?jl>fQSFyXmq}{P2MKc6+?ZjoDP|uTFNuZ<#n( zG!^Pw-|qS#S0Waz0msY2YUEVKnERJ!cJAGLW^Fl+y)^{9bfj$U4hJGHcc32o^d{xY zqB|AOeS2Z~3n)tkM-IqpvlYfDe4@p9G_yz@{Kh94oGY zH^HZy{NU4(>kT4+#i0aaSNV^_{k~JpLExYs&5{?|909%#cA14smEy}Zns$<`(URs1PnxH1g8EpV z{QSJHra#T%dL11+Jk)J{bZ8yoDfp0VQsj232W3756W-CtoJhyKVg*Mcw_SF$pAXPx zTuoyhs_#*5|J(LemM<>Q;ovDq)V%Y7{BfazO@EU#kjk+0T&sU~!CRj2=Mfgj&gi=B zI)ao0qdoQ^(s}EzTe>sm{pQ#+rl>YxHaQaC)9iJ8*rYdr)6bdOwd zkKk?w^||4Pd`n*UfYK^@$v?(m=kBWSKw&-~xLh}(zn}U$AGzxF-XXi?`8qa{zDo0* zwAH`zE~Ni{ODA^SxwR9Xt}cWS@Ojqk-Pn_!Jr8pTNY54Ss5@4)Y51@lecaXwZ@e2Q z@m1#r$#Xf#q{1UME_5w5&`ICa@AQ*#k1-C^4ex=>!eopl&RMaja}`SLtS#ObsB(`N zR~Am(iMmOZYHRUi0CFC%jQ{%PXu0v)YZAKlCU8DHM&lyb$Nb)v)9@c6Ww;i7V)Vj| z=fsb_YGm7VF08|g)U8rlvW7zVtxZAdG=40?X;sBrMZSLdPMLfnqv=ugDCMB!+G-uw zBSC|Giyv3i$sL{WX~V%X2hVz8xywdEv2m-pM}%OwH8!=@ei*t;vMZ@$6gtHt@@*yxE9JGY|ZX= zzMsBV+nDZzneHyc9H6kM#+}Yd=mOZ2J8kCGl&P#{&yp1$}Ur#-p0U{j)Mjp75`^1 z2e~i+gp&(`Z8U*Rp*`5(BS>Zibn#TQAcj~_2VvMQ6VchowBRIK7Pl74_LhdK8|?GT zzYV;`=RWr2#r6q$xd2^|_f}%E^H$$$wEc9ioXFX79y@>+7RTIQXFKmn4WQRQ9^G|( z%Cpd|S91aSJ235I2B^*?-q!rt+2MMYhTw%I zj#iV}@boY?y($R>8eCoY)Gc0~htn)oG(e8bmIHCBSr^NaSpGCTUc`I3<QfHk*}jz?vtd-1%6S%Y@xnuILqHd=#Zu?Z}=W! z{*|{sA$;DMpYHJ<4`e(R(wA}JRHPz6CU8i0sF<7~{uL3~t)XAoMICc!RgkoPD` zb{1(D!wZg5;DDYFRm-;LGrJ0Qxddoiw}zJ11n^uDYopEvA6v@DLK?Iuk1g>NhD~lM zep0T{fF#&QD$Ii}xzO8oE+Pu>j%5>nE4-$%fRvYR)HZU}xz1ZV=|pk?0qJmwkC2?b zk)s%W|D!$UAcI~&9Suy0owY{F*@=93t*fhJ3u(2NrKL8}<)gFnc-Llq&dT!C64Ro| z+UOAqP4w5&4a^IAs11-Yx0EjYxi^bH{ct@&7yA3deB}2xAHzV;z9tDs6~ z9we#gFGUbuABR@X_1_}AnS(DE&lR?uu#FhB?m|jJ-}ldwB4BR zl$(9XHGW!hjuAxN$%$=} zloKadF?eD9pL5iP>=^@LekNIqpMsAEvm4Cg=bc!J|z~Q`^->{^7 z>&c{ljI6MspGC=IZN`!w(|KcKAI&C6DQ126Ho&8uPkx}Ps2EV1y6g)~3%e;sAm zs0W5WEaT_sl@_R0NF0}t$)Hw4omiPwP!1}42$!A$cAMP?tZnSoknCuIoFy%z&o3xc zZ5|rGU9%1A?G1)}=X@m#I05Yn=bCx~ zzDwEqW=KZPQg&4>BESVY(0qTj-qHU{c16s3lC1N%>XJ_bn)k{4Do{3>j*U;$c z@LX}u4|PjXp`e+5Ht`qc*$sjLnLEBtS$~_6^{Fx|zaL#5zpMfcRwlDMh+zJ(kFe_% zKSi7B`-2%4ZXYU`FUiLYKKy+fHiM5iM-#o3u%oGQlo%!c{emt;tF18RKt;6GKck)p z%)XRIcPcWVs_%N_Dj{xU^mwZ7%VJ0^Z_k^SxQb5Dh+Ek5M2-r3|CdOBA3*Q5lJkG6 z9;zLl{2faY9ag0z7ljbHdyt_A?P8dW0?yzu(Ss5oMu;=b(RY_mGm4Wpd^Y56+1qDA zjJqWw+{ssU3Ied-;i{BSvns;}$CjVvM^f-pCZ0x#s|4an@1tN7m+gL`)plqs$oSf8 z;ZLx>GQkkpD&_^bG8$Xy%8J3%yL}KXaMx<6&`T241!y7>&X@0Mgx1tggoT^2k_I-+ zjfVfIf8lA2lRX2)uQxyLXWZnw%9rh-MQ+3}4-?(Oso}4%c$EdWg+&#D@ zob;mBzA5!%Iy;wuizY4Ug&#ywOsd2~O4e6pHC#%zsr{WPaESK8nq%fWD~gKf(hiIt&FZgxgqr1{D?$=yvCm6)$yaOy!d?punRM zSSIxfa&`n((VR z_v+FRy$iyL+BnDjM-)dZ~r7 zg;2|hf>*|$0~S}TptQ)X`KOG)!tN2}951-ba2)b131Q0+R0{^nr)c)ajgMb~k4()? z*X8IEi)+9Q%HS?zvBU^_Dr0On%gfzs(}N43wnTAPI8$}wrJf^jIm`kwE+OlqmK zE2-qItvpXYloiI8Z`6${f<;^)Cz)kSFMmCu|7C^AeZgCz-65@IU{gYTe$g&| zBhGX2@^h~bn>|0I5{*BPeC>mLzvvL8(2zrrPBg#Vmizue=?mWM$m{Tk$YN0-y}C)0 zqOJ{JU0HiGU;h1had(-99>S*&p_SX>%yTbiSeUpm$0|A89v87$DOM^~t4P=*YFeFh z>wN1xV0)9h;P-|2*?52TJy9;E&izPG^$T8t;*5td)al2Mr?NdEVV+e#$9L#gfpP;( z8bVk=MG4D5pAOS{@ZmTFmlaRD?Igo^g~b?m{Dop8=Ihn>G*L0rtNEj^q6E6^{JB|q zXEx_u(j5p@Ss8#+#DSVH&$iP8#w&Hy;NeR7a3_D?foBIE4t3dw9tmzIS0_*mt3vuO zENK1;*)um%3LCs|m?d_$L%9pC12s)dg1t27|Fuy5)xuu%|LX?UXsq<^`Nv%$%F7kT z+1m~NT1HlS`?x8}>RmhoLaYhhnQQa`>pqw^8pf)5y_oHFce{OHfjjYe9!kGaaOpG~ z*AkbmIAmN6xiG<%L>G zzcGX2@mv|HQNuZ%&>VpbIm5dZKR-T?4mH*g>mRoZNKc?LZAp?bQD&&6e$LpCvAvAU zEeqB43{V);$QIBTc`;y2-1K{OyHV{btv)g`4s?pIkW}5 z&Ry2kvc#&KE4cJNR_WFyO$yCJR~hE3kUSOMKwAC6s#ZeF*gnI7$iy#{C>c4+J?c2 zM@1CbD|+t`hxMMVa>D4->_dyjuG3!A26C&_84-%4SdU%QS=(2O1}MT1m`n_-Ar3k?6Q zTl)GP@S}1*4)xrzi-3-(cg9eSlb>AA5?dUrsD~5Tkl1!RMcGJNsim!l2DaZelx0rn za%Z59vtuHwhKXA7U)pZ^68+=?jI9GTO)--sO61mH`+?gRPEPP!3cku;Chas@f zP!WzXm@!bXcPbKz*f(}wI=UDF@_4^AbtgH26afhIHx3PBk)5X^VT@wnQR9<#L~M(t z<|)q#&C+uHckQ{3V@rmH1)a?PErGz)Z(vQbtpP~Tu*$HJ0!UBbWHc~NI~)D~XzGHV ze+bR+51~;sySiaLdz4E59%BD1t

S7OCC|LD6nvnaZmSU}{r0$xP)chApVHzX_8P zw?E!~(|rq?Dof+f-Ib>l3`bMF>;I1g|2jyf7op%GUjhyHiRqqQ-%vbK(@`x|e);bI E0Qf`1nwWpG`n;AQ{n z`I&uUM9v1Z%Vf(3 z$CtOlr`PwBdsYsIQui=3@06Yr_wl`>ISGA)sjP>@)z-y@iOpZb6B85ry>V3)!Hbys zOmp~W6X)r+PpI0uFr$>sePZ6vhGld_^Pjc$z1};fev5xPm)G_zBPUDG0anYg;! z6!c^K50Qwcd)Kj&YltTj4RcqPettPTJj88pEE9+EM10=Z;`|iG{#iz9d4|7#Kp$~= z?qIXCcXFYxwRW!S`1p8nagj=;5{X2oX&jE`uSsonRb%g|&9rC>bM1tV@7L~UUlM)C zq;b;8mHx*1nCwuD0?{$q?t$Xk_QYD5Z9f;g`HXhn>@0T~WnVe3aGO)Wj@&}{ff_uy zEoTCQr`6B+y7WRA`ARQv&@(~>u=Ii8QwWv*4rE-IYOEG>>vHR=xYx%^T{$ssiPpF{ z6<3up9W#XcC0~^>h|mihM9o@d>;%9@cx&+fW!v^5-s>qdmVWG~?_&o(go?@f)0^8f z3-+cA(8o&aauq?~nCX#T(`TXsQYEg|3Zia8NwMmFP|fn49_rCR!|~&(&_7G}1fh>4 z)W(It@vedwKDY`!NJp(Lf%ZMelEAA|;jNmcR|B+mUGwgfz4Kd|-X#lj*$*=t!i^uD$uM+#q#kn*MXVPq+amjsjbs+@3hSk`WIc4HVJgM|HW>~ z`e%k)KK`EuOM4$XXOTZ@oQGzlFh-&h*9t(N373dr8)LA9G`L=CZ%CP`>)^%wZc+`0 z+^9zR(eX4VMKS^q&l9CO%q|42`%3lWo(->cL_J4Kpu^%lDAoa&c{Hn!(i4Q$$GP%3B%#SX1Cn&CY#vi1PAyk_)HQCS<174qX!?o@rVlX+wgxftkxw~bR zw~0XX&AM8~JI*3NzXR?O_M)?OHw4KQ@EFi+RpaCE}Ww_9C!4=gV=p=Q4M`sk8r2 zmNlKW0zye|J1gJ~?Sbxo%J>QozvBfnP)lb&CTnQtbuvMX;XN^Toz=f%b36>Sub7zUG1}~bdFE$%(K3PXf0Gr;YV`N7)hgf z21IHMYW8oU4B z-lemOf^tTYD6Jo%R^g`H%xQ~Borzr)UG2NL%&Mw=ufYbe8xOhO!^~NPb9uWaT?Rgg8Pxa8vx#EGVL7Rpb2pJvv8V)>DO8JEpGxmn z*>~?mfO41Vn3$t&U8uRxRlh^DybIJ#0$ewL0@q(Pd4P>5elLql7rPN#rh_@?%YDr6 zS0&2w^o^`BS5kJ4bs=`EbV@GQaOlmYq01H#aT?(luf3n%x|+wr;5u|DcIzCZ2-49V z12n(6#0l`?`tKyeZQckAoq5yh{`|F5J12jHi{=Vv2y|5ZtHq-EX@jc~azPqZ%)q!p z&64q}=XL!G9AAM=10Coomk1}ahFirC?B;udztIQ}7kfxK^y})}tZ<9g?#H_dV!tlj z@x8Jh36i|&zc+h06v%v{D{DPSnyIkA#{H{wiHRX4Sgsp!S=Y?{?##51sdrU*_A8Uo3HT!$mXdyP1Pd zM|EuL&xrzVe)~6K<9K_(MU`2|P`-Z8Tw@vDP5x<)7R$@lKEnp#i-hbGknimK?NSLS zFo%@8fRD>}g!iD6PVb;vcNW&_OJ65?N&(*AsamA{b~EKGsPn+;%PbDdW|Aa-_4K<4 z>zYGZkc-48L%#f_!W%5uH+6fHPs&IZ4<|V$Z9)&D){%H4KIC&T{9|VChs(huH*ji; zBF3MuqFy?QT07mpav^M0t>Ea77!bh&^7w-AT%VydTSyG}X#E-=R?I`~M*bwN3bMFe z$a{nD-C3O6|IM#gryDF{B78Z;w2GZb+csGLWF9Ztq6y(W=TmeIL3L4U9Uzb3Ri7u3 z+}se<>YeR?!5%7Gk$_@>{@klXP2+3h)SwR!N-m@wPj48%y1ckYM*tq>-E2&AG!Q+r z858BNgA=)UA(brnr(bydg-^A5*VMOnU=PtN{%jd7Q|h=&gZi2P66{3|tHmIrE?{2ea}i+oh(E=!BnElxG!_n+wu92gzhdw0UPathxvKOMi#ukAWPADx{SZ5ly9V{F z{eD2MSzSsE;F6E13qkju!K0@Ylm}@f*&2!Uu(TZnu8U0_bN2Yz z>DEc6-8K8wF!Q}CGZDOT&t3Lp&{V3c&mBG*hR3YB6>94AC8$Z>kKI;)2ymJLBlS8< z5u`3+KE^l(TtPxMJEaz>*Pg`F;CuPkgH*EGE`&3u*V*cw6}X1{!>^ifiM|Y*-wd+B z&Ty%VwOTzcCuA7=Z_IhkBJ!8YspW0oPwLG1*u!C+LG;@vyPm$Mb~!_h<$@(X<9lY; zY8wJ1{Id?8I%ux|Zh7TJNwRafo0Lc-*e~C%#$M9UqI@PHOS!MIK!3GZ@Mz}Us4h`s z={~PLoT`D#Y~Y`<$qd4ZmkZU(weWFnv)Aba2kw6Cx2OD`|~o+ zbuZ9xw4wvd)YgJ2EKN7pAG&2ack)rcqAFFEu>S^5Jy529U9{DQ3!tZyI znJGbizalPz8sLY0S~^U`{!DYaQ7 zn8&yyQX61VT=v%u?7Z6gDAy-5@?$C-9;U27^cPAgS}|>7YOb?~A9^bcvp)GRHp|l6 zKgiM?`8*rAu#9v4O}QRDcDTkE^dx2>==dT_00*D=sNyY_ns=zQAntvinnX~~`hCCg z52z%oiIJEgF8#AqQ3+KbUZKO@fHFhO-$i!c+(qKXaL9*-CK67j-WIZM;S*4C@QQtD z+SI8hUKw*YD=skwMzg=4os5N^+Ic(`e5z9@ckbddroE;<7HEsi0K^IE*#P@J5+t2d z&~|I(*-Y{>ey@B7-X+h(j@ikuhr)lh4WyuESS+55Vv)?L9e{`F+$4 ztq#;6*8S}LtYM`?zD{P=Y{ixW=0@$EnNJrhBjUZyBlr^2>yNElY!8re2+`^D*4#I0 z{fzAt`;qR?!X^;UgI~w$JINSZpUV>L*)_Zc96

t7mHzQv=Rahf7`Y7b0Wy0waKz zFY(r0sJzx^qwu#M$>l?ZvjOsvus)as+1i-YQZ3>1i(2=Jlz$uXik#&22ZSo};>&KY z&UDjzy0^Bid|-mJ28Nae#jl;S;pLvk;@aC#h$!#t?u%i>LrcV>mat~txwr+X?rz-i z^e4*oG=AtyQfqJ#*}8cAj#svVzc(Sn`OFH3k`d73$95jpD00JK+q@s?q3+>UsXzEc zTRv1>ea~dIHymIpyCkZ&SwuE|xA4*m3T8Wv;z1xnVSVNyu)Y`!II9poS<>Ym82xAl zD$WWF>uA0Z9b)zq^@!a9((8A@iP~qhKElqdNtkvcU$@QlXt9RP>-H}p_K!LNBS)9BPTt`*6x%{N7Lsc?V6I3 zzYG0zJ->xELtsCPeel5D5Smt42^&sxdtuamnZ>CQvs$#^ zFwj}Q7_`f3EqswAt{v@DaBm}xhxRoh_5R}hYsMsoTZ^spcZZry6@}<9l%yWmx$?j= zQq2Pu`D+3>%MCvyOP#lsok8cX(AJMc2hRpZ6BVBeK>U|AaF=S+xTsJ4112r5?+af$ zMWfw+dA1EckQ9`mK#S@-`TRgFjJvQG#Q6Vc_dRwTeE42zm&k~A7+{h0n5)c7N821&V=X6s7+i20k+ zJgpxu=7ZA1XQq6MzT#635)L2O6e3F+M!nLVaT@au7WrB8Jr zG1JQD7S60gqiSor(!P+WB&slZZ9Hc}U1rogJ1(7*(0w4;n*78IEgx?fDx+-gNgo?` z2?I52_-6UovjOS#r{#}YvF2jQsA%7+1ixKD4W2MuS=;fl@cU_b@{6KbT)K9OjE2(_ z!OFSl8Bjpcf%x2~-W4^|H~P@ckgt*JxQ+8})r2$uFu2L%j49wn(*aWZO8eBEE>|Ds zhjAgDcQvUcHSsUbD5Vp-LfdGhcxEH#j+bF+I~CCjXK`f1a1A)N$(>&MBAZ%vFNe>E zRpeq~;!I1PClVE>Q75r=xwWEr>w!I5%Wbvn?Qg_c&NZ`;wqPwX1u_Qc8>rP-BOFb%wDNPy{WmsQ%kH8j$ia+K3l zwIn}Oy4pp{R`aVcF>Jz9DuJ`_S80oEsw-|mK4}2PWG|JorZ=?wY^Mz6WmDIMz53m< zsx}09CNFz+Z)AKj;~STvSp8{V zLRc41HUY9}P*_#mVeu?W?pz&nk95^SR3vz?Ihf`;StgGo0sKg;l3cGwDEzUD>6Y%Z z(i~!}RmO0DCifZgv)`_oAB&8MzaHJEsKyr(GVHMq%NXUc-ESvR>>P`3Ot$ZTAKj+$ z)^zt5y3pd=YX2rnxv;*D@?NV5%2{Pya^O&<<+MwAqHT%W>&9=>)T0O6x=J zh$>Z@^&=mbdUJcf~S-M(k< zr|w`g*YA|JBN{Em?Sd4(L-^9xmGaKmhIJX*P!_Pwl2VTJGUp2BTixQAO9>rcw+a#V zn^iY~-Pm}FOu~5lZbKX?c20*g24nHM3elymhHti3k*b{LbnrO|luYhvLDsrxJR*w; zpyjr!P^iz5RC#3$XWvTsNb51gX`?)Dpc;=rqVCA$ouwB2dJ1)?w%ZL{`U@2j|ELeKz%iS_%_xE!Tn>wTC84f|J z9t68zq|;K%lTttJiA~4dwB%P}77wQ~kNF&d`y5>afGgfd@X`G}U^Nuy+LK3Bk&8n4 z!l_~pV8@_(AB4IC?D&fL#v1HrKc*GGm;R`IB;dsFf0o692S)-98sSvP-(lo`i5&k| z|Hz13W}pmX$2G}sfByKl0#b4os14vNZ0+<-boE(3Aut||QtYr{!K^faXbQkeCupwW zGhgu%7DxO+(^c9)4LzQ4*<}njQ_~|pfY(T@^%QkgCMs^arUo$-K3#*ki5YYHRGQ1# z4N}w%y>irF_Jzz57B(*^Lt2@Yl_yAxCvJZ8sOCp9TS^1p^;Dd#du`RPeCEx$T`C^@yV8x@38v<6hL*U1f=WB#RBf9bZK@uD!kEN(MRCuHYG>V5&& zDr9AYqZwhKi~N0nZo7hjaz_fmHKXA_0mg1Ed3X(AhDZi^I3yDB6I=Qz7I25u;)^fE7VZ33 zmv?bd0GyZ|N{Va;iOB`kH@t+N%gV|kO3bS%b3Cj^zY^uv3;)r;a6Bo5!>LWBiW!nu zwNqXQDOi7UI^vIrNHj!Nr0naF|r% zT?_Bj)gv*)hm^X2B3i#J>T}*?fc5Sg8v=1@ywjZ=dz^?zUpxUVkpKvOtbcXTfq3u? zz&N(RaL9S*A87oOf&M>edIkjxQ8;g0`t|*LMR+of#s_LFwt13t$E< zF6;spXVTF1HCTMP1(I7w2#fZ-_<6*8dT^e`fILCN%WgW1|MWm^!Q3hFn8V2Y>fCv? z=x81^4^;C)`_YSUikDB!jIKgRMZR}oql^fG7CR>gjEB&A9e9KCBPaA$jA*I+|xX;6CO0RPJ>)xGVM*UbS@gmAl;qXa{pp@WGica7Qjd+ud&z znaNYPu*e}qaZLD@4-)Jd44DR%4mI994ZLe5gC%{A3 zcj^C<@c$OKX=(l4BgDshT=#NxWPxzjk4(k|F~RLf$c74IVB=7h;0rUvMecAeMxT$d p-l9-}Rjz->{;6X$9RuC7h9?0m;teNg|JUfWH4M~C?mT(-e*lHFgdG3? literal 0 HcmV?d00001 diff --git a/docs/SITL/assets/serial_receiver_sbus.png b/docs/SITL/assets/serial_receiver_sbus.png new file mode 100644 index 0000000000000000000000000000000000000000..ce4ebcce1690df41de20165d5f73fedf4d640a48 GIT binary patch literal 6803 zcmaJ`cQl;cw)_Kp~`<(sk^G4}tsa&JJO-)8dc1`t};&U>x zOF%L*@`qO`ND|d|4Bey`ipzO(USm$SZT=(nZ$W31L=kuy~JNXl~u zxkoOgV~@+`%0HHi#r2CTJBx^jVbS&T^Yf``X`&+D)!sS{4GqrDl?Gm)$jH16%IBQY z24d8Gv&K#y1Xou$yXLxpDux##jN~U#t1rncA`#oYTUcr5lHs9Ur$-SHkIWJ}XF8Hy z=C+R0`!-GnQcuUSk5IpG$A_tdrvnou)6>(fVG^@NCb$j6>hkpO$sY=)!6AB?MP^#L zhHeX8Ik1NB-6#FOk8$wPruFuij+M>TmEN_@}UKcZJl^?}4hUk$61Fk}zE>SpL23Gf7r8E)W{DFIYY|!|S-uAgq$w&!CBx;=e zr_*y(5nzm>JdT4Q4t7=RivGiW@?gM*+4Hz(+7fd&GAMzg4E~uZ{?wrS+s6#IWo5;8 zDiFw8B$t~1KH*J(|G70S{P4#kK}D*VJ4uSY#j?PnS3&4c?As^js%+8qeKS(;GYrE8 zpRxC&l<#$-o^|EI;_!h@<}EuaJC;(q(;G3i0Z)Lk-YdunE`}?&lEoFQP{XLi3Bt4N zg@dVg^iagjaLr|&fB&juvwzuxd)_KT@5D?7eVl65(HwzWB5(d+ON`gS=SwJBK^!qQvG?ofFqlT` z=Asmqn?;*bXLq8QCVFtMB+iphJ+{&Cdxpz3584u|zfeQ7vRg+`5rg0j*kl?5Mj=2& zyxBb6JpIwyy$+SRQ+JbJ^h|xf&+A8e7HhDa9?|`ldq-FXE#(=YGro+{!~(Mx% zvhJ-+sI&W7^Q+(;J)^d5X51SwR`MFn~^x#?ZhE}?n$0FE3I;l4c)F6&O0r7VvT>{c$-w84T`6z$>Prsdt@_(uP z3An%2{*M@SkI!=%@688{`JO@i2+#QCoZUXW%JtX=h8%>W5eT%lm}FOi)+(0;kMAm7 zcPN}LkMadsF$!BY^=&m)B}uj~7^a1?si9*REAK!q(|ROmp^7Z9SX=0~Wq3A?LrpD7 z)6+!autp}7witme3su<~9Yk7DL*41Q46$@)5zvs4(;f1~jDd=6Bl9uw z-dyJ!ilD8F);zV6-Ct4hE!&`{*o|kP-}-N>*u0YbSjNa@+K~)l4KJvuTrNqqBgev) z2=afd_^&0MC_wiM=$KZh!cH##V=k{?tnci&jI0vd3vo~`$e>L^U7y*j2D|Z!ot-v; zUB6Qag>n|V3~ERAVW-)i@on;~N#1yzbC4qa^7K`_Nh1r;@TQ&rn48{+jG0|O+|5oo zc}F`{loh_MvU4~Mn|S1f&utzFT=Y^Fw5o zn`X-=N-uY4z6rD)8#Pme_5EFTp6HmOORP`!!?T!N%N7C#>@Btt9sRx{;knLx5sq$5 zmcAFkpE@9)`&S>G_}&Z53e;`YF5PHuRpXnZ^OL9sw5kn9qPMz2KAco6U$ztONTq|T zZSD5Z)5vW;kw%<~^4|-iQm-2rRi|2v?;L-5H~D+jElWbcunIcG3#9Cz;l24+JnwjR zih&`va5_PNnOV0-&Fkfn;Vnk%?f4d*N57vo9zI!$Y~WyfgIyuzBR6}Ef!v)c8HI)3emhYQ1^8&95b zjMC5TycD^+@{9oS>+Ks7P6y{HR4mgt>L>%967~D-U>v|#@>1x z1UEgj_1LQD_%I#T|1&<$RW{SdNvY;ow&Tfq=<>yuW(cUaD;ptQUj{WwsB1({cqp+O zZZI9P6nq$Tu)WLTV0Mny>Z0<=bH+Ci+DA$el_wFB<~#o@O*zUYUQT)HNP~x{|oz|dq6;7`3v^kQYD10>w{1$p7QvGH8?zG z^1iLHte>?Xk83@v?EtfU`8my;xQuz>lqm68pMT=}DmQh{Vzy|<+3>m>(3f-;;k?n8 zdHAAtqT^F8&{?lRt2M5bwZg(x7zY3EWi8%tY&d@C{@fTBSLRM{c3%?9!?JQ*tS?T^ zY~<>k>8>JV{}62h>!x9O267BK0qJD@IJO~<8ts8)x{#K`%Qp@&=$1}0Nu7lt$#2B# z)XFXDm7U3)(7V&Grr9=1ma3HW45_TYJI4Mx$`mqZ!^`?+8A-(amO+@=9V_5I#wy_X z$1UnVDB!d8F4)+>6F|{9?IQ_`xdg;4H{Bpxf>P93;RD9*+f;Azy}VYYp+!}f*Xy`; zZ2EF)>+zwVrwWiQwOVoG7r0l@r@6rk9bvfG&#FT#%R?2@e-W3~2O^3?e#F6U3kLwh zKS{L!pj#s>huXtbTnds)NvQMsDR;u&9b&j6k4HLp72+1C%LB)jJ?8Of@0a$-jojG9 z4f5FP?FZLfky$rxU3shG4$&IDfAdx>p6lfhcm0~<2_0v#t8sS(Z+|4PB2=bgDeEc- zTZ7rK+3%Q2L$V7a0TMO>%)%?i*a8i|`3IjiW9fBL#hx>QO}isbgWa)P#A86tdjMTz zIFbPZsUN&eF{!Lbvn3~ovWb{=9;jCmgyW0qx~+F-h48Vl3ojqlDwyYA0&6z&IIe`I zj_n%3Z|Hk=3l+l4jxGI7b`jrJx4%$4IPhO@nv$qHC}8Z4w;$@B&Gv8%X49#Ea@U)C zkKY;j7Wokk%sBocNu-DLPIMebIJ=6q8|i-JdL-BVo3LCcG3=#Wc3CVh@wEY=0~cA2 zwyzh;byhG3wm8H9@6ca^abaKjNj|#5nYz5r^ay0k*LJa&#eD;4ExFgmUtZyIdmSz% z)7e_qB$7IJj(fFYX)6XSWL>di)b>N~Do^}sGvF}}sA=Jyympw=PZ-DwTL`Mz=ZvAg z%*YL=`}|HjAl*A!b6S^bwZ_j_2)&-AnA<37z1c=-8-Q$LpRlufKgw4q<;;HYISr>Y=yi~&?En?!ze!t%6+<3$6N})0K6SMG?`+N#4!{#Gksyoi ze;`CE5@Z(`I-3p)KS%6yroNqJGbn{#7_aNIIZS%x(TPnoT6l$^B%s{EkVSBWyk~;@k_D_tdT(ajB@n1hjjuh z-zmL-DJq99pAbK;zI-fE$g4+pDLD4VO2>mX{;;^wrF1`x;vaa?Dlm1h7%CYd>K<~Vo=KkW#*pW1t==nX*_QpaC!UNDt47E)o0 zE#v>v(&$L6{9?RBJ+x@}2;?7e6#B9vA}JeCp-SuCT$<*Lm$iUrZJP=_~*g+K_w?l^phF(5C*cX9(RlFIASMG!9G3t}E? zP1sSBJI}4K2Qr4j%CT~&EzS2{n-corsVDw z-fMLMJkRCt+csWP48S77)}a2owOGW40W0l94}b<($*LfEc_>*agN6ZvaJp`@TKyH` z$Q>POGZ*&2f&0m{ZtK1!JE$4p$U&s1VZ8hB!YLjIc^y=7VfEsM0YTUe0aVt{`)t0v zd5PtFdZ0cnsCndusKQ@%wz^*u(zG>j5q;4}M}N#dBa*@*#jWbV4@f z!3@t)1m`*}YfyTWZFXyU_&|(h48ORt=uynC)GU!v$O_jE+~K3PjO7s}%0uAn0G&2v z{qaNJYx_SOs_EV}hkNCm-?-GR%9jsB$IfRQ_C+#LO&%`gMarT%zi<#{MjWuAXmIoyAmKW#*DCKCB8AChtVeaRd z+J>`gM0rlK={A__nv3%H#kgszs_OH}o5k*$F)I;t)LFjOfyrEMBaTv&%t6d<5~U*)ACI} zUf6Cdq2GRO6+bim03eY6SzAZD`(e1)UCWZ5`qIld6{Z2HhHi=xaPK9>{TleSO>oui zC4e3yIGjJDLYmvvJbo#Z&mv)2D+3i_^+W;VJj{8`AwC*-1-N?9DJ5*R5NH)Zg!NRf z-PPlP_I|CyG8i~tINSr?rI1)riF4OfKNDIYoSG6uH0=cxo-y4zGb^e3%RZ1#*M<5AX=#Y^xF`OQ~;R%Y$kQCaDJ2nrxIl(tbQ9 z?Xg$9KG?Y=35qUotC-AH_oOLXsvmU3RG#D|Ug4JR+za0e4dQ!Ksf4I6eRN^walQl* zej*Oy&Cg~?)H_Ec+v2fz^fWG*px`la@cbb=IT8!KZqaE`4VNYjKL;fo;vlQGjaJIZk z?M|d(kRRTP4v+{+*E^K6N2bOYFQGK1ki2s9N!5L}S&;3TjW+Rpr)PRNj(SnM^C)W2 z{`sB1i|wzhzy|QuFUv*ddqOgK4sB0)&=jXU3f~}g#*1f1z#T2xG6eR?@_0}_9SU)> z36|FNo*>Cd!p{L`BYR)9iNR6{u%bKyD(8IDukdUWth0`>f}Ux=s@NiFPr`{$*?J?; zo8Ssk^S5apfx`RNpW6T5qW+&kc72EnU$Xou4ohAT82rzmIWvh(NZBHgGX8H$jVdP6ETcwQ7} z?6KwzJQ|&As@62}4?xWx{lf0Qap=>rSIiK=`0T#h@~bQ+HgB$m3Uz-lvds}dTFGm! z+2p5TMCiTaddzQ(1k&CTId1j(%w+AK3-JxvcG>`zR7pnkL`diD$FAi-_DhWKR%+s& zXz1G};f$jI8a(nDPu?ez$G=anZpOS9-6r915uy(@DjrXA1Dja4Aq_X{dl=Py^%+pY zx`~?ZmQR4&g5e?3TL-Piz@>3Ut+@1D2SUxQnAC4;D&nE8KJP-?4*n znS;p84l1sqUL3DP3?NT9WjICVFa%aj_P9C?;@fpyWa+xAIt@xieU|C2wLmI$%TdL7 z-!Xkq_~ilc?oD4p2R1>y!TTF?L{zkk65df4+J6Y!v^$u|jk}h(bF@S@AReN2<}lOR z%l#j72t)!W+U)cu(`Mc}jx$!JHN|hk8$hM6hQzy%y-C#X4;##h2jE))f4E~P#~<#< z`G2_M-%j`X3;PaCjklr)bJPj_69)M-2$hmge&zXeB=@F7`%m-hT8(zE6n*mZ6@GfA z^A}Sb3}H{Ye}N@oYNQkNe`v=)F$PUMM;GA=hS4@D?AqTUVdpVDRO4OzTO`u)i+;-u z1Y9ZTl@?dblka;e-vSkr_z*7!f@<>FaNh!Vsw({zCK`QRpn%Sd5!{GuGP@4GpUVE- z_TiT#?aJ=LEk)64twCIiy3PAhNPH^2!mh~I*o6`9Rj%iyuOvNQKpRt3ku-|rkG**< zFmLqIhu-=0bri*{+Bn=seK8w~kOKMnwiBABw|nDQQE26*4j(=CHirLHj@~{Tsowi? z=ezIhHSXhdPS@cVWu22fuxBT)J+cWENhm72lda#@wy|lrRV+b(D`W-Plc0%zNevE@ z)o{k8-w{$d`+{~^70xR0slGy9xcr6^xF}G;p6ZowPBQ@Burpo-c9)n45RLn!2Nt;} zq8=_4eT6jiJTy6*GG~X;1F0h7$Q9koop8S0QaqL8^1DK&M6{|NOzQ5aC0^*Z<}y=)RLbsN5~>1GMKi<0GciJTrw$A?lp2T=mkLSSx_ zLkY#=WkipIMay;->4V_K^}maQvH}u&;phzq{Ia|{g9$!Q;xq$s{Lg*gY$O0%)DFCY zp^!hn^iPe^C`@86Oe6>%O7jN-<}f220?ZRVkkP~13cS_QRSJ-o-zR*8vYz44doGOt z30{@k51al|8q{J57c28v`Qr zUAqXScYXlAQUKA1@`dqWT1*XbR8FXGkh5e95(dvvik)` z)C@;}Jp%{hu5DX>gPF<6&osG#xlQpoynI!02LMrzsD#i2DZ%jN6X7Rc9B2fN@m9r} z1=5-MOIB;47uPz+3Wg2wiqzhcpnnQ0OO z;1{U*#NOq503MS*B#YjSfFR~R&!UpcDm}qpw8iseC(UXMayRO zIXZolEYjBf$5$Qyudn_+uSxDph=PfduJ^0H3?pqH50e_Zs&N3LVpayqFUqjpNOcsd w!ddi^+$~KH8A>3%SM0yovYE3_WE5oY#^62)TLa%n|3Ju8m9!MAserialPort.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; } 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 2839a338f22..b1352c1ce27 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 246d65c0a7e..406622af38f 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -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] = { diff --git a/src/main/main.c b/src/main/main.c index c002fbab25c..c303602dcbc 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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(); } 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 } From f3f0397e07003eb3dc510f23e27fdd07192a2fdd Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 29 Apr 2024 15:02:20 +0200 Subject: [PATCH 13/14] Revert enforcing mode on TIM3 --- src/main/target/MAMBAF405_2022A/config.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/target/MAMBAF405_2022A/config.c b/src/main/target/MAMBAF405_2022A/config.c index ba3a5344c20..d632230c240 100644 --- a/src/main/target/MAMBAF405_2022A/config.c +++ b/src/main/target/MAMBAF405_2022A/config.c @@ -57,7 +57,4 @@ void targetConfiguration(void) serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; - - // To improve backwards compatibility with INAV versions 6.x and older - timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; } From ab7ab945a2f1b45578ff54baab1e3256b7310eff Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Mon, 29 Apr 2024 17:37:25 +0200 Subject: [PATCH 14/14] Looks like inav never had the pins for the NANO variant correct. --- src/main/target/FLYWOOF745/target.c | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/src/main/target/FLYWOOF745/target.c b/src/main/target/FLYWOOF745/target.c index a82a708f51a..04c083ac94f 100644 --- a/src/main/target/FLYWOOF745/target.c +++ b/src/main/target/FLYWOOF745/target.c @@ -30,17 +30,26 @@ #include "drivers/pinio.h" timerHardware_t timerHardware[] = { - // DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 +// DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, +// DMA2_ST6 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA2_ST7 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M3 , DMA1_ST7 - DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M5 , DMA2_ST2 - DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // M6 , DMA1_ST1 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // M7 , DMA1_ST4 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // M8 , DMA1_ST5 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_STRIP, DMA1_ST0 +#ifdef FLYWOOF745 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA2_ST7 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M3 , DMA1_ST7 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M5 , DMA2_ST2 +#else /* FLYWOOF745NANO */ + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA1_ST2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 , DMA2_ST2 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M5 , DMA2_ST7 +#endif + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // M6 , DMA1_ST1 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // M7 , DMA1_ST4 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // M8 , DMA1_ST5 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_STRIP, DMA1_ST0 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]);