mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 18:08:30 -04:00
HAL_ChibiOS: automatically control pullup/pulldown on inverted UARTs
this makes setup of sport telemetry much easier
This commit is contained in:
parent
8792adb6f0
commit
faf34970e1
@ -1302,17 +1302,31 @@ bool UARTDriver::set_options(uint8_t options)
|
|||||||
|
|
||||||
#if defined(STM32F7) || defined(STM32H7) || defined(STM32F3)
|
#if defined(STM32F7) || defined(STM32H7) || defined(STM32F3)
|
||||||
// F7 has built-in support for inversion in all uarts
|
// F7 has built-in support for inversion in all uarts
|
||||||
|
ioline_t rx_line = (options & OPTION_SWAP)?sdef.tx_line:sdef.rx_line;
|
||||||
|
ioline_t tx_line = (options & OPTION_SWAP)?sdef.rx_line:sdef.tx_line;
|
||||||
if (options & OPTION_RXINV) {
|
if (options & OPTION_RXINV) {
|
||||||
cr2 |= USART_CR2_RXINV;
|
cr2 |= USART_CR2_RXINV;
|
||||||
_cr2_options |= USART_CR2_RXINV;
|
_cr2_options |= USART_CR2_RXINV;
|
||||||
|
if (rx_line != 0) {
|
||||||
|
palLineSetPushPull(rx_line, PAL_PUSHPULL_PULLDOWN);
|
||||||
|
}
|
||||||
} else {
|
} else {
|
||||||
cr2 &= ~USART_CR2_RXINV;
|
cr2 &= ~USART_CR2_RXINV;
|
||||||
|
if (rx_line != 0) {
|
||||||
|
palLineSetPushPull(rx_line, PAL_PUSHPULL_PULLUP);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
if (options & OPTION_TXINV) {
|
if (options & OPTION_TXINV) {
|
||||||
cr2 |= USART_CR2_TXINV;
|
cr2 |= USART_CR2_TXINV;
|
||||||
_cr2_options |= USART_CR2_TXINV;
|
_cr2_options |= USART_CR2_TXINV;
|
||||||
|
if (tx_line != 0) {
|
||||||
|
palLineSetPushPull(tx_line, PAL_PUSHPULL_PULLDOWN);
|
||||||
|
}
|
||||||
} else {
|
} else {
|
||||||
cr2 &= ~USART_CR2_TXINV;
|
cr2 &= ~USART_CR2_TXINV;
|
||||||
|
if (tx_line != 0) {
|
||||||
|
palLineSetPushPull(tx_line, PAL_PUSHPULL_PULLUP);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
// F7 can also support swapping RX and TX pins
|
// F7 can also support swapping RX and TX pins
|
||||||
if (options & OPTION_SWAP) {
|
if (options & OPTION_SWAP) {
|
||||||
|
@ -72,6 +72,8 @@ public:
|
|||||||
uint8_t dma_tx_stream_id;
|
uint8_t dma_tx_stream_id;
|
||||||
uint32_t dma_tx_channel_id;
|
uint32_t dma_tx_channel_id;
|
||||||
#endif
|
#endif
|
||||||
|
ioline_t tx_line;
|
||||||
|
ioline_t rx_line;
|
||||||
ioline_t rts_line;
|
ioline_t rts_line;
|
||||||
int8_t rxinv_gpio;
|
int8_t rxinv_gpio;
|
||||||
uint8_t rxinv_polarity;
|
uint8_t rxinv_polarity;
|
||||||
|
@ -342,7 +342,18 @@ iomode_t palReadLineMode(ioline_t line)
|
|||||||
}
|
}
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
/*
|
||||||
|
set pin as pullup, pulldown or floating
|
||||||
|
*/
|
||||||
|
void palLineSetPushPull(ioline_t line, enum PalPushPull pp)
|
||||||
|
{
|
||||||
|
ioportid_t port = PAL_PORT(line);
|
||||||
|
uint8_t pad = PAL_PAD(line);
|
||||||
|
port->PUPDR = (port->PUPDR & ~(3<<(pad*2))) | (pp<<(pad*2));
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // F7, H7, F4
|
||||||
|
|
||||||
void stm32_cacheBufferInvalidate(const void *p, size_t size)
|
void stm32_cacheBufferInvalidate(const void *p, size_t size)
|
||||||
{
|
{
|
||||||
|
@ -80,6 +80,14 @@ void malloc_init(void);
|
|||||||
*/
|
*/
|
||||||
#if defined(STM32F7) || defined(STM32H7) || defined(STM32F4)
|
#if defined(STM32F7) || defined(STM32H7) || defined(STM32F4)
|
||||||
iomode_t palReadLineMode(ioline_t line);
|
iomode_t palReadLineMode(ioline_t line);
|
||||||
|
|
||||||
|
enum PalPushPull {
|
||||||
|
PAL_PUSHPULL_NOPULL=0,
|
||||||
|
PAL_PUSHPULL_PULLUP=1,
|
||||||
|
PAL_PUSHPULL_PULLDOWN=2
|
||||||
|
};
|
||||||
|
|
||||||
|
void palLineSetPushPull(ioline_t line, enum PalPushPull pp);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// set n RTC backup registers starting at given idx
|
// set n RTC backup registers starting at given idx
|
||||||
|
@ -985,6 +985,16 @@ def write_UART_config(f):
|
|||||||
else:
|
else:
|
||||||
error("Invalid element %s in UART_ORDER" % dev)
|
error("Invalid element %s in UART_ORDER" % dev)
|
||||||
devlist.append('HAL_%s_CONFIG' % dev)
|
devlist.append('HAL_%s_CONFIG' % dev)
|
||||||
|
if dev + "_TX" in bylabel:
|
||||||
|
p = bylabel[dev + '_TX']
|
||||||
|
tx_line = 'PAL_LINE(GPIO%s,%uU)' % (p.port, p.pin)
|
||||||
|
else:
|
||||||
|
tx_line = "0"
|
||||||
|
if dev + "_RX" in bylabel:
|
||||||
|
p = bylabel[dev + '_RX']
|
||||||
|
rx_line = 'PAL_LINE(GPIO%s,%uU)' % (p.port, p.pin)
|
||||||
|
else:
|
||||||
|
rx_line = "0"
|
||||||
if dev + "_RTS" in bylabel:
|
if dev + "_RTS" in bylabel:
|
||||||
p = bylabel[dev + '_RTS']
|
p = bylabel[dev + '_RTS']
|
||||||
rts_line = 'PAL_LINE(GPIO%s,%uU)' % (p.port, p.pin)
|
rts_line = 'PAL_LINE(GPIO%s,%uU)' % (p.port, p.pin)
|
||||||
@ -1006,10 +1016,10 @@ def write_UART_config(f):
|
|||||||
"#define HAL_%s_CONFIG { (BaseSequentialStream*) &SD%u, false, "
|
"#define HAL_%s_CONFIG { (BaseSequentialStream*) &SD%u, false, "
|
||||||
% (dev, n))
|
% (dev, n))
|
||||||
if mcu_series.startswith("STM32F1"):
|
if mcu_series.startswith("STM32F1"):
|
||||||
f.write("%s, " % rts_line)
|
f.write("%s, %s, %s, " % (tx_line, rx_line, rts_line))
|
||||||
else:
|
else:
|
||||||
f.write("STM32_%s_RX_DMA_CONFIG, STM32_%s_TX_DMA_CONFIG, %s, " %
|
f.write("STM32_%s_RX_DMA_CONFIG, STM32_%s_TX_DMA_CONFIG, %s, %s, %s, " %
|
||||||
(dev, dev, rts_line))
|
(dev, dev, tx_line, rx_line, rts_line))
|
||||||
|
|
||||||
# add inversion pins, if any
|
# add inversion pins, if any
|
||||||
f.write("%d, " % get_gpio_bylabel(dev + "_RXINV"))
|
f.write("%d, " % get_gpio_bylabel(dev + "_RXINV"))
|
||||||
|
Loading…
Reference in New Issue
Block a user