mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 12:55:19 +03:00
Change port mode MODE_BIDIR into a port option instead
This commit is contained in:
parent
344e8fbf04
commit
3c543d36c8
6 changed files with 124 additions and 94 deletions
|
@ -20,8 +20,7 @@
|
||||||
typedef enum portMode_t {
|
typedef enum portMode_t {
|
||||||
MODE_RX = 1 << 0,
|
MODE_RX = 1 << 0,
|
||||||
MODE_TX = 1 << 1,
|
MODE_TX = 1 << 1,
|
||||||
MODE_RXTX = MODE_RX | MODE_TX,
|
MODE_RXTX = MODE_RX | MODE_TX
|
||||||
MODE_BIDIR = 1 << 3
|
|
||||||
} portMode_t;
|
} portMode_t;
|
||||||
|
|
||||||
typedef enum portOptions_t {
|
typedef enum portOptions_t {
|
||||||
|
@ -31,6 +30,8 @@ typedef enum portOptions_t {
|
||||||
SERIAL_STOPBITS_2 = 1 << 1,
|
SERIAL_STOPBITS_2 = 1 << 1,
|
||||||
SERIAL_PARITY_NO = 0 << 2,
|
SERIAL_PARITY_NO = 0 << 2,
|
||||||
SERIAL_PARITY_EVEN = 1 << 2,
|
SERIAL_PARITY_EVEN = 1 << 2,
|
||||||
|
SERIAL_UNIDIR = 0 << 3,
|
||||||
|
SERIAL_BIDIR = 1 << 3
|
||||||
} portOptions_t;
|
} portOptions_t;
|
||||||
|
|
||||||
typedef void (*serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app
|
typedef void (*serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app
|
||||||
|
|
|
@ -52,14 +52,19 @@ static void usartConfigurePinInversion(uartPort_t *uartPort) {
|
||||||
#ifdef STM32F303xC
|
#ifdef STM32F303xC
|
||||||
uint32_t inversionPins = 0;
|
uint32_t inversionPins = 0;
|
||||||
|
|
||||||
if (uartPort->port.mode & MODE_TX) {
|
// Inversion when using OPTION_BIDIR not supported yet.
|
||||||
inversionPins |= USART_InvPin_Tx;
|
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;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
if (uartPort->port.mode & MODE_RX) {
|
|
||||||
inversionPins |= USART_InvPin_Rx;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Note: inversion when using MODE_BIDIR not supported yet.
|
|
||||||
|
|
||||||
USART_InvPinCmd(uartPort->USARTx, inversionPins, inverted ? ENABLE : DISABLE);
|
USART_InvPinCmd(uartPort->USARTx, inversionPins, inverted ? ENABLE : DISABLE);
|
||||||
#endif
|
#endif
|
||||||
|
@ -83,8 +88,6 @@ static void uartReconfigure(uartPort_t *uartPort)
|
||||||
USART_InitStructure.USART_Mode |= USART_Mode_Rx;
|
USART_InitStructure.USART_Mode |= USART_Mode_Rx;
|
||||||
if (uartPort->port.mode & MODE_TX)
|
if (uartPort->port.mode & MODE_TX)
|
||||||
USART_InitStructure.USART_Mode |= USART_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);
|
USART_Init(uartPort->USARTx, &USART_InitStructure);
|
||||||
|
|
||||||
|
@ -98,14 +101,14 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,
|
||||||
uartPort_t *s = NULL;
|
uartPort_t *s = NULL;
|
||||||
|
|
||||||
if (USARTx == USART1) {
|
if (USARTx == USART1) {
|
||||||
s = serialUSART1(baudRate, mode);
|
s = serialUSART1(baudRate, mode, options);
|
||||||
#ifdef USE_USART2
|
#ifdef USE_USART2
|
||||||
} else if (USARTx == USART2) {
|
} else if (USARTx == USART2) {
|
||||||
s = serialUSART2(baudRate, mode);
|
s = serialUSART2(baudRate, mode, options);
|
||||||
#endif
|
#endif
|
||||||
#ifdef USE_USART3
|
#ifdef USE_USART3
|
||||||
} else if (USARTx == USART3) {
|
} else if (USARTx == USART3) {
|
||||||
s = serialUSART3(baudRate, mode);
|
s = serialUSART3(baudRate, mode, options);
|
||||||
#endif
|
#endif
|
||||||
} else {
|
} else {
|
||||||
return (serialPort_t *)s;
|
return (serialPort_t *)s;
|
||||||
|
@ -125,7 +128,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,
|
||||||
|
|
||||||
// Receive DMA or IRQ
|
// Receive DMA or IRQ
|
||||||
DMA_InitTypeDef DMA_InitStructure;
|
DMA_InitTypeDef DMA_InitStructure;
|
||||||
if ((mode & MODE_RX) || (mode & MODE_BIDIR)) {
|
if (mode & MODE_RX) {
|
||||||
if (s->rxDMAChannel) {
|
if (s->rxDMAChannel) {
|
||||||
DMA_StructInit(&DMA_InitStructure);
|
DMA_StructInit(&DMA_InitStructure);
|
||||||
DMA_InitStructure.DMA_PeripheralBaseAddr = s->rxDMAPeripheralBaseAddr;
|
DMA_InitStructure.DMA_PeripheralBaseAddr = s->rxDMAPeripheralBaseAddr;
|
||||||
|
@ -152,7 +155,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,
|
||||||
}
|
}
|
||||||
|
|
||||||
// Transmit DMA or IRQ
|
// Transmit DMA or IRQ
|
||||||
if ((mode & MODE_TX) || (mode & MODE_BIDIR)) {
|
if (mode & MODE_TX) {
|
||||||
if (s->txDMAChannel) {
|
if (s->txDMAChannel) {
|
||||||
DMA_StructInit(&DMA_InitStructure);
|
DMA_StructInit(&DMA_InitStructure);
|
||||||
DMA_InitStructure.DMA_PeripheralBaseAddr = s->txDMAPeripheralBaseAddr;
|
DMA_InitStructure.DMA_PeripheralBaseAddr = s->txDMAPeripheralBaseAddr;
|
||||||
|
@ -179,7 +182,7 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,
|
||||||
|
|
||||||
USART_Cmd(s->USARTx, ENABLE);
|
USART_Cmd(s->USARTx, ENABLE);
|
||||||
|
|
||||||
if (mode & MODE_BIDIR)
|
if (options & SERIAL_BIDIR)
|
||||||
USART_HalfDuplexCmd(s->USARTx, ENABLE);
|
USART_HalfDuplexCmd(s->USARTx, ENABLE);
|
||||||
else
|
else
|
||||||
USART_HalfDuplexCmd(s->USARTx, DISABLE);
|
USART_HalfDuplexCmd(s->USARTx, DISABLE);
|
||||||
|
|
|
@ -23,7 +23,7 @@ extern const struct serialPortVTable uartVTable[];
|
||||||
|
|
||||||
void uartStartTxDMA(uartPort_t *s);
|
void uartStartTxDMA(uartPort_t *s);
|
||||||
|
|
||||||
uartPort_t *serialUSART1(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);
|
uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode, portOptions_t options);
|
||||||
uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode);
|
uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode, portOptions_t options);
|
||||||
|
|
||||||
|
|
|
@ -84,7 +84,7 @@ void usartIrqCallback(uartPort_t *s)
|
||||||
|
|
||||||
#ifdef USE_USART1
|
#ifdef USE_USART1
|
||||||
// USART1 - Telemetry (RX/TX by DMA)
|
// 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;
|
uartPort_t *s;
|
||||||
static volatile uint8_t rx1Buffer[UART1_RX_BUFFER_SIZE];
|
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_TX PA9
|
||||||
// USART1_RX PA10
|
// USART1_RX PA10
|
||||||
gpio.speed = Speed_2MHz;
|
gpio.speed = Speed_2MHz;
|
||||||
|
|
||||||
gpio.pin = Pin_9;
|
gpio.pin = Pin_9;
|
||||||
gpio.mode = Mode_AF_PP;
|
if (options & SERIAL_BIDIR) {
|
||||||
if (mode & MODE_TX)
|
gpio.mode = Mode_AF_OD;
|
||||||
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)
|
|
||||||
gpioInit(GPIOA, &gpio);
|
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
|
// DMA TX Interrupt
|
||||||
NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel4_IRQn;
|
NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel4_IRQn;
|
||||||
|
@ -174,7 +180,7 @@ void USART1_IRQHandler(void)
|
||||||
|
|
||||||
#ifdef USE_USART2
|
#ifdef USE_USART2
|
||||||
// USART2 - GPS or Spektrum or ?? (RX + TX by IRQ)
|
// 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;
|
uartPort_t *s;
|
||||||
static volatile uint8_t rx2Buffer[UART2_RX_BUFFER_SIZE];
|
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_TX PA2
|
||||||
// USART2_RX PA3
|
// USART2_RX PA3
|
||||||
gpio.speed = Speed_2MHz;
|
gpio.speed = Speed_2MHz;
|
||||||
|
|
||||||
gpio.pin = Pin_2;
|
gpio.pin = Pin_2;
|
||||||
gpio.mode = Mode_AF_PP;
|
if (options & SERIAL_BIDIR) {
|
||||||
if (mode & MODE_TX)
|
gpio.mode = Mode_AF_OD;
|
||||||
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)
|
|
||||||
gpioInit(GPIOA, &gpio);
|
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
|
// RX/TX Interrupt
|
||||||
NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
|
NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
|
||||||
|
@ -237,7 +249,7 @@ void USART2_IRQHandler(void)
|
||||||
|
|
||||||
#ifdef USE_USART3
|
#ifdef USE_USART3
|
||||||
// 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;
|
uartPort_t *s;
|
||||||
static volatile uint8_t rx3Buffer[UART3_RX_BUFFER_SIZE];
|
static volatile uint8_t rx3Buffer[UART3_RX_BUFFER_SIZE];
|
||||||
|
@ -268,17 +280,23 @@ uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
gpio.speed = Speed_2MHz;
|
gpio.speed = Speed_2MHz;
|
||||||
|
|
||||||
gpio.pin = USART3_TX_PIN;
|
gpio.pin = USART3_TX_PIN;
|
||||||
gpio.mode = Mode_AF_PP;
|
if (options & SERIAL_BIDIR) {
|
||||||
if (mode & MODE_TX)
|
gpio.mode = Mode_AF_OD;
|
||||||
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)
|
|
||||||
gpioInit(USART3_GPIO, &gpio);
|
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
|
// RX/TX Interrupt
|
||||||
NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
|
NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
|
||||||
|
|
|
@ -82,7 +82,7 @@ static uartPort_t uartPort3;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_USART1
|
#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;
|
uartPort_t *s;
|
||||||
static volatile uint8_t rx1Buffer[UART1_RX_BUFFER_SIZE];
|
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_OType = GPIO_OType_PP;
|
||||||
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
|
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
|
||||||
|
|
||||||
if (mode & MODE_TX) {
|
if (options & SERIAL_BIDIR) {
|
||||||
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) {
|
|
||||||
GPIO_InitStructure.GPIO_Pin = UART1_TX_PIN;
|
GPIO_InitStructure.GPIO_Pin = UART1_TX_PIN;
|
||||||
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
|
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
|
||||||
GPIO_PinAFConfig(UART1_GPIO, UART1_TX_PINSOURCE, UART1_GPIO_AF);
|
GPIO_PinAFConfig(UART1_GPIO, UART1_TX_PINSOURCE, UART1_GPIO_AF);
|
||||||
GPIO_Init(UART1_GPIO, &GPIO_InitStructure);
|
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
|
// DMA TX Interrupt
|
||||||
|
@ -157,7 +157,7 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_USART2
|
#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;
|
uartPort_t *s;
|
||||||
static volatile uint8_t rx2Buffer[UART2_RX_BUFFER_SIZE];
|
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_OType = GPIO_OType_PP;
|
||||||
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
|
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
|
||||||
|
|
||||||
if (mode & MODE_TX) {
|
if (options & SERIAL_BIDIR) {
|
||||||
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) {
|
|
||||||
GPIO_InitStructure.GPIO_Pin = UART2_TX_PIN;
|
GPIO_InitStructure.GPIO_Pin = UART2_TX_PIN;
|
||||||
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
|
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
|
||||||
GPIO_PinAFConfig(UART2_GPIO, UART2_TX_PINSOURCE, UART2_GPIO_AF);
|
GPIO_PinAFConfig(UART2_GPIO, UART2_TX_PINSOURCE, UART2_GPIO_AF);
|
||||||
GPIO_Init(UART2_GPIO, &GPIO_InitStructure);
|
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
|
#ifdef USE_USART2_TX_DMA
|
||||||
|
@ -238,7 +238,7 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_USART3
|
#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;
|
uartPort_t *s;
|
||||||
static volatile uint8_t rx3Buffer[UART3_RX_BUFFER_SIZE];
|
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_OType = GPIO_OType_PP;
|
||||||
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
|
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
|
||||||
|
|
||||||
if (mode & MODE_TX) {
|
if (options & SERIAL_BIDIR) {
|
||||||
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) {
|
|
||||||
GPIO_InitStructure.GPIO_Pin = UART3_TX_PIN;
|
GPIO_InitStructure.GPIO_Pin = UART3_TX_PIN;
|
||||||
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
|
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
|
||||||
GPIO_PinAFConfig(UART3_GPIO, UART3_TX_PINSOURCE, UART3_GPIO_AF);
|
GPIO_PinAFConfig(UART3_GPIO, UART3_TX_PINSOURCE, UART3_GPIO_AF);
|
||||||
GPIO_Init(UART3_GPIO, &GPIO_InitStructure);
|
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
|
#ifdef USE_USART3_TX_DMA
|
||||||
|
|
|
@ -134,7 +134,7 @@ const uint16_t frSkyDataIdTable[] = {
|
||||||
|
|
||||||
#define __USE_C99_MATH // for roundf()
|
#define __USE_C99_MATH // for roundf()
|
||||||
#define SMARTPORT_BAUD 57600
|
#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_SERVICE_DELAY_MS 5 // telemetry requests comes in at roughly 12 ms intervals, keep this under that
|
||||||
#define SMARTPORT_NOT_CONNECTED_TIMEOUT_MS 7000
|
#define SMARTPORT_NOT_CONNECTED_TIMEOUT_MS 7000
|
||||||
|
|
||||||
|
@ -229,11 +229,19 @@ void freeSmartPortTelemetryPort(void)
|
||||||
|
|
||||||
void configureSmartPortTelemetryPort(void)
|
void configureSmartPortTelemetryPort(void)
|
||||||
{
|
{
|
||||||
|
portOptions_t portOptions;
|
||||||
|
|
||||||
if (!portConfig) {
|
if (!portConfig) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, telemetryConfig->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED);
|
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) {
|
if (!smartPortSerialPort) {
|
||||||
return;
|
return;
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue