[4.4.2] Remove 15 m/s limit on estimated vario (#12788)
[betaflight.git] / src / main / drivers / serial_uart_stm32f4xx.c
blob0c3ba9e5f3ce4fba55394d3fc0fb8c5f31d2a722
1 /*
2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
8 * any later version.
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
22 * jflyper - Refactoring, cleanup and made pin-configurable
25 #include <stdbool.h>
26 #include <stdint.h>
28 #include "platform.h"
29 #include "build/debug.h"
31 #ifdef USE_UART
33 #include "drivers/system.h"
34 #include "drivers/io.h"
35 #include "drivers/dma.h"
36 #include "drivers/nvic.h"
37 #include "drivers/rcc.h"
39 #include "drivers/serial.h"
40 #include "drivers/serial_uart.h"
41 #include "drivers/serial_uart_impl.h"
43 const uartHardware_t uartHardware[UARTDEV_COUNT] = {
44 #ifdef USE_UART1
46 .device = UARTDEV_1,
47 .reg = USART1,
48 .rxDMAChannel = DMA_Channel_4,
49 .txDMAChannel = DMA_Channel_4,
50 #ifdef USE_UART1_RX_DMA
51 .rxDMAResource = (dmaResource_t *)DMA2_Stream5,
52 #endif
53 #ifdef USE_UART1_TX_DMA
54 .txDMAResource = (dmaResource_t *)DMA2_Stream7,
55 #endif
56 .rxPins = { { DEFIO_TAG_E(PA10) }, { DEFIO_TAG_E(PB7) },
57 #if defined (STM32F411xE)
58 { DEFIO_TAG_E(PB3) },
59 #endif
61 .txPins = { { DEFIO_TAG_E(PA9) }, { DEFIO_TAG_E(PB6) },
62 #if defined (STM32F411xE)
63 { DEFIO_TAG_E(PA15) },
64 #endif
66 .af = GPIO_AF_USART1,
67 .rcc = RCC_APB2(USART1),
68 .irqn = USART1_IRQn,
69 .txPriority = NVIC_PRIO_SERIALUART1_TXDMA,
70 .rxPriority = NVIC_PRIO_SERIALUART1,
71 .txBuffer = uart1TxBuffer,
72 .rxBuffer = uart1RxBuffer,
73 .txBufferSize = sizeof(uart1TxBuffer),
74 .rxBufferSize = sizeof(uart1RxBuffer),
76 #endif
78 #ifdef USE_UART2
80 .device = UARTDEV_2,
81 .reg = USART2,
82 .rxDMAChannel = DMA_Channel_4,
83 .txDMAChannel = DMA_Channel_4,
84 #ifdef USE_UART2_RX_DMA
85 .rxDMAResource = (dmaResource_t *)DMA1_Stream5,
86 #endif
87 #ifdef USE_UART2_TX_DMA
88 .txDMAResource = (dmaResource_t *)DMA1_Stream6,
89 #endif
90 .rxPins = { { DEFIO_TAG_E(PA3) }, { DEFIO_TAG_E(PD6) } },
91 .txPins = { { DEFIO_TAG_E(PA2) }, { DEFIO_TAG_E(PD5) } },
92 .af = GPIO_AF_USART2,
93 .rcc = RCC_APB1(USART2),
94 .irqn = USART2_IRQn,
95 .txPriority = NVIC_PRIO_SERIALUART2_TXDMA,
96 .rxPriority = NVIC_PRIO_SERIALUART2,
97 .txBuffer = uart2TxBuffer,
98 .rxBuffer = uart2RxBuffer,
99 .txBufferSize = sizeof(uart2TxBuffer),
100 .rxBufferSize = sizeof(uart2RxBuffer),
102 #endif
104 #ifdef USE_UART3
106 .device = UARTDEV_3,
107 .reg = USART3,
108 .rxDMAChannel = DMA_Channel_4,
109 .txDMAChannel = DMA_Channel_4,
110 #ifdef USE_UART3_RX_DMA
111 .rxDMAResource = (dmaResource_t *)DMA1_Stream1,
112 #endif
113 #ifdef USE_UART3_TX_DMA
114 .txDMAResource = (dmaResource_t *)DMA1_Stream3,
115 #endif
116 .rxPins = { { DEFIO_TAG_E(PB11) }, { DEFIO_TAG_E(PC11) }, { DEFIO_TAG_E(PD9) } },
117 .txPins = { { DEFIO_TAG_E(PB10) }, { DEFIO_TAG_E(PC10) }, { DEFIO_TAG_E(PD8) } },
118 .af = GPIO_AF_USART3,
119 .rcc = RCC_APB1(USART3),
120 .irqn = USART3_IRQn,
121 .txPriority = NVIC_PRIO_SERIALUART3_TXDMA,
122 .rxPriority = NVIC_PRIO_SERIALUART3,
123 .txBuffer = uart3TxBuffer,
124 .rxBuffer = uart3RxBuffer,
125 .txBufferSize = sizeof(uart3TxBuffer),
126 .rxBufferSize = sizeof(uart3RxBuffer),
128 #endif
130 #ifdef USE_UART4
132 .device = UARTDEV_4,
133 .reg = UART4,
134 .rxDMAChannel = DMA_Channel_4,
135 .txDMAChannel = DMA_Channel_4,
136 #ifdef USE_UART4_RX_DMA
137 .rxDMAResource = (dmaResource_t *)DMA1_Stream2,
138 #endif
139 #ifdef USE_UART4_TX_DMA
140 .txDMAResource = (dmaResource_t *)DMA1_Stream4,
141 #endif
142 .rxPins = { { DEFIO_TAG_E(PA1) }, { DEFIO_TAG_E(PC11) } },
143 .txPins = { { DEFIO_TAG_E(PA0) }, { DEFIO_TAG_E(PC10) } },
144 .af = GPIO_AF_UART4,
145 .rcc = RCC_APB1(UART4),
146 .irqn = UART4_IRQn,
147 .txPriority = NVIC_PRIO_SERIALUART4_TXDMA,
148 .rxPriority = NVIC_PRIO_SERIALUART4,
149 .txBuffer = uart4TxBuffer,
150 .rxBuffer = uart4RxBuffer,
151 .txBufferSize = sizeof(uart4TxBuffer),
152 .rxBufferSize = sizeof(uart4RxBuffer),
154 #endif
156 #ifdef USE_UART5
158 .device = UARTDEV_5,
159 .reg = UART5,
160 .rxDMAChannel = DMA_Channel_4,
161 .txDMAChannel = DMA_Channel_4,
162 #ifdef USE_UART5_RX_DMA
163 .rxDMAResource = (dmaResource_t *)DMA1_Stream0,
164 #endif
165 #ifdef USE_UART5_TX_DMA
166 .txDMAResource = (dmaResource_t *)DMA1_Stream7,
167 #endif
168 .rxPins = { { DEFIO_TAG_E(PD2) } },
169 .txPins = { { DEFIO_TAG_E(PC12) } },
170 .af = GPIO_AF_UART5,
171 .rcc = RCC_APB1(UART5),
172 .irqn = UART5_IRQn,
173 .txPriority = NVIC_PRIO_SERIALUART5_TXDMA,
174 .rxPriority = NVIC_PRIO_SERIALUART5,
175 .txBuffer = uart5TxBuffer,
176 .rxBuffer = uart5RxBuffer,
177 .txBufferSize = sizeof(uart5TxBuffer),
178 .rxBufferSize = sizeof(uart5RxBuffer),
180 #endif
182 #ifdef USE_UART6
184 .device = UARTDEV_6,
185 .reg = USART6,
186 .rxDMAChannel = DMA_Channel_5,
187 .txDMAChannel = DMA_Channel_5,
188 #ifdef USE_UART6_RX_DMA
189 .rxDMAResource = (dmaResource_t *)DMA2_Stream1,
190 #endif
191 #ifdef USE_UART6_TX_DMA
192 .txDMAResource = (dmaResource_t *)DMA2_Stream6,
193 #endif
194 .rxPins = { { DEFIO_TAG_E(PC7) },
195 #if defined (STM32F411xE)
196 { DEFIO_TAG_E(PA12) },
197 #else
198 { DEFIO_TAG_E(PG9) },
199 #endif
201 .txPins = { { DEFIO_TAG_E(PC6) },
202 #if defined (STM32F411xE)
203 { DEFIO_TAG_E(PA11) },
204 #else
205 { DEFIO_TAG_E(PG14) },
206 #endif
208 .af = GPIO_AF_USART6,
209 .rcc = RCC_APB2(USART6),
210 .irqn = USART6_IRQn,
211 .txPriority = NVIC_PRIO_SERIALUART6_TXDMA,
212 .rxPriority = NVIC_PRIO_SERIALUART6,
213 .txBuffer = uart6TxBuffer,
214 .rxBuffer = uart6RxBuffer,
215 .txBufferSize = sizeof(uart6TxBuffer),
216 .rxBufferSize = sizeof(uart6RxBuffer),
218 #endif
221 bool checkUsartTxOutput(uartPort_t *s)
223 uartDevice_t *uart = container_of(s, uartDevice_t, port);
224 IO_t txIO = IOGetByTag(uart->tx.pin);
226 if ((uart->txPinState == TX_PIN_MONITOR) && txIO) {
227 if (IORead(txIO)) {
228 // TX is high so we're good to transmit
230 // Enable USART TX output
231 uart->txPinState = TX_PIN_ACTIVE;
232 IOConfigGPIOAF(txIO, IOCFG_AF_PP, uart->hardware->af);
233 return true;
234 } else {
235 // TX line is pulled low so don't enable USART TX
236 return false;
240 return true;
243 void uartTxMonitor(uartPort_t *s)
245 uartDevice_t *uart = container_of(s, uartDevice_t, port);
246 IO_t txIO = IOGetByTag(uart->tx.pin);
248 // Switch TX to an input with pullup so it's state can be monitored
249 uart->txPinState = TX_PIN_MONITOR;
250 IOConfigGPIO(txIO, IOCFG_IPU);
253 static void handleUsartTxDma(uartPort_t *s)
255 uartDevice_t *uart = container_of(s, uartDevice_t, port);
257 uartTryStartTxDMA(s);
259 if (s->txDMAEmpty && (uart->txPinState != TX_PIN_IGNORE)) {
260 // Switch TX to an input with pullup so it's state can be monitored
261 uartTxMonitor(s);
265 void uartDmaIrqHandler(dmaChannelDescriptor_t* descriptor)
267 uartPort_t *s = &(((uartDevice_t*)(descriptor->userParam))->port);
268 if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF))
270 DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
271 DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF);
272 if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_FEIF))
274 DMA_CLEAR_FLAG(descriptor, DMA_IT_FEIF);
276 handleUsartTxDma(s);
278 if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TEIF))
280 DMA_CLEAR_FLAG(descriptor, DMA_IT_TEIF);
282 if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_DMEIF))
284 DMA_CLEAR_FLAG(descriptor, DMA_IT_DMEIF);
288 // XXX Should serialUART be consolidated?
290 uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_e mode, portOptions_e options)
292 uartDevice_t *uart = uartDevmap[device];
293 if (!uart) return NULL;
295 const uartHardware_t *hardware = uart->hardware;
297 if (!hardware) return NULL; // XXX Can't happen !?
299 uartPort_t *s = &(uart->port);
300 s->port.vTable = uartVTable;
302 s->port.baudRate = baudRate;
304 s->port.rxBuffer = hardware->rxBuffer;
305 s->port.txBuffer = hardware->txBuffer;
306 s->port.rxBufferSize = hardware->rxBufferSize;
307 s->port.txBufferSize = hardware->txBufferSize;
309 s->USARTx = hardware->reg;
311 s->checkUsartTxOutput = checkUsartTxOutput;
313 #ifdef USE_DMA
314 uartConfigureDma(uart);
315 #endif
317 IO_t txIO = IOGetByTag(uart->tx.pin);
318 IO_t rxIO = IOGetByTag(uart->rx.pin);
320 if (hardware->rcc) {
321 RCC_ClockCmd(hardware->rcc, ENABLE);
324 uart->txPinState = TX_PIN_IGNORE;
326 if (options & SERIAL_BIDIR) {
327 IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
328 IOConfigGPIOAF(txIO, ((options & SERIAL_BIDIR_PP) || (options & SERIAL_BIDIR_PP_PD)) ? IOCFG_AF_PP : IOCFG_AF_OD, hardware->af);
329 } else {
330 if ((mode & MODE_TX) && txIO) {
331 IOInit(txIO, OWNER_SERIAL_TX, RESOURCE_INDEX(device));
333 if ((options & SERIAL_INVERTED) == SERIAL_NOT_INVERTED) {
334 uart->txPinState = TX_PIN_ACTIVE;
335 // Switch TX to an input with pullup so it's state can be monitored
336 uartTxMonitor(s);
337 } else {
338 IOConfigGPIOAF(txIO, IOCFG_AF_PP, hardware->af);
342 if ((mode & MODE_RX) && rxIO) {
343 IOInit(rxIO, OWNER_SERIAL_RX, RESOURCE_INDEX(device));
344 IOConfigGPIOAF(rxIO, IOCFG_AF_PP_UP, hardware->af);
348 #ifdef USE_DMA
349 if (!(s->rxDMAResource)) {
350 NVIC_InitTypeDef NVIC_InitStructure;
352 NVIC_InitStructure.NVIC_IRQChannel = hardware->irqn;
353 NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(hardware->rxPriority);
354 NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(hardware->rxPriority);
355 NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
356 NVIC_Init(&NVIC_InitStructure);
358 #endif
360 return s;
363 void uartIrqHandler(uartPort_t *s)
365 if (!s->rxDMAResource && (USART_GetITStatus(s->USARTx, USART_IT_RXNE) == SET)) {
366 if (s->port.rxCallback) {
367 s->port.rxCallback(s->USARTx->DR, s->port.rxCallbackData);
368 } else {
369 s->port.rxBuffer[s->port.rxBufferHead] = s->USARTx->DR;
370 s->port.rxBufferHead = (s->port.rxBufferHead + 1) % s->port.rxBufferSize;
374 // Detect completion of transmission
375 if (USART_GetITStatus(s->USARTx, USART_IT_TC) == SET) {
376 // Switch TX to an input with pullup so it's state can be monitored
377 uartTxMonitor(s);
379 USART_ClearITPendingBit(s->USARTx, USART_IT_TC);
382 if (!s->txDMAResource && (USART_GetITStatus(s->USARTx, USART_IT_TXE) == SET)) {
383 if (s->port.txBufferTail != s->port.txBufferHead) {
384 USART_SendData(s->USARTx, s->port.txBuffer[s->port.txBufferTail]);
385 s->port.txBufferTail = (s->port.txBufferTail + 1) % s->port.txBufferSize;
386 } else {
387 USART_ITConfig(s->USARTx, USART_IT_TXE, DISABLE);
391 if (USART_GetITStatus(s->USARTx, USART_IT_ORE) == SET) {
392 USART_ClearITPendingBit(s->USARTx, USART_IT_ORE);
395 if (USART_GetITStatus(s->USARTx, USART_IT_IDLE) == SET) {
396 if (s->port.idleCallback) {
397 s->port.idleCallback();
400 // clear
401 (void) s->USARTx->SR;
402 (void) s->USARTx->DR;
405 #endif