From 240a40e6167455e7d6ea3bfe6c728439696f1c92 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Tue, 23 Jul 2024 11:46:58 +0100 Subject: [PATCH] Add pinswap option to serial ports This commit adds CLI options: - serialrx_pinswap - tlm_pinswap - vtx_pinswap - esc_sensor_pinswap Setting the pinswap to TRUE swaps the Tx and Rx pins around, vs. the selected pins in the resources. If the resource pins are already swapped, then pinswap will swap them again, effectively to unswapped. --- src/main/cli/settings.c | 14 +++++++- src/main/drivers/serial.h | 46 +++++++++++++++--------- src/main/drivers/serial_softserial.c | 15 ++++++-- src/main/drivers/serial_uart.c | 2 ++ src/main/drivers/serial_uart_hal.c | 42 ++++++++++++++++------ src/main/drivers/serial_uart_impl.h | 6 +++- src/main/drivers/serial_uart_pinconfig.c | 15 ++++---- src/main/drivers/serial_uart_stdperiph.c | 12 +++++++ src/main/io/vtx_smartaudio.c | 8 +++-- src/main/io/vtx_tramp.c | 6 ++-- src/main/msp/msp.c | 17 +++++++++ src/main/pg/esc_sensor.c | 1 + src/main/pg/esc_sensor.h | 1 + src/main/pg/rx.c | 1 + src/main/pg/rx.h | 1 + src/main/pg/telemetry.c | 1 + src/main/pg/telemetry.h | 1 + src/main/pg/vtx.c | 3 +- src/main/pg/vtx.h | 1 + src/main/rx/fport.c | 5 ++- src/main/rx/ibus.c | 4 ++- src/main/rx/sbus.c | 5 ++- src/main/rx/spektrum.c | 5 +-- src/main/rx/srxl2.c | 11 +++--- src/main/rx/sumd.c | 4 ++- src/main/rx/xbus.c | 4 ++- src/main/sensors/esc_sensor.c | 8 +++-- src/main/target/common_pre.h | 4 +++ src/main/telemetry/hott.c | 17 ++++----- src/main/telemetry/smartport.c | 5 ++- 30 files changed, 192 insertions(+), 73 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 311fffbfe4..d2fa2e1a8a 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -715,6 +715,10 @@ const clivalue_t valueTable[] = { #ifdef USE_SERIAL_RX { PARAM_NAME_SERIAL_RX_PROVIDER, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SERIAL_RX }, PG_RX_CONFIG, offsetof(rxConfig_t, serialrx_provider) }, { "serialrx_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, serialrx_inverted) }, + { "serialrx_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, halfDuplex) }, +#ifdef USE_SERIAL_PINSWAP + { "serialrx_pinswap", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, pinSwap) }, +#endif #endif #ifdef USE_SPEKTRUM_BIND { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX}, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_sat_bind) }, @@ -734,7 +738,6 @@ const clivalue_t valueTable[] = { { "crsf_use_negotiated_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, crsf_use_negotiated_baud) }, #endif #endif - { "serialrx_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, halfDuplex) }, #ifdef USE_RX_SPI { "rx_spi_protocol", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RX_SPI }, PG_RX_SPI_CONFIG, offsetof(rxSpiConfig_t, rx_spi_protocol) }, { "rx_spi_bus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_RX_SPI_CONFIG, offsetof(rxSpiConfig_t, spibus) }, @@ -1173,6 +1176,9 @@ const clivalue_t valueTable[] = { #ifdef USE_TELEMETRY { "tlm_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) }, { "tlm_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, halfDuplex) }, +#ifdef USE_SERIAL_PINSWAP + { "tlm_pinswap", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, pinSwap) }, +#endif #if defined(USE_TELEMETRY_FRSKY_HUB) #if defined(USE_GPS) { "frsky_default_lat", VAR_INT16 | MASTER_VALUE, .config.minmax = { -9000, 9000 }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, gpsNoFixLatitude) }, @@ -1485,6 +1491,9 @@ const clivalue_t valueTable[] = { // PG_VTX_CONFIG #if defined(USE_VTX_CONTROL) && defined(USE_VTX_COMMON) { "vtx_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_VTX_CONFIG, offsetof(vtxConfig_t, halfDuplex) }, +#ifdef USE_SERIAL_PINSWAP + { "vtx_pinswap", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_VTX_CONFIG, offsetof(vtxConfig_t, pinSwap) }, +#endif #endif // PG_VTX_IO @@ -1529,6 +1538,9 @@ const clivalue_t valueTable[] = { #ifdef USE_ESC_SENSOR { "esc_sensor_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ESC_SENSOR_PROTO }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, protocol) }, { "esc_sensor_halfduplex", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, halfDuplex) }, +#ifdef USE_SERIAL_PINSWAP + { "esc_sensor_pinswap", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, pinSwap) }, +#endif { "esc_sensor_update_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 500 }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, update_hz) }, { "esc_sensor_current_offset", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 16000 }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, current_offset) }, { "esc_sensor_hw4_current_offset", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_ESC_SENSOR_CONFIG, offsetof(escSensorConfig_t, hw4_current_offset) }, diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index a6fca676cb..faf040762f 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -27,21 +27,33 @@ #include "pg/serial_pinconfig.h" typedef enum { - MODE_RX = 1 << 0, - MODE_TX = 1 << 1, - MODE_RXTX = MODE_RX | MODE_TX + MODE_RX = BIT(0), + MODE_TX = BIT(1), + MODE_RXTX = MODE_RX | MODE_TX } portMode_e; typedef enum { - SERIAL_NOT_INVERTED = 0 << 0, - SERIAL_INVERTED = 1 << 0, - SERIAL_STOPBITS_1 = 0 << 1, - SERIAL_STOPBITS_2 = 1 << 1, - SERIAL_PARITY_NO = 0 << 2, - SERIAL_PARITY_EVEN = 1 << 2, - SERIAL_UNIDIR = 0 << 3, - SERIAL_BIDIR = 1 << 3, + SERIAL_PINSWAP_BIT = 0, + SERIAL_INVERTED_BIT = 1, + SERIAL_STOPBITS_BIT = 2, + SERIAL_PARITY_BIT = 3, + SERIAL_BIDIR_BIT = 4, + SERIAL_BIDIR_OD_BIT = 5, + SERIAL_BIDIR_NOPULL_BIT = 6, + SERIAL_BIDIR_PULLDOWN_BIT = 7, +} portOptionBits_e; +typedef enum { + SERIAL_NOSWAP = 0, + SERIAL_PINSWAP = BIT(SERIAL_PINSWAP_BIT), + SERIAL_NOT_INVERTED = 0, + SERIAL_INVERTED = BIT(SERIAL_INVERTED_BIT), + SERIAL_STOPBITS_1 = 0, + SERIAL_STOPBITS_2 = BIT(SERIAL_STOPBITS_BIT), + SERIAL_PARITY_NO = 0, + SERIAL_PARITY_EVEN = BIT( SERIAL_PARITY_BIT), + SERIAL_UNIDIR = 0, + SERIAL_BIDIR = BIT(SERIAL_BIDIR_BIT), /* * Note on SERIAL_BIDIR_PP * With SERIAL_BIDIR_PP, the very first start bit of back-to-back bytes @@ -49,15 +61,15 @@ typedef enum { * To ensure the first start bit to be sent, prepend a zero byte (0x00) * to actual data bytes. */ - SERIAL_BIDIR_OD = 0 << 4, - SERIAL_BIDIR_PP = 1 << 4, - SERIAL_BIDIR_NOPULL = 1 << 5, // disable pulls in BIDIR RX mode - SERIAL_BIDIR_PP_PD = 1 << 6, // PP mode, normall inverted, but with PullDowns, to fix SA after bidir issue fixed (#10220) + SERIAL_BIDIR_OD = 0, + SERIAL_BIDIR_PP = BIT(SERIAL_BIDIR_OD_BIT), + SERIAL_BIDIR_NOPULL = BIT(SERIAL_BIDIR_NOPULL_BIT), // disable pulls in BIDIR RX mode + SERIAL_BIDIR_PP_PD = BIT(SERIAL_BIDIR_PULLDOWN_BIT), // PP mode, normall inverted, but with PullDowns, to fix SA after bidir issue fixed (#10220) } portOptions_e; // Define known line control states which may be passed up by underlying serial driver callback -#define CTRL_LINE_STATE_DTR (1 << 0) -#define CTRL_LINE_STATE_RTS (1 << 1) +#define CTRL_LINE_STATE_DTR BIT(0) +#define CTRL_LINE_STATE_RTS BIT(1) typedef void (*serialReceiveCallbackPtr)(uint16_t data, void *rxCallbackData); // used by serial drivers to return frames to app typedef void (*serialIdleCallbackPtr)(); diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index cbe734589e..4ad2545e53 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -221,8 +221,19 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb int pinCfgIndex = portIndex + RESOURCE_SOFT_OFFSET; - ioTag_t tagRx = serialPinConfig()->ioTagRx[pinCfgIndex]; - ioTag_t tagTx = serialPinConfig()->ioTagTx[pinCfgIndex]; + ioTag_t tagRx, tagTx; +#ifdef USE_SERIAL_PINSWAP + if (options & SERIAL_PINSWAP) + { + tagTx = serialPinConfig()->ioTagRx[pinCfgIndex]; + tagRx = serialPinConfig()->ioTagTx[pinCfgIndex]; + } + else +#endif + { + tagRx = serialPinConfig()->ioTagRx[pinCfgIndex]; + tagTx = serialPinConfig()->ioTagTx[pinCfgIndex]; + } const timerHardware_t *timerTx = timerAllocate(tagTx, OWNER_SERIAL_TX, RESOURCE_INDEX(portIndex + RESOURCE_SOFT_OFFSET)); const timerHardware_t *timerRx = (tagTx == tagRx) ? timerTx : timerAllocate(tagRx, OWNER_SERIAL_RX, RESOURCE_INDEX(portIndex + RESOURCE_SOFT_OFFSET)); diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index b03b80a0ee..32ba234c08 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -117,6 +117,8 @@ LPUART_BUFFERS(1); serialPort_t *uartOpen(UARTDevice_e device, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options) { + uartSelectPins(device, options); + uartPort_t *uartPort = serialUART(device, baudRate, mode, options); if (!uartPort) diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index 7d3c6c765d..1c8e082ae0 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -64,7 +64,7 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) { } } -static uartDevice_t *uartFindDevice(uartPort_t *uartPort) +uartDevice_t *uartFindDevice(uartPort_t *uartPort) { for (uint32_t i = 0; i < UARTDEV_COUNT_MAX; i++) { uartDevice_t *candidate = uartDevmap[i]; @@ -76,17 +76,39 @@ static uartDevice_t *uartFindDevice(uartPort_t *uartPort) return NULL; } -#if !(defined(STM32F4)) -static void uartConfigurePinSwap(uartPort_t *uartPort) +void uartSelectPins(UARTDevice_e device, portOptions_e options) { - uartDevice_t *uartDevice = uartFindDevice(uartPort); - if (!uartDevice) { - return; + UNUSED(options); + + uartDevice_t *uartDevice = uartDevmap[device]; + + if (uartDevice) { +#ifdef USE_SERIAL_PINSWAP + bool swapOption = (options & SERIAL_PINSWAP); + + if (uartDevice->pinSwap ^ swapOption) { + uartDevice->tx = uartDevice->rxPin; + uartDevice->rx = uartDevice->txPin; + } + else +#endif + { + uartDevice->rx = uartDevice->rxPin; + uartDevice->tx = uartDevice->txPin; + } } +} - if (uartDevice->pinSwap) { - uartDevice->port.Handle.AdvancedInit.AdvFeatureInit |= UART_ADVFEATURE_SWAP_INIT; - uartDevice->port.Handle.AdvancedInit.Swap = UART_ADVFEATURE_SWAP_ENABLE; +#ifdef USE_SERIAL_PINSWAP +static void uartConfigurePinSwap(uartPort_t *uartPort) +{ + uartDevice_t *uartDevice = uartFindDevice(uartPort); + if (uartDevice) { + bool swapOption = (uartPort->port.options & SERIAL_PINSWAP); + if (uartDevice->pinSwap ^ swapOption) { + uartDevice->port.Handle.AdvancedInit.AdvFeatureInit |= UART_ADVFEATURE_SWAP_INIT; + uartDevice->port.Handle.AdvancedInit.Swap = UART_ADVFEATURE_SWAP_ENABLE; + } } } #endif @@ -118,7 +140,7 @@ void uartReconfigure(uartPort_t *uartPort) usartConfigurePinInversion(uartPort); -#if !(defined(STM32F1) || defined(STM32F4)) +#ifdef USE_SERIAL_PINSWAP uartConfigurePinSwap(uartPort); #endif diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h index 87e55230c7..d3286e3a84 100644 --- a/src/main/drivers/serial_uart_impl.h +++ b/src/main/drivers/serial_uart_impl.h @@ -200,11 +200,13 @@ extern const uartHardware_t uartHardware[]; typedef struct uartDevice_s { uartPort_t port; const uartHardware_t *hardware; + uartPinDef_t rxPin; + uartPinDef_t txPin; uartPinDef_t rx; uartPinDef_t tx; volatile uint8_t *rxBuffer; volatile uint8_t *txBuffer; -#if !defined(STM32F4) // Don't support pin swap. +#if defined(USE_SERIAL_PINSWAP) bool pinSwap; #endif } uartDevice_t; @@ -219,6 +221,8 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, void uartIrqHandler(uartPort_t *s); +void uartSelectPins(UARTDevice_e device, portOptions_e options); + void uartReconfigure(uartPort_t *uartPort); void uartConfigureDma(uartDevice_t *uartdev); diff --git a/src/main/drivers/serial_uart_pinconfig.c b/src/main/drivers/serial_uart_pinconfig.c index 7a8a56b895..5afe9aa2c6 100644 --- a/src/main/drivers/serial_uart_pinconfig.c +++ b/src/main/drivers/serial_uart_pinconfig.c @@ -53,34 +53,33 @@ void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig) const uartHardware_t *hardware = &uartHardware[hindex]; const UARTDevice_e device = hardware->device; -#if !defined(STM32F4) // Don't support pin swap. +#ifdef USE_SERIAL_PINSWAP uartdev->pinSwap = false; #endif for (int pindex = 0 ; pindex < UARTHARDWARE_MAX_PINS ; pindex++) { if (pSerialPinConfig->ioTagRx[device] && (pSerialPinConfig->ioTagRx[device] == hardware->rxPins[pindex].pin)) { - uartdev->rx = hardware->rxPins[pindex]; + uartdev->rxPin = hardware->rxPins[pindex]; } if (pSerialPinConfig->ioTagTx[device] && (pSerialPinConfig->ioTagTx[device] == hardware->txPins[pindex].pin)) { - uartdev->tx = hardware->txPins[pindex]; + uartdev->txPin = hardware->txPins[pindex]; } - -#if !defined(STM32F4) +#ifdef USE_SERIAL_PINSWAP // Check for swapped pins if (pSerialPinConfig->ioTagTx[device] && (pSerialPinConfig->ioTagTx[device] == hardware->rxPins[pindex].pin)) { - uartdev->tx = hardware->rxPins[pindex]; + uartdev->rxPin = hardware->rxPins[pindex]; uartdev->pinSwap = true; } if (pSerialPinConfig->ioTagRx[device] && (pSerialPinConfig->ioTagRx[device] == hardware->txPins[pindex].pin)) { - uartdev->rx = hardware->txPins[pindex]; + uartdev->txPin = hardware->txPins[pindex]; uartdev->pinSwap = true; } #endif } - if (uartdev->rx.pin || uartdev->tx.pin) { + if (uartdev->rxPin.pin || uartdev->txPin.pin) { uartdev->hardware = hardware; uartDevmap[device] = uartdev++; } diff --git a/src/main/drivers/serial_uart_stdperiph.c b/src/main/drivers/serial_uart_stdperiph.c index a77d03d3e2..c90d79337d 100644 --- a/src/main/drivers/serial_uart_stdperiph.c +++ b/src/main/drivers/serial_uart_stdperiph.c @@ -63,6 +63,18 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) { #endif } +void uartSelectPins(UARTDevice_e device, portOptions_e options) +{ + UNUSED(options); + + uartDevice_t *uartDevice = uartDevmap[device]; + + if (uartDevice) { + uartDevice->rx = uartDevice->rxPin; + uartDevice->tx = uartDevice->txPin; + } +} + void uartReconfigure(uartPort_t *uartPort) { USART_InitTypeDef USART_InitStructure; diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index fe54b453b8..6750d222b2 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -700,15 +700,17 @@ bool vtxSmartAudioInit(void) dprintf(("smartAudioInit: OK\r\n")); #endif - // Note, for SA, which uses bidirectional mode, would normally require pullups. + // Note, for SA, which uses bidirectional mode, would normally require pullups. // the SA protocol instead requires pulldowns, and therefore uses SERIAL_BIDIR_PP_PD instead of SERIAL_BIDIR_PP const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_VTX_SMARTAUDIO); if (portConfig) { portOptions_e portOptions = SERIAL_STOPBITS_2 | SERIAL_BIDIR_NOPULL; #if defined(USE_VTX_COMMON) - portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP_PD : SERIAL_UNIDIR); + portOptions |= + (vtxConfig()->halfDuplex ? SERIAL_BIDIR | SERIAL_BIDIR_PP_PD : SERIAL_UNIDIR) | + (vtxConfig()->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP); #else - portOptions = SERIAL_BIDIR; + portOptions |= SERIAL_BIDIR; #endif smartAudioSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_SMARTAUDIO, NULL, NULL, 4800, MODE_RXTX, portOptions); diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 562eb30319..988c5d1bf7 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -654,9 +654,11 @@ bool vtxTrampInit(void) if (portConfig) { portOptions_e portOptions = 0; #if defined(USE_VTX_COMMON) - portOptions = portOptions | (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR); + portOptions |= + (vtxConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | + (vtxConfig()->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP); #else - portOptions = SERIAL_BIDIR; + portOptions |= SERIAL_BIDIR; #endif trampSerialPort = openSerialPort(portConfig->identifier, FUNCTION_VTX_TRAMP, NULL, NULL, 9600, MODE_RXTX, portOptions); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 55c3ae03bc..ad1dc4e1ee 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -803,6 +803,7 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce sbufWriteU16(dst, escSensorConfig()->hw4_current_offset); sbufWriteU8(dst, escSensorConfig()->hw4_current_gain); sbufWriteU8(dst, escSensorConfig()->hw4_voltage_gain); + sbufWriteU8(dst, escSensorConfig()->pinSwap); break; case MSP_ESC_PARAMETERS: @@ -1535,6 +1536,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) sbufWriteU32(dst, 0); sbufWriteU8(dst, 0); #endif + sbufWriteU8(dst, rxConfig()->pinSwap); break; case MSP_FAILSAFE_CONFIG: @@ -1578,6 +1580,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, telemetryConfig()->telemetry_inverted); sbufWriteU8(dst, telemetryConfig()->halfDuplex); sbufWriteU32(dst, telemetryConfig()->enableSensors); + sbufWriteU8(dst, telemetryConfig()->pinSwap); break; case MSP_SERIAL_CONFIG: @@ -2650,6 +2653,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, escSensorConfigMutable()->hw4_current_offset = sbufReadU16(src); escSensorConfigMutable()->hw4_current_gain = sbufReadU8(src); escSensorConfigMutable()->hw4_voltage_gain = sbufReadU8(src); + if (sbufBytesRemaining(src) >= 1) { + escSensorConfigMutable()->pinSwap = sbufReadU8(src); + } break; case MSP_SET_ESC_PARAMETERS: @@ -3016,6 +3022,14 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, rxConfigMutable()->halfDuplex = sbufReadU8(src); rxConfigMutable()->rx_pulse_min = sbufReadU16(src); rxConfigMutable()->rx_pulse_max = sbufReadU16(src); + if (sbufBytesRemaining(src) >= 6) { + sbufReadU8(src); + sbufReadU32(src); + sbufReadU8(src); + } + if (sbufBytesRemaining(src) >= 1) { + rxConfigMutable()->pinSwap = sbufReadU8(src); + } } #ifdef USE_RX_SPI if (sbufBytesRemaining(src) >= 6) { @@ -3072,6 +3086,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, telemetryConfigMutable()->telemetry_inverted = sbufReadU8(src); telemetryConfigMutable()->halfDuplex = sbufReadU8(src); telemetryConfigMutable()->enableSensors = sbufReadU32(src); + if (sbufBytesRemaining(src) >= 1) { + telemetryConfigMutable()->pinSwap = sbufReadU8(src); + } break; case MSP_SET_SERIAL_CONFIG: diff --git a/src/main/pg/esc_sensor.c b/src/main/pg/esc_sensor.c index 4e88a95941..275753bb23 100644 --- a/src/main/pg/esc_sensor.c +++ b/src/main/pg/esc_sensor.c @@ -27,6 +27,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, PG_ESC_SENSO PG_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, .protocol = ESC_SENSOR_PROTO_NONE, .halfDuplex = 0, + .pinSwap = 0, .update_hz = ESC_SENSOR_TASK_FREQ_HZ, .current_offset = 0, .hw4_current_offset = 0, diff --git a/src/main/pg/esc_sensor.h b/src/main/pg/esc_sensor.h index 60f98316c6..0ceb9f031b 100644 --- a/src/main/pg/esc_sensor.h +++ b/src/main/pg/esc_sensor.h @@ -43,6 +43,7 @@ enum { typedef struct { uint8_t protocol; // ESC telemetry protocol uint8_t halfDuplex; // Set to false to listen on the TX pin for telemetry data + uint8_t pinSwap; // Swap rx and tx pins around compared to the resource settings uint16_t update_hz; // Update frequency uint16_t current_offset; // Offset (extra current) consumed by the VTX / cam (mA) uint16_t hw4_current_offset; // HobbyWing V4 raw current offset diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index 41a2f0d2d1..a4bff7fdc3 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -40,6 +40,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig) .serialrx_provider = SERIALRX_PROVIDER, .serialrx_inverted = 0, .halfDuplex = 0, + .pinSwap = 0, .rx_pulse_min = RX_PWM_PULSE_MIN, .rx_pulse_max = RX_PWM_PULSE_MAX, .rssi_src_frame_errors = false, diff --git a/src/main/pg/rx.h b/src/main/pg/rx.h index f7d3a6a2d7..39a9ab4be1 100644 --- a/src/main/pg/rx.h +++ b/src/main/pg/rx.h @@ -31,6 +31,7 @@ typedef struct rxConfig_s { uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Must be enabled by FEATURE_RX_SERIAL first. uint8_t serialrx_inverted; // invert the serial RX protocol compared to it's default setting uint8_t halfDuplex; // allow rx to operate in half duplex mode on F4, ignored for F1 and F3. + uint8_t pinSwap; // swap rx and tx pins around compared to the resource settings uint16_t rx_pulse_min; // Absolute minimum pulse accepted uint16_t rx_pulse_max; // Absolute maximum pulse accepted uint8_t rssi_channel; diff --git a/src/main/pg/telemetry.c b/src/main/pg/telemetry.c index c960383f57..de13595eeb 100644 --- a/src/main/pg/telemetry.c +++ b/src/main/pg/telemetry.c @@ -34,6 +34,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .telemetry_inverted = false, .halfDuplex = 1, + .pinSwap = 0, .gpsNoFixLatitude = 0, .gpsNoFixLongitude = 0, .frsky_coordinate_format = FRSKY_FORMAT_DMS, diff --git a/src/main/pg/telemetry.h b/src/main/pg/telemetry.h index f08fbd351a..87d91baa8d 100644 --- a/src/main/pg/telemetry.h +++ b/src/main/pg/telemetry.h @@ -64,6 +64,7 @@ typedef struct { int16_t gpsNoFixLongitude; uint8_t telemetry_inverted; uint8_t halfDuplex; + uint8_t pinSwap; uint8_t frsky_coordinate_format; uint8_t frsky_unit; uint8_t frsky_vfas_precision; diff --git a/src/main/pg/vtx.c b/src/main/pg/vtx.c index 291091e3fb..52ef7a671f 100644 --- a/src/main/pg/vtx.c +++ b/src/main/pg/vtx.c @@ -30,7 +30,8 @@ PG_REGISTER_WITH_RESET_FN(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTING PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 1); PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, - .halfDuplex = true + .halfDuplex = 1, + .pinSwap = 0 ); #endif \ No newline at end of file diff --git a/src/main/pg/vtx.h b/src/main/pg/vtx.h index 873216f97a..3d1c9ccf2a 100644 --- a/src/main/pg/vtx.h +++ b/src/main/pg/vtx.h @@ -49,6 +49,7 @@ typedef struct { typedef struct { vtxChannelActivationCondition_t vtxChannelActivationConditions[MAX_CHANNEL_ACTIVATION_CONDITION_COUNT]; uint8_t halfDuplex; + uint8_t pinSwap; } vtxConfig_t; PG_DECLARE(vtxConfig_t, vtxConfig); diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index 4bd0e8e472..9d0e37dcc7 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -408,7 +408,10 @@ bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) NULL, FPORT_BAUDRATE, MODE_RXTX, - FPORT_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0) + FPORT_PORT_OPTIONS | + (rxConfig->serialrx_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) | + (rxConfig->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | + (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP) ); if (fportPort) { diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index 753e25b8d5..5f4b3eba26 100644 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -232,7 +232,9 @@ bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) NULL, IBUS_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, - (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex || portShared ? SERIAL_BIDIR : 0) + (rxConfig->serialrx_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) | + (rxConfig->halfDuplex || portShared ? SERIAL_BIDIR : SERIAL_UNIDIR) | + (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP) ); #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS) diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index a63889376f..d0512dcf8c 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -198,7 +198,10 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) &sbusFrameData, sbusBaudRate, portShared ? MODE_RXTX : MODE_RX, - SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0) + SBUS_PORT_OPTIONS | + (rxConfig->serialrx_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED) | + (rxConfig->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | + (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP) ); if (rxConfig->rssi_src_frame_errors) { diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c index 8333b2ecce..248ed53f78 100644 --- a/src/main/rx/spektrum.c +++ b/src/main/rx/spektrum.c @@ -400,8 +400,9 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) rxRuntimeState, SPEKTRUM_BAUDRATE, portShared || srxlEnabled ? MODE_RXTX : MODE_RX, - (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | - ((srxlEnabled || rxConfig->halfDuplex) ? SERIAL_BIDIR : 0) + (rxConfig->serialrx_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) | + ((rxConfig->halfDuplex || srxlEnabled) ? SERIAL_BIDIR : SERIAL_UNIDIR) | + (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP) ); #if defined(USE_TELEMETRY_SRXL) diff --git a/src/main/rx/srxl2.c b/src/main/rx/srxl2.c index 9406e1e2c3..979bede0bd 100644 --- a/src/main/rx/srxl2.c +++ b/src/main/rx/srxl2.c @@ -500,13 +500,10 @@ bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) return false; } - portOptions_e options = SRXL2_PORT_OPTIONS; - if (rxConfig->serialrx_inverted) { - options |= SERIAL_INVERTED; - } - if (rxConfig->halfDuplex) { - options |= SERIAL_BIDIR; - } + portOptions_e options = SRXL2_PORT_OPTIONS | + (rxConfig->serialrx_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) | + (rxConfig->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | + (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP); serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, srxl2DataReceive, NULL, SRXL2_PORT_BAUDRATE_DEFAULT, SRXL2_PORT_MODE, options); diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 76a60b4496..daf92b2481 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -195,7 +195,9 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) NULL, SUMD_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, - (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0) + (rxConfig->serialrx_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) | + (rxConfig->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | + (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP) ); #ifdef USE_TELEMETRY diff --git a/src/main/rx/xbus.c b/src/main/rx/xbus.c index 3981bd4c4a..0fb64d4d3f 100644 --- a/src/main/rx/xbus.c +++ b/src/main/rx/xbus.c @@ -314,7 +314,9 @@ bool xBusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) NULL, baudRate, portShared ? MODE_RXTX : MODE_RX, - (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0) + (rxConfig->serialrx_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) | + (rxConfig->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | + (rxConfig->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP) ); #ifdef USE_TELEMETRY diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index f0edb88062..b4678980d6 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -1541,8 +1541,8 @@ static bool pl5ParamCommit(uint8_t cmd) return false; // params dirty? - if (memcmp(paramPayload + PL5_RESP_DEVINFO_PAYLOAD_LENGTH, - paramUpdPayload + PL5_RESP_DEVINFO_PAYLOAD_LENGTH, + if (memcmp(paramPayload + PL5_RESP_DEVINFO_PAYLOAD_LENGTH, + paramUpdPayload + PL5_RESP_DEVINFO_PAYLOAD_LENGTH, PL5_RESP_GETPARAMS_PAYLOAD_LENGTH) != 0) { // set dirty flag, will schedule read pl5DirtyParams = true; @@ -2803,7 +2803,9 @@ bool INIT_CODE escSensorInit(void) return false; } - options = SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_NOT_INVERTED | (escHalfDuplex ? SERIAL_BIDIR : 0); + options = SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_NOT_INVERTED | + (escHalfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | + (escSensorConfig()->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP); switch (escSensorConfig()->protocol) { case ESC_SENSOR_PROTO_BLHELI32: diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 31ad214b98..b3fab1ea04 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -224,6 +224,10 @@ extern uint8_t _dmaram_end__; #define USE_DMA #define USE_TIMER +#ifndef STM32F4 +#define USE_SERIAL_PINSWAP +#endif + #define USE_CLI #define USE_SERIAL_PASSTHROUGH #define USE_GYRO_REGISTER_DUMP // Adds gyroregisters command to cli to dump configured register values diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index 61c7fcef30..d2a034f7fe 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -371,11 +371,10 @@ static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_e { closeSerialPort(hottPort); - portOptions_e portOptions = telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED; - - if (telemetryConfig()->halfDuplex) { - portOptions |= SERIAL_BIDIR; - } + portOptions_e portOptions = + (telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED) | + (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | + (telemetryConfig()->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP); hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, NULL, HOTT_BAUDRATE, mode, portOptions); } @@ -416,11 +415,9 @@ void configureHoTTTelemetryPort(void) return; } - portOptions_e portOptions = SERIAL_NOT_INVERTED; - - if (telemetryConfig()->halfDuplex) { - portOptions |= SERIAL_BIDIR; - } + portOptions_e portOptions = SERIAL_NOT_INVERTED | + (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | + (telemetryConfig()->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP); hottPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_HOTT, NULL, NULL, HOTT_BAUDRATE, HOTT_PORT_MODE, portOptions); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 82f16cc4cf..3871beb7dd 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -502,7 +502,10 @@ static void freeSmartPortTelemetryPort(void) static void configureSmartPortTelemetryPort(void) { if (portConfig) { - portOptions_e portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED); + portOptions_e portOptions = + (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED) | + (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | + (telemetryConfig()->pinSwap ? SERIAL_PINSWAP : SERIAL_NOSWAP); smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions); }