diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index ddf9c59056..ffc1df7f62 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -62,6 +62,8 @@ #ifdef BLACKBOX +#define BLACKBOX_SERIAL_PORT_MODE MODE_TX + // How many bytes should we transmit per loop iteration? uint8_t blackboxWriteChunkSize = 16; @@ -446,7 +448,7 @@ bool blackboxDeviceOpen(void) { serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_BLACKBOX); baudRate_e baudRateIndex; - portMode_t portMode; + portOptions_t portOptions = SERIAL_PARITY_NO | SERIAL_NOT_INVERTED; if (!portConfig) { return false; @@ -455,18 +457,18 @@ bool blackboxDeviceOpen(void) blackboxPortSharing = determinePortSharing(portConfig, FUNCTION_BLACKBOX); baudRateIndex = portConfig->blackbox_baudrateIndex; - portMode = MODE_TX; - if (baudRates[baudRateIndex] == 230400) { /* * OpenLog's 230400 baud rate is very inaccurate, so it requires a larger inter-character gap in * order to maintain synchronization. */ - portMode |= MODE_STOPBITS2; + portOptions |= SERIAL_STOPBITS_2; + } else { + portOptions |= SERIAL_STOPBITS_1; } blackboxPort = openSerialPort(portConfig->identifier, FUNCTION_BLACKBOX, NULL, baudRates[baudRateIndex], - portMode, SERIAL_NOT_INVERTED); + BLACKBOX_SERIAL_PORT_MODE, portOptions); return blackboxPort != NULL; } diff --git a/src/main/config/config.c b/src/main/config/config.c index 4fb440db42..ff2bf1bbc8 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -240,7 +240,7 @@ void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) void resetTelemetryConfig(telemetryConfig_t *telemetryConfig) { - telemetryConfig->telemetry_inversion = SERIAL_NOT_INVERTED; + telemetryConfig->telemetry_inversion = 0; telemetryConfig->telemetry_switch = 0; telemetryConfig->gpsNoFixLatitude = 0; telemetryConfig->gpsNoFixLongitude = 0; diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index a6d6836ab2..d197991f77 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -17,20 +17,23 @@ #pragma once -typedef enum { - SERIAL_NOT_INVERTED = 0, - SERIAL_INVERTED -} serialInversion_e; - typedef enum portMode_t { MODE_RX = 1 << 0, MODE_TX = 1 << 1, - MODE_RXTX = MODE_RX | MODE_TX, - MODE_SBUS = 1 << 2, - MODE_BIDIR = 1 << 3, - MODE_STOPBITS2 = 1 << 4, + MODE_RXTX = MODE_RX | MODE_TX } portMode_t; +typedef enum portOptions_t { + 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 +} portOptions_t; + typedef void (*serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app typedef struct serialPort { @@ -39,7 +42,8 @@ typedef struct serialPort { uint8_t identifier; portMode_t mode; - serialInversion_e inversion; + portOptions_t options; + uint32_t baudRate; uint32_t rxBufferSize; diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index 82a0eb4689..2490c83499 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -52,7 +52,7 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture); void setTxSignal(softSerial_t *softSerial, uint8_t state) { - if (softSerial->port.inversion == SERIAL_INVERTED) { + if (softSerial->port.options & SERIAL_INVERTED) { state = !state; } @@ -119,10 +119,10 @@ static void serialICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) TIM_ICInit(tim, &TIM_ICInitStructure); } -static void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference, serialInversion_e inversion) +static void serialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_t options) { // start bit is usually a FALLING signal - serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, inversion == SERIAL_INVERTED ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); + serialICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, (options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); timerChCCHandlerInit(&softSerialPorts[reference].edgeCb, onSerialRxPinChange); timerChConfigCallbacks(timerHardwarePtr, &softSerialPorts[reference].edgeCb, NULL); } @@ -145,7 +145,7 @@ static void resetBuffers(softSerial_t *softSerial) softSerial->port.txBufferHead = 0; } -serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint32_t baud, serialInversion_e inversion) +serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint32_t baud, portOptions_t options) { softSerial_t *softSerial = &(softSerialPorts[portIndex]); @@ -166,7 +166,7 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb softSerial->port.vTable = softSerialVTable; softSerial->port.baudRate = baud; softSerial->port.mode = MODE_RXTX; - softSerial->port.inversion = inversion; + softSerial->port.options = options; softSerial->port.callback = callback; resetBuffers(softSerial); @@ -188,7 +188,7 @@ serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallb delay(50); serialTimerTxConfig(softSerial->txTimerHardware, portIndex, baud); - serialTimerRxConfig(softSerial->rxTimerHardware, portIndex, inversion); + serialTimerRxConfig(softSerial->rxTimerHardware, portIndex, options); return &softSerial->port; } @@ -258,7 +258,7 @@ void prepareForNextRxByte(softSerial_t *softSerial) serialICConfig( softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, - softSerial->port.inversion == SERIAL_INVERTED ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling + (softSerial->port.options & SERIAL_INVERTED) ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling ); } } @@ -328,6 +328,7 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) UNUSED(capture); softSerial_t *softSerial = container_of(cbRec, softSerial_t, edgeCb); + bool inverted = softSerial->port.options & SERIAL_INVERTED; if ((softSerial->port.mode & MODE_RX) == 0) { return; @@ -342,7 +343,7 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) softSerial->transmissionErrors++; } - serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, softSerial->port.inversion == SERIAL_INVERTED ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); + serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); softSerial->rxEdge = LEADING; softSerial->rxBitIndex = 0; @@ -360,10 +361,10 @@ void onSerialRxPinChange(timerCCHandlerRec_t *cbRec, captureCompare_t capture) if (softSerial->rxEdge == TRAILING) { softSerial->rxEdge = LEADING; - serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, softSerial->port.inversion == SERIAL_INVERTED ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); + serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Falling : TIM_ICPolarity_Rising); } else { softSerial->rxEdge = TRAILING; - serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, softSerial->port.inversion == SERIAL_INVERTED ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); + serialICConfig(softSerial->rxTimerHardware->tim, softSerial->rxTimerHardware->channel, inverted ? TIM_ICPolarity_Rising : TIM_ICPolarity_Falling); } } @@ -408,7 +409,7 @@ void softSerialWriteByte(serialPort_t *s, uint8_t ch) void softSerialSetBaudRate(serialPort_t *s, uint32_t baudRate) { softSerial_t *softSerial = (softSerial_t *)s; - openSoftSerial(softSerial->softSerialPortIndex, s->callback, baudRate, softSerial->port.inversion); + openSoftSerial(softSerial->softSerialPortIndex, s->callback, baudRate, softSerial->port.options); } void softSerialSetMode(serialPort_t *instance, portMode_t mode) diff --git a/src/main/drivers/serial_softserial.h b/src/main/drivers/serial_softserial.h index 89eda56c3a..17df635df3 100644 --- a/src/main/drivers/serial_softserial.h +++ b/src/main/drivers/serial_softserial.h @@ -58,7 +58,7 @@ extern softSerial_t softSerialPorts[]; extern const struct serialPortVTable softSerialVTable[]; -serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint32_t baud, serialInversion_e inversion); +serialPort_t *openSoftSerial(softSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint32_t baud, portOptions_t options); // serialPort API void softSerialWriteByte(serialPort_t *instance, uint8_t ch); diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index c7b8bc4d64..19b5c0bb51 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -39,10 +39,11 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) { #if !defined(INVERTER) && !defined(STM32F303xC) UNUSED(uartPort); -#endif +#else + bool inverted = uartPort->port.options & SERIAL_INVERTED; #ifdef INVERTER - if (uartPort->port.inversion == SERIAL_INVERTED && uartPort->USARTx == INVERTER_USART) { + if (inverted && uartPort->USARTx == INVERTER_USART) { // Enable hardware inverter if available. INVERTER_ON; } @@ -51,18 +52,23 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) { #ifdef STM32F303xC uint32_t inversionPins = 0; - if (uartPort->port.mode & MODE_TX) { - inversionPins |= USART_InvPin_Tx; - } - if (uartPort->port.mode & MODE_RX) { - inversionPins |= USART_InvPin_Rx; + // Inversion when using OPTION_BIDIR not supported yet. + if (uartPort->port.options & SERIAL_BIDIR) { + // Clear inversion on both Tx and Rx + inversionPins |= USART_InvPin_Tx | USART_InvPin_Rx; + inverted = false; + } else { + if (uartPort->port.mode & MODE_TX) { + inversionPins |= USART_InvPin_Tx; + } + if (uartPort->port.mode & MODE_RX) { + inversionPins |= USART_InvPin_Rx; + } } - // Note: inversion when using MODE_BIDIR not supported yet. - - USART_InvPinCmd(uartPort->USARTx, inversionPins, uartPort->port.inversion == SERIAL_INVERTED ? ENABLE : DISABLE); + USART_InvPinCmd(uartPort->USARTx, inversionPins, inverted ? ENABLE : DISABLE); +#endif #endif - } static void uartReconfigure(uartPort_t *uartPort) @@ -72,25 +78,16 @@ static void uartReconfigure(uartPort_t *uartPort) USART_InitStructure.USART_BaudRate = uartPort->port.baudRate; USART_InitStructure.USART_WordLength = USART_WordLength_8b; - if (uartPort->port.mode & MODE_SBUS) { - USART_InitStructure.USART_StopBits = USART_StopBits_2; - USART_InitStructure.USART_Parity = USART_Parity_Even; - } else { - if (uartPort->port.mode & MODE_STOPBITS2) - USART_InitStructure.USART_StopBits = USART_StopBits_2; - else - USART_InitStructure.USART_StopBits = USART_StopBits_1; - USART_InitStructure.USART_Parity = USART_Parity_No; - } + USART_InitStructure.USART_StopBits = (uartPort->port.options & SERIAL_STOPBITS_2) ? USART_StopBits_2 : USART_StopBits_1; + USART_InitStructure.USART_Parity = (uartPort->port.options & SERIAL_PARITY_EVEN) ? USART_Parity_Even : USART_Parity_No; + USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; USART_InitStructure.USART_Mode = 0; if (uartPort->port.mode & MODE_RX) USART_InitStructure.USART_Mode |= USART_Mode_Rx; if (uartPort->port.mode & MODE_TX) USART_InitStructure.USART_Mode |= USART_Mode_Tx; - if (uartPort->port.mode & MODE_BIDIR) - USART_InitStructure.USART_Mode |= USART_Mode_Tx | USART_Mode_Rx; USART_Init(uartPort->USARTx, &USART_InitStructure); @@ -99,19 +96,19 @@ static void uartReconfigure(uartPort_t *uartPort) USART_Cmd(uartPort->USARTx, ENABLE); } -serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion) +serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s = NULL; if (USARTx == USART1) { - s = serialUSART1(baudRate, mode); + s = serialUSART1(baudRate, mode, options); #ifdef USE_USART2 } else if (USARTx == USART2) { - s = serialUSART2(baudRate, mode); + s = serialUSART2(baudRate, mode, options); #endif #ifdef USE_USART3 } else if (USARTx == USART3) { - s = serialUSART3(baudRate, mode); + s = serialUSART3(baudRate, mode, options); #endif } else { return (serialPort_t *)s; @@ -125,13 +122,13 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, s->port.callback = callback; s->port.mode = mode; s->port.baudRate = baudRate; - s->port.inversion = inversion; + s->port.options = options; uartReconfigure(s); // Receive DMA or IRQ DMA_InitTypeDef DMA_InitStructure; - if ((mode & MODE_RX) || (mode & MODE_BIDIR)) { + if (mode & MODE_RX) { if (s->rxDMAChannel) { DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = s->rxDMAPeripheralBaseAddr; @@ -158,7 +155,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, } // Transmit DMA or IRQ - if ((mode & MODE_TX) || (mode & MODE_BIDIR)) { + if (mode & MODE_TX) { if (s->txDMAChannel) { DMA_StructInit(&DMA_InitStructure); DMA_InitStructure.DMA_PeripheralBaseAddr = s->txDMAPeripheralBaseAddr; @@ -185,7 +182,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, USART_Cmd(s->USARTx, ENABLE); - if (mode & MODE_BIDIR) + if (options & SERIAL_BIDIR) USART_HalfDuplexCmd(s->USARTx, ENABLE); else USART_HalfDuplexCmd(s->USARTx, DISABLE); diff --git a/src/main/drivers/serial_uart.h b/src/main/drivers/serial_uart.h index 6a7357d26d..6b2757f4a9 100644 --- a/src/main/drivers/serial_uart.h +++ b/src/main/drivers/serial_uart.h @@ -44,7 +44,7 @@ typedef struct { USART_TypeDef *USARTx; } uartPort_t; -serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion); +serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options); // serialPort API void uartWrite(serialPort_t *instance, uint8_t ch); diff --git a/src/main/drivers/serial_uart_impl.h b/src/main/drivers/serial_uart_impl.h index 713405bc09..7ff2deb142 100644 --- a/src/main/drivers/serial_uart_impl.h +++ b/src/main/drivers/serial_uart_impl.h @@ -23,7 +23,7 @@ extern const struct serialPortVTable uartVTable[]; void uartStartTxDMA(uartPort_t *s); -uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode); -uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode); -uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode); +uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode, portOptions_t options); +uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode, portOptions_t options); +uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode, portOptions_t options); diff --git a/src/main/drivers/serial_uart_stm32f10x.c b/src/main/drivers/serial_uart_stm32f10x.c index 02a99b3f66..09dd7520cf 100644 --- a/src/main/drivers/serial_uart_stm32f10x.c +++ b/src/main/drivers/serial_uart_stm32f10x.c @@ -84,7 +84,7 @@ void usartIrqCallback(uartPort_t *s) #ifdef USE_USART1 // USART1 - Telemetry (RX/TX by DMA) -uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) +uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s; static volatile uint8_t rx1Buffer[UART1_RX_BUFFER_SIZE]; @@ -118,17 +118,23 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) // USART1_TX PA9 // USART1_RX PA10 gpio.speed = Speed_2MHz; + gpio.pin = Pin_9; - gpio.mode = Mode_AF_PP; - if (mode & MODE_TX) - gpioInit(GPIOA, &gpio); - gpio.mode = Mode_AF_OD; - if (mode & MODE_BIDIR) - gpioInit(GPIOA, &gpio); - gpio.pin = Pin_10; - gpio.mode = Mode_IPU; - if (mode & MODE_RX) + if (options & SERIAL_BIDIR) { + gpio.mode = Mode_AF_OD; gpioInit(GPIOA, &gpio); + } else { + if (mode & MODE_TX) { + gpio.mode = Mode_AF_PP; + gpioInit(GPIOA, &gpio); + } + + if (mode & MODE_RX) { + gpio.pin = Pin_10; + gpio.mode = Mode_IPU; + gpioInit(GPIOA, &gpio); + } + } // DMA TX Interrupt NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel4_IRQn; @@ -174,7 +180,7 @@ void USART1_IRQHandler(void) #ifdef USE_USART2 // USART2 - GPS or Spektrum or ?? (RX + TX by IRQ) -uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode) +uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s; static volatile uint8_t rx2Buffer[UART2_RX_BUFFER_SIZE]; @@ -203,17 +209,23 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode) // USART2_TX PA2 // USART2_RX PA3 gpio.speed = Speed_2MHz; + gpio.pin = Pin_2; - gpio.mode = Mode_AF_PP; - if (mode & MODE_TX) - gpioInit(GPIOA, &gpio); - gpio.mode = Mode_AF_OD; - if (mode & MODE_BIDIR) - gpioInit(GPIOA, &gpio); - gpio.pin = Pin_3; - gpio.mode = Mode_IPU; - if (mode & MODE_RX) + if (options & SERIAL_BIDIR) { + gpio.mode = Mode_AF_OD; gpioInit(GPIOA, &gpio); + } else { + if (mode & MODE_TX) { + gpio.mode = Mode_AF_PP; + gpioInit(GPIOA, &gpio); + } + + if (mode & MODE_RX) { + gpio.pin = Pin_3; + gpio.mode = Mode_IPU; + gpioInit(GPIOA, &gpio); + } + } // RX/TX Interrupt NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn; @@ -237,7 +249,7 @@ void USART2_IRQHandler(void) #ifdef USE_USART3 // USART3 -uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode) +uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s; static volatile uint8_t rx3Buffer[UART3_RX_BUFFER_SIZE]; @@ -268,17 +280,23 @@ uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode) #endif gpio.speed = Speed_2MHz; + gpio.pin = USART3_TX_PIN; - gpio.mode = Mode_AF_PP; - if (mode & MODE_TX) - gpioInit(USART3_GPIO, &gpio); - gpio.mode = Mode_AF_OD; - if (mode & MODE_BIDIR) - gpioInit(USART3_GPIO, &gpio); - gpio.pin = USART3_RX_PIN; - gpio.mode = Mode_IPU; - if (mode & MODE_RX) + if (options & SERIAL_BIDIR) { + gpio.mode = Mode_AF_OD; gpioInit(USART3_GPIO, &gpio); + } else { + if (mode & MODE_TX) { + gpio.mode = Mode_AF_PP; + gpioInit(USART3_GPIO, &gpio); + } + + if (mode & MODE_RX) { + gpio.pin = USART3_RX_PIN; + gpio.mode = Mode_IPU; + gpioInit(USART3_GPIO, &gpio); + } + } // RX/TX Interrupt NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn; diff --git a/src/main/drivers/serial_uart_stm32f30x.c b/src/main/drivers/serial_uart_stm32f30x.c index 6565e27fb0..fa16225616 100644 --- a/src/main/drivers/serial_uart_stm32f30x.c +++ b/src/main/drivers/serial_uart_stm32f30x.c @@ -82,7 +82,7 @@ static uartPort_t uartPort3; #endif #ifdef USE_USART1 -uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) +uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s; static volatile uint8_t rx1Buffer[UART1_RX_BUFFER_SIZE]; @@ -118,23 +118,23 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; - if (mode & MODE_TX) { - GPIO_InitStructure.GPIO_Pin = UART1_TX_PIN; - GPIO_PinAFConfig(UART1_GPIO, UART1_TX_PINSOURCE, UART1_GPIO_AF); - GPIO_Init(UART1_GPIO, &GPIO_InitStructure); - } - - if (mode & MODE_RX) { - GPIO_InitStructure.GPIO_Pin = UART1_RX_PIN; - GPIO_PinAFConfig(UART1_GPIO, UART1_RX_PINSOURCE, UART1_GPIO_AF); - GPIO_Init(UART1_GPIO, &GPIO_InitStructure); - } - - if (mode & MODE_BIDIR) { + if (options & SERIAL_BIDIR) { GPIO_InitStructure.GPIO_Pin = UART1_TX_PIN; GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; GPIO_PinAFConfig(UART1_GPIO, UART1_TX_PINSOURCE, UART1_GPIO_AF); GPIO_Init(UART1_GPIO, &GPIO_InitStructure); + } else { + if (mode & MODE_TX) { + GPIO_InitStructure.GPIO_Pin = UART1_TX_PIN; + GPIO_PinAFConfig(UART1_GPIO, UART1_TX_PINSOURCE, UART1_GPIO_AF); + GPIO_Init(UART1_GPIO, &GPIO_InitStructure); + } + + if (mode & MODE_RX) { + GPIO_InitStructure.GPIO_Pin = UART1_RX_PIN; + GPIO_PinAFConfig(UART1_GPIO, UART1_RX_PINSOURCE, UART1_GPIO_AF); + GPIO_Init(UART1_GPIO, &GPIO_InitStructure); + } } // DMA TX Interrupt @@ -157,7 +157,7 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode) #endif #ifdef USE_USART2 -uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode) +uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s; static volatile uint8_t rx2Buffer[UART2_RX_BUFFER_SIZE]; @@ -197,23 +197,23 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode) GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; - if (mode & MODE_TX) { - GPIO_InitStructure.GPIO_Pin = UART2_TX_PIN; - GPIO_PinAFConfig(UART2_GPIO, UART2_TX_PINSOURCE, UART2_GPIO_AF); - GPIO_Init(UART2_GPIO, &GPIO_InitStructure); - } - - if (mode & MODE_RX) { - GPIO_InitStructure.GPIO_Pin = UART2_RX_PIN; - GPIO_PinAFConfig(UART2_GPIO, UART2_RX_PINSOURCE, UART2_GPIO_AF); - GPIO_Init(UART2_GPIO, &GPIO_InitStructure); - } - - if (mode & MODE_BIDIR) { + if (options & SERIAL_BIDIR) { GPIO_InitStructure.GPIO_Pin = UART2_TX_PIN; GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; GPIO_PinAFConfig(UART2_GPIO, UART2_TX_PINSOURCE, UART2_GPIO_AF); GPIO_Init(UART2_GPIO, &GPIO_InitStructure); + } else { + if (mode & MODE_TX) { + GPIO_InitStructure.GPIO_Pin = UART2_TX_PIN; + GPIO_PinAFConfig(UART2_GPIO, UART2_TX_PINSOURCE, UART2_GPIO_AF); + GPIO_Init(UART2_GPIO, &GPIO_InitStructure); + } + + if (mode & MODE_RX) { + GPIO_InitStructure.GPIO_Pin = UART2_RX_PIN; + GPIO_PinAFConfig(UART2_GPIO, UART2_RX_PINSOURCE, UART2_GPIO_AF); + GPIO_Init(UART2_GPIO, &GPIO_InitStructure); + } } #ifdef USE_USART2_TX_DMA @@ -238,7 +238,7 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode) #endif #ifdef USE_USART3 -uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode) +uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode, portOptions_t options) { uartPort_t *s; static volatile uint8_t rx3Buffer[UART3_RX_BUFFER_SIZE]; @@ -278,23 +278,23 @@ uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode) GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; - if (mode & MODE_TX) { - GPIO_InitStructure.GPIO_Pin = UART3_TX_PIN; - GPIO_PinAFConfig(UART3_GPIO, UART3_TX_PINSOURCE, UART3_GPIO_AF); - GPIO_Init(UART3_GPIO, &GPIO_InitStructure); - } - - if (mode & MODE_RX) { - GPIO_InitStructure.GPIO_Pin = UART3_RX_PIN; - GPIO_PinAFConfig(UART3_GPIO, UART3_RX_PINSOURCE, UART3_GPIO_AF); - GPIO_Init(UART3_GPIO, &GPIO_InitStructure); - } - - if (mode & MODE_BIDIR) { + if (options & SERIAL_BIDIR) { GPIO_InitStructure.GPIO_Pin = UART3_TX_PIN; GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; GPIO_PinAFConfig(UART3_GPIO, UART3_TX_PINSOURCE, UART3_GPIO_AF); GPIO_Init(UART3_GPIO, &GPIO_InitStructure); + } else { + if (mode & MODE_TX) { + GPIO_InitStructure.GPIO_Pin = UART3_TX_PIN; + GPIO_PinAFConfig(UART3_GPIO, UART3_TX_PINSOURCE, UART3_GPIO_AF); + GPIO_Init(UART3_GPIO, &GPIO_InitStructure); + } + + if (mode & MODE_RX) { + GPIO_InitStructure.GPIO_Pin = UART3_RX_PIN; + GPIO_PinAFConfig(UART3_GPIO, UART3_RX_PINSOURCE, UART3_GPIO_AF); + GPIO_Init(UART3_GPIO, &GPIO_InitStructure); + } } #ifdef USE_USART3_TX_DMA diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 4f0ab6eea7..377d326229 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -238,7 +238,7 @@ serialPort_t *openSerialPort( serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, - serialInversion_e inversion) + portOptions_t options) { serialPortUsage_t *serialPortUsage = findSerialPortUsageByIdentifier(identifier); if (serialPortUsage->function != FUNCTION_NONE) { @@ -256,28 +256,28 @@ serialPort_t *openSerialPort( #endif #ifdef USE_USART1 case SERIAL_PORT_USART1: - serialPort = uartOpen(USART1, callback, baudRate, mode, inversion); + serialPort = uartOpen(USART1, callback, baudRate, mode, options); break; #endif #ifdef USE_USART2 case SERIAL_PORT_USART2: - serialPort = uartOpen(USART2, callback, baudRate, mode, inversion); + serialPort = uartOpen(USART2, callback, baudRate, mode, options); break; #endif #ifdef USE_USART3 case SERIAL_PORT_USART3: - serialPort = uartOpen(USART3, callback, baudRate, mode, inversion); + serialPort = uartOpen(USART3, callback, baudRate, mode, options); break; #endif #ifdef USE_SOFTSERIAL1 case SERIAL_PORT_SOFTSERIAL1: - serialPort = openSoftSerial(SOFTSERIAL1, callback, baudRate, inversion); + serialPort = openSoftSerial(SOFTSERIAL1, callback, baudRate, options); serialSetMode(serialPort, mode); break; #endif #ifdef USE_SOFTSERIAL2 case SERIAL_PORT_SOFTSERIAL2: - serialPort = openSoftSerial(SOFTSERIAL2, callback, baudRate, inversion); + serialPort = openSoftSerial(SOFTSERIAL2, callback, baudRate, options); serialSetMode(serialPort, mode); break; #endif diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 385849a254..e02bf83182 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -114,7 +114,7 @@ serialPort_t *openSerialPort( serialReceiveCallbackPtr callback, uint32_t baudrate, portMode_t mode, - serialInversion_e inversion + portOptions_t options ); void closeSerialPort(serialPort_t *serialPort); diff --git a/src/main/rx/sbus.c b/src/main/rx/sbus.c index 53e9bf272b..cad62c9c8a 100644 --- a/src/main/rx/sbus.c +++ b/src/main/rx/sbus.c @@ -68,6 +68,7 @@ static uint16_t sbusStateFlags = 0; #define SBUS_FRAME_END_BYTE 0x00 #define SBUS_BAUDRATE 100000 +#define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN | SERIAL_INVERTED) #define SBUS_DIGITAL_CHANNEL_MIN 173 #define SBUS_DIGITAL_CHANNEL_MAX 1812 @@ -92,7 +93,7 @@ bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRa return false; } - serialPort_t *sBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sbusDataReceive, SBUS_BAUDRATE, (portMode_t)(MODE_RX | MODE_SBUS), SERIAL_INVERTED); + serialPort_t *sBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sbusDataReceive, SBUS_BAUDRATE, MODE_RX, SBUS_PORT_OPTIONS); return sBusPort != NULL; } diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 5a6bca3aef..0813d0e8c8 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -433,7 +433,7 @@ void configureFrSkyTelemetryPort(void) return; } - frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion); + frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED); if (!frskyPort) { return; } diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 58d855645b..bf55828fbd 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -134,7 +134,7 @@ const uint16_t frSkyDataIdTable[] = { #define __USE_C99_MATH // for roundf() #define SMARTPORT_BAUD 57600 -#define SMARTPORT_UART_MODE MODE_BIDIR +#define SMARTPORT_UART_MODE MODE_RXTX #define SMARTPORT_SERVICE_DELAY_MS 5 // telemetry requests comes in at roughly 12 ms intervals, keep this under that #define SMARTPORT_NOT_CONNECTED_TIMEOUT_MS 7000 @@ -229,11 +229,19 @@ void freeSmartPortTelemetryPort(void) void configureSmartPortTelemetryPort(void) { + portOptions_t portOptions; + if (!portConfig) { return; } - smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, telemetryConfig->telemetry_inversion); + portOptions = SERIAL_BIDIR; + + if (telemetryConfig->telemetry_inversion) { + portOptions |= SERIAL_INVERTED; + } + + smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions); if (!smartPortSerialPort) { return; diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 010331ac2f..96b91713bc 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -37,7 +37,7 @@ typedef enum { typedef struct telemetryConfig_s { uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. - serialInversion_e telemetry_inversion; // also shared with smartport inversion + uint8_t telemetry_inversion; // also shared with smartport inversion float gpsNoFixLatitude; float gpsNoFixLongitude; frskyGpsCoordFormat_e frsky_coordinate_format; diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index 85edf2a3b1..27247f9bab 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -187,13 +187,13 @@ void serialSetMode(serialPort_t *instance, portMode_t mode) { } -serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion) { +serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options) { UNUSED(identifier); UNUSED(functionMask); UNUSED(baudRate); UNUSED(callback); UNUSED(mode); - UNUSED(inversion); + UNUSED(options); return NULL; }