CC3D - Support USART3 instead of USART2.
This has highlighted that the existing codebase is quite targeted towards systems that use USART1 and 2. Annoyingly the inverter is on USART1 and the sbus code requires callbacks so still won't work yet.
This commit is contained in:
parent
9906294cd8
commit
b01724681a
|
@ -36,6 +36,7 @@
|
|||
|
||||
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);
|
||||
|
||||
static void uartReconfigure(uartPort_t *uartPort)
|
||||
{
|
||||
|
@ -76,8 +77,14 @@ serialPort_t *uartOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback,
|
|||
|
||||
if (USARTx == USART1) {
|
||||
s = serialUSART1(baudRate, mode);
|
||||
#ifdef USE_USART2
|
||||
} else if (USARTx == USART2) {
|
||||
s = serialUSART2(baudRate, mode);
|
||||
#endif
|
||||
#ifdef USE_USART3
|
||||
} else if (USARTx == USART3) {
|
||||
s = serialUSART3(baudRate, mode);
|
||||
#endif
|
||||
} else {
|
||||
return (serialPort_t *)s;
|
||||
}
|
||||
|
|
|
@ -23,6 +23,8 @@
|
|||
#define UART1_TX_BUFFER_SIZE 256
|
||||
#define UART2_RX_BUFFER_SIZE 128
|
||||
#define UART2_TX_BUFFER_SIZE 64
|
||||
#define UART3_RX_BUFFER_SIZE 128
|
||||
#define UART3_TX_BUFFER_SIZE 64
|
||||
|
||||
typedef struct {
|
||||
serialPort_t port;
|
||||
|
|
|
@ -34,11 +34,44 @@
|
|||
#include "serial.h"
|
||||
#include "serial_uart.h"
|
||||
|
||||
#ifdef USE_USART1
|
||||
static uartPort_t uartPort1;
|
||||
#endif
|
||||
|
||||
#ifdef USE_USART2
|
||||
static uartPort_t uartPort2;
|
||||
#endif
|
||||
|
||||
#ifdef USE_USART3
|
||||
static uartPort_t uartPort3;
|
||||
#endif
|
||||
|
||||
void uartStartTxDMA(uartPort_t *s);
|
||||
|
||||
void usartIrqCallback(uartPort_t *s)
|
||||
{
|
||||
uint16_t SR = s->USARTx->SR;
|
||||
|
||||
if (SR & USART_FLAG_RXNE) {
|
||||
// If we registered a callback, pass crap there
|
||||
if (s->port.callback) {
|
||||
s->port.callback(s->USARTx->DR);
|
||||
} else {
|
||||
s->port.rxBuffer[s->port.rxBufferHead] = s->USARTx->DR;
|
||||
s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize;
|
||||
}
|
||||
}
|
||||
if (SR & USART_FLAG_TXE) {
|
||||
if (s->port.txBufferTail != s->port.txBufferHead) {
|
||||
s->USARTx->DR = s->port.txBuffer[s->port.txBufferTail];
|
||||
s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
|
||||
} else {
|
||||
USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef USE_USART1
|
||||
// USART1 - Telemetry (RX/TX by DMA)
|
||||
uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
|
||||
{
|
||||
|
@ -91,6 +124,39 @@ uartPort_t *serialUSART1(uint32_t baudRate, portMode_t mode)
|
|||
return s;
|
||||
}
|
||||
|
||||
|
||||
// USART1 Tx DMA Handler
|
||||
void DMA1_Channel4_IRQHandler(void)
|
||||
{
|
||||
uartPort_t *s = &uartPort1;
|
||||
DMA_ClearITPendingBit(DMA1_IT_TC4);
|
||||
DMA_Cmd(s->txDMAChannel, DISABLE);
|
||||
|
||||
if (s->port.txBufferHead != s->port.txBufferTail)
|
||||
uartStartTxDMA(s);
|
||||
else
|
||||
s->txDMAEmpty = true;
|
||||
}
|
||||
|
||||
// USART1 Tx IRQ Handler
|
||||
void USART1_IRQHandler(void)
|
||||
{
|
||||
uartPort_t *s = &uartPort1;
|
||||
uint16_t SR = s->USARTx->SR;
|
||||
|
||||
if (SR & USART_FLAG_TXE) {
|
||||
if (s->port.txBufferTail != s->port.txBufferHead) {
|
||||
s->USARTx->DR = s->port.txBuffer[s->port.txBufferTail];
|
||||
s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
|
||||
} else {
|
||||
USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef USE_USART2
|
||||
// USART2 - GPS or Spektrum or ?? (RX + TX by IRQ)
|
||||
uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode)
|
||||
{
|
||||
|
@ -140,58 +206,72 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode)
|
|||
return s;
|
||||
}
|
||||
|
||||
// Handlers
|
||||
|
||||
// USART1 Tx DMA Handler
|
||||
void DMA1_Channel4_IRQHandler(void)
|
||||
{
|
||||
uartPort_t *s = &uartPort1;
|
||||
DMA_ClearITPendingBit(DMA1_IT_TC4);
|
||||
DMA_Cmd(s->txDMAChannel, DISABLE);
|
||||
|
||||
if (s->port.txBufferHead != s->port.txBufferTail)
|
||||
uartStartTxDMA(s);
|
||||
else
|
||||
s->txDMAEmpty = true;
|
||||
}
|
||||
|
||||
// USART1 Tx IRQ Handler
|
||||
void USART1_IRQHandler(void)
|
||||
{
|
||||
uartPort_t *s = &uartPort1;
|
||||
uint16_t SR = s->USARTx->SR;
|
||||
|
||||
if (SR & USART_FLAG_TXE) {
|
||||
if (s->port.txBufferTail != s->port.txBufferHead) {
|
||||
s->USARTx->DR = s->port.txBuffer[s->port.txBufferTail];
|
||||
s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
|
||||
} else {
|
||||
USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// USART2 Rx/Tx IRQ Handler
|
||||
void USART2_IRQHandler(void)
|
||||
{
|
||||
uartPort_t *s = &uartPort2;
|
||||
uint16_t SR = s->USARTx->SR;
|
||||
|
||||
if (SR & USART_FLAG_RXNE) {
|
||||
// If we registered a callback, pass crap there
|
||||
if (s->port.callback) {
|
||||
s->port.callback(s->USARTx->DR);
|
||||
} else {
|
||||
s->port.rxBuffer[s->port.rxBufferHead] = s->USARTx->DR;
|
||||
s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize;
|
||||
}
|
||||
}
|
||||
if (SR & USART_FLAG_TXE) {
|
||||
if (s->port.txBufferTail != s->port.txBufferHead) {
|
||||
s->USARTx->DR = s->port.txBuffer[s->port.txBufferTail];
|
||||
s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
|
||||
} else {
|
||||
USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
|
||||
}
|
||||
}
|
||||
usartIrqCallback(s);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef USE_USART3
|
||||
// USART3
|
||||
uartPort_t *serialUSART3(uint32_t baudRate, portMode_t mode)
|
||||
{
|
||||
uartPort_t *s;
|
||||
static volatile uint8_t rx3Buffer[UART3_RX_BUFFER_SIZE];
|
||||
static volatile uint8_t tx3Buffer[UART3_TX_BUFFER_SIZE];
|
||||
gpio_config_t gpio;
|
||||
NVIC_InitTypeDef NVIC_InitStructure;
|
||||
|
||||
s = &uartPort3;
|
||||
s->port.vTable = uartVTable;
|
||||
|
||||
s->port.baudRate = baudRate;
|
||||
|
||||
s->port.rxBuffer = rx3Buffer;
|
||||
s->port.txBuffer = tx3Buffer;
|
||||
s->port.rxBufferSize = UART3_RX_BUFFER_SIZE;
|
||||
s->port.txBufferSize = UART3_TX_BUFFER_SIZE;
|
||||
|
||||
s->USARTx = USART3;
|
||||
|
||||
s->txDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
|
||||
s->rxDMAPeripheralBaseAddr = (uint32_t)&s->USARTx->DR;
|
||||
|
||||
#ifdef USART3_APB1_PERIPHERALS
|
||||
RCC_APB1PeriphClockCmd(USART3_APB1_PERIPHERALS, ENABLE);
|
||||
#endif
|
||||
#ifdef USART3_APB2_PERIPHERALS
|
||||
RCC_APB2PeriphClockCmd(USART3_APB2_PERIPHERALS, ENABLE);
|
||||
#endif
|
||||
|
||||
gpio.speed = Speed_2MHz;
|
||||
gpio.pin = USART3_TX_PIN;
|
||||
gpio.mode = Mode_AF_PP;
|
||||
if (mode & MODE_TX)
|
||||
gpioInit(USART3_GPIO, &gpio);
|
||||
gpio.pin = USART3_RX_PIN;
|
||||
gpio.mode = Mode_IPU;
|
||||
if (mode & MODE_RX)
|
||||
gpioInit(USART3_GPIO, &gpio);
|
||||
|
||||
// RX/TX Interrupt
|
||||
NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
|
||||
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
|
||||
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
|
||||
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
|
||||
NVIC_Init(&NVIC_InitStructure);
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
// USART2 Rx/Tx IRQ Handler
|
||||
void USART3_IRQHandler(void)
|
||||
{
|
||||
uartPort_t *s = &uartPort3;
|
||||
usartIrqCallback(s);
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -83,6 +83,22 @@ const static serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
|
|||
|
||||
#else
|
||||
|
||||
#ifdef CC3D
|
||||
static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
|
||||
{SERIAL_PORT_USART1, NULL, SCENARIO_UNUSED, FUNCTION_NONE},
|
||||
{SERIAL_PORT_USART3, NULL, SCENARIO_UNUSED, FUNCTION_NONE},
|
||||
{SERIAL_PORT_SOFTSERIAL1, NULL, SCENARIO_UNUSED, FUNCTION_NONE},
|
||||
{SERIAL_PORT_SOFTSERIAL2, NULL, SCENARIO_UNUSED, FUNCTION_NONE}
|
||||
};
|
||||
|
||||
static const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
|
||||
{SERIAL_PORT_USART1, 9600, 115200, SPF_NONE | SPF_SUPPORTS_SBUS_MODE },
|
||||
{SERIAL_PORT_USART3, 9600, 115200, SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE},
|
||||
{SERIAL_PORT_SOFTSERIAL1, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE},
|
||||
{SERIAL_PORT_SOFTSERIAL2, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}
|
||||
};
|
||||
#else
|
||||
|
||||
static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
|
||||
{SERIAL_PORT_USART1, NULL, SCENARIO_UNUSED, FUNCTION_NONE},
|
||||
{SERIAL_PORT_USART2, NULL, SCENARIO_UNUSED, FUNCTION_NONE},
|
||||
|
@ -96,7 +112,7 @@ static const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
|
|||
{SERIAL_PORT_SOFTSERIAL1, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE},
|
||||
{SERIAL_PORT_SOFTSERIAL2, 9600, 19200, SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}
|
||||
};
|
||||
|
||||
#endif
|
||||
#endif
|
||||
|
||||
const functionConstraint_t functionConstraints[] = {
|
||||
|
@ -459,11 +475,19 @@ void applySerialConfigToPortFunctions(serialConfig_t *serialConfig)
|
|||
serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_SOFTSERIAL1)].scenario = serialPortScenarios[serialConfig->serial_port_4_scenario];
|
||||
serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_SOFTSERIAL2)].scenario = serialPortScenarios[serialConfig->serial_port_5_scenario];
|
||||
#else
|
||||
|
||||
#ifdef CC3D
|
||||
serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_USART1)].scenario = serialPortScenarios[serialConfig->serial_port_1_scenario];
|
||||
serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_USART3)].scenario = serialPortScenarios[serialConfig->serial_port_2_scenario];
|
||||
serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_SOFTSERIAL1)].scenario = serialPortScenarios[serialConfig->serial_port_3_scenario];
|
||||
serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_SOFTSERIAL2)].scenario = serialPortScenarios[serialConfig->serial_port_4_scenario];
|
||||
#else
|
||||
serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_USART1)].scenario = serialPortScenarios[serialConfig->serial_port_1_scenario];
|
||||
serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_USART2)].scenario = serialPortScenarios[serialConfig->serial_port_2_scenario];
|
||||
serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_SOFTSERIAL1)].scenario = serialPortScenarios[serialConfig->serial_port_3_scenario];
|
||||
serialPortFunctions[lookupSerialPortFunctionIndexByIdentifier(SERIAL_PORT_SOFTSERIAL2)].scenario = serialPortScenarios[serialConfig->serial_port_4_scenario];
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
serialPort_t *openSerialPort(serialPortFunction_e function, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion)
|
||||
|
@ -502,12 +526,21 @@ serialPort_t *openSerialPort(serialPortFunction_e function, serialReceiveCallbac
|
|||
serialPort = usbVcpOpen();
|
||||
break;
|
||||
#endif
|
||||
#ifdef USE_USART1
|
||||
case SERIAL_PORT_USART1:
|
||||
serialPort = uartOpen(USART1, callback, baudRate, mode, inversion);
|
||||
break;
|
||||
#endif
|
||||
#ifdef USE_USART2
|
||||
case SERIAL_PORT_USART2:
|
||||
serialPort = uartOpen(USART2, callback, baudRate, mode, inversion);
|
||||
break;
|
||||
#endif
|
||||
#ifdef USE_USART3
|
||||
case SERIAL_PORT_USART3:
|
||||
serialPort = uartOpen(USART3, callback, baudRate, mode, inversion);
|
||||
break;
|
||||
#endif
|
||||
#ifdef SOFT_SERIAL
|
||||
case SERIAL_PORT_SOFTSERIAL1:
|
||||
serialPort = openSoftSerial(SOFTSERIAL1, callback, baudRate, inversion);
|
||||
|
|
|
@ -87,9 +87,11 @@ typedef enum {
|
|||
SERIAL_PORT_3,
|
||||
SERIAL_PORT_4
|
||||
} serialPortIndex_e;
|
||||
|
||||
typedef enum {
|
||||
SERIAL_PORT_USART1 = 0,
|
||||
SERIAL_PORT_USART2,
|
||||
SERIAL_PORT_USART3,
|
||||
SERIAL_PORT_SOFTSERIAL1,
|
||||
SERIAL_PORT_SOFTSERIAL2
|
||||
} serialPortIdentifier_e;
|
||||
|
|
|
@ -33,6 +33,16 @@
|
|||
// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
|
||||
// #define SOFT_I2C_PB67
|
||||
|
||||
#define USE_USART1
|
||||
|
||||
#define USE_USART3
|
||||
#define USART3_RX_PIN Pin_11
|
||||
#define USART3_TX_PIN Pin_10
|
||||
#define USART3_GPIO GPIOB
|
||||
#define USART3_APB1_PERIPHERALS RCC_APB1Periph_USART3
|
||||
#define USART3_APB2_PERIPHERALS RCC_APB2Periph_GPIOB
|
||||
|
||||
|
||||
#define SENSORS_SET (SENSOR_ACC)
|
||||
|
||||
#define GPS
|
||||
|
|
|
@ -37,6 +37,9 @@
|
|||
|
||||
#define BRUSHED_MOTORS
|
||||
|
||||
#define USE_USART1
|
||||
#define USE_USART2
|
||||
|
||||
#define I2C_DEVICE (I2CDEV_1)
|
||||
|
||||
// #define SOFT_I2C // enable to test software i2c
|
||||
|
|
|
@ -47,6 +47,9 @@
|
|||
#define LED1
|
||||
#define INVERTER
|
||||
|
||||
#define USE_USART1
|
||||
#define USE_USART2
|
||||
|
||||
#define I2C_DEVICE (I2CDEV_2)
|
||||
|
||||
// #define SOFT_I2C // enable to test software i2c
|
||||
|
|
|
@ -40,6 +40,9 @@
|
|||
#define GYRO
|
||||
#define MAG
|
||||
|
||||
#define USE_USART1
|
||||
#define USE_USART2
|
||||
|
||||
#define I2C_DEVICE (I2CDEV_2)
|
||||
|
||||
// #define SOFT_I2C // enable to test software i2c
|
||||
|
|
Loading…
Reference in New Issue