From 993d694fbbe6d35d057a8171d07ccaa8e536466c Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 17 Jul 2024 16:40:30 +1000 Subject: [PATCH] AP_HAL_ChibiOS: allow setup for low noise clock mismatch tolerant UART line --- libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 7 ++++ libraries/AP_HAL_ChibiOS/UARTDriver.h | 7 ++++ .../hwdef/scripts/chibios_hwdef.py | 33 ++++++++++++++++--- 3 files changed, 43 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index fcf29a8545..86689cfdba 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -455,6 +455,13 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) sercfg.cr3 |= USART_CR3_DMAT; } sercfg.irq_cb = rx_irq_cb; +#if HAL_HAVE_LOW_NOISE_UART + if (sdef.low_noise_line) { + // we can mark UART to sample on one bit instead of default 3 bits + // this allows us to be slightly less sensitive to clock differences + sercfg.cr3 |= USART_CR3_ONEBIT; + } +#endif #endif // HAL_UART_NODMA if (!(sercfg.cr2 & USART_CR2_STOP2_BITS)) { sercfg.cr2 |= USART_CR2_STOP1_BITS; diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index 2315192f9e..c549b8eebd 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -28,6 +28,10 @@ // enough for serial0 to serial9, plus IOMCU #define UART_MAX_DRIVERS 11 +#ifndef HAL_HAVE_LOW_NOISE_UART +#define HAL_HAVE_LOW_NOISE_UART 0 +#endif + class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { public: UARTDriver(uint8_t serial_num); @@ -79,7 +83,10 @@ public: uint8_t get_index(void) const { return uint8_t(this - &_serial_tab[0]); } + +#if HAL_HAVE_LOW_NOISE_UART bool low_noise_line; +#endif }; bool wait_timeout(uint16_t n, uint32_t timeout_ms) override; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index e01e4283a4..e59b850f75 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -1865,6 +1865,7 @@ INCLUDE common.ld OTG2_index = None devlist = [] have_rts_cts = False + have_low_noise = False crash_uart = None # write config for CrashCatcher UART @@ -1878,6 +1879,14 @@ INCLUDE common.ld f.write('#define IRQ_DISABLE_HAL_CRASH_SERIAL_PORT() nvicDisableVector(STM32_%s_NUMBER)\n' % crash_uart) f.write('#define RCC_RESET_HAL_CRASH_SERIAL_PORT() rccReset%s(); rccEnable%s(true)\n' % (crash_uart, crash_uart)) f.write('#define HAL_CRASH_SERIAL_PORT_CLOCK STM32_%sCLK\n' % crash_uart) + # check if we have a UART with a low noise RX pin + for num, dev in enumerate(serial_list): + if not dev.startswith('UART') and not dev.startswith('USART'): + continue + rx_port = dev + '_RX' + if rx_port in self.bylabel and self.bylabel[rx_port].has_extra('LOW_NOISE'): + have_low_noise = True + break for num, dev in enumerate(serial_list): if dev.startswith('UART'): n = int(dev[4:]) @@ -1904,12 +1913,20 @@ INCLUDE common.ld if dev.startswith('OTG2'): f.write( - '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU2, 2, true, false, 0, 0, false, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, UINT8_MAX}\n' % dev) # noqa + '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU2, 2, true, false, 0, 0, false, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, UINT8_MAX,' % dev) # noqa + if have_low_noise: + f.write('false}\n') + else: + f.write('}\n') OTG2_index = serial_list.index(dev) self.dual_USB_enabled = True elif dev.startswith('OTG'): f.write( - '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU1, 1, true, false, 0, 0, false, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, UINT8_MAX}\n' % dev) # noqa + '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU1, 1, true, false, 0, 0, false, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, UINT8_MAX,' % dev) # noqa + if have_low_noise: + f.write('false}\n') + else: + f.write('}\n') else: need_uart_driver = True f.write( @@ -1948,9 +1965,17 @@ INCLUDE common.ld if s not in lib.AltFunction_map: return "UINT8_MAX" return lib.AltFunction_map[s] + if have_low_noise: + low_noise = 'false' + rx_port = dev + '_RX' + if rx_port in self.bylabel and self.bylabel[rx_port].has_extra('LOW_NOISE'): + low_noise = 'true' + f.write("%s, %s}\n" % (get_RTS_alt_function(), low_noise)) + else: + f.write("%s}\n" % get_RTS_alt_function()) - f.write("%s}\n" % get_RTS_alt_function()) - + if have_low_noise: + f.write('#define HAL_HAVE_LOW_NOISE_UART 1\n') if have_rts_cts: f.write('#define AP_FEATURE_RTSCTS 1\n') if OTG2_index is not None: