From e6a0abdfce85f1479b1e4f5bcc01a3023747bbc1 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Tue, 30 Apr 2024 17:42:33 +0100 Subject: [PATCH] AP_HAL_ChibiOS: hwdef.py: find alt function for UART RTS and add to init struct --- .../hwdef/scripts/chibios_hwdef.py | 32 ++++++++++++++++--- 1 file changed, 28 insertions(+), 4 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 4e1e04a75563e..ca5f396d102c0 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -1885,7 +1885,8 @@ def write_UART_config(self, f): devlist.append('HAL_%s_CONFIG' % dev) tx_line = self.make_line(dev + '_TX') rx_line = self.make_line(dev + '_RX') - rts_line = self.make_line(dev + '_RTS') + rts_line_name = dev + '_RTS' + rts_line = self.make_line(rts_line_name) cts_line = self.make_line(dev + '_CTS') if rts_line != "0": have_rts_cts = True @@ -1893,12 +1894,12 @@ def write_UART_config(self, f): 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}\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}\n' % dev) # noqa 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}\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}\n' % dev) # noqa else: need_uart_driver = True f.write( @@ -1914,7 +1915,30 @@ def write_UART_config(self, f): f.write("%d, " % self.get_gpio_bylabel(dev + "_RXINV")) f.write("%s, " % self.get_extra_bylabel(dev + "_RXINV", "POL", "0")) f.write("%d, " % self.get_gpio_bylabel(dev + "_TXINV")) - f.write("%s, 0}\n" % self.get_extra_bylabel(dev + "_TXINV", "POL", "0")) + f.write("%s, " % self.get_extra_bylabel(dev + "_TXINV", "POL", "0")) + + # USB endpoint ID, not used + f.write("0, ") + + # Find and add RTS alt fuction number if avalable + def get_RTS_alt_function(): + # Typicaly we do software RTS control, so there is no requirement for the pin to have valid UART RTS alternative function + # If it does this enables hardware flow control for RS-485 + lib = self.get_mcu_lib(self.mcu_type) + if (rts_line == "0") or (rts_line_name not in self.bylabel) or not hasattr(lib, "AltFunction_map"): + # No pin, 0 is a valid alt function, use UINT8_MAX for invalid + return "UINT8_MAX" + + pin = self.bylabel[rts_line_name] + for label in self.af_labels: + if rts_line_name.startswith(label): + s = pin.portpin + ":" + rts_line_name + if s not in lib.AltFunction_map: + return "UINT8_MAX" + return lib.AltFunction_map[s] + + f.write("%s}\n" % get_RTS_alt_function()) + if have_rts_cts: f.write('#define AP_FEATURE_RTSCTS 1\n') if OTG2_index is not None: