From 17b3607aa8d84955803898e6bccec8e5bbda5a84 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 12 Feb 2018 12:12:00 +1100 Subject: [PATCH] HAL_ChibiOS: fixed RCIN on revo-mini this also adds an attempt at using 'N' (inverted) channels for RCIN. This doesn't work yet, but would be better for the revo-mini, as it would allow the normal RCIN pin to be used --- libraries/AP_HAL_ChibiOS/SoftSigReader.cpp | 3 +++ .../AP_HAL_ChibiOS/hwdef/revo-mini/hwdef.dat | 15 ++++++++------- .../AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py | 9 ++++++++- 3 files changed, 19 insertions(+), 8 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/SoftSigReader.cpp b/libraries/AP_HAL_ChibiOS/SoftSigReader.cpp index e64e67e701..b28c5d8931 100644 --- a/libraries/AP_HAL_ChibiOS/SoftSigReader.cpp +++ b/libraries/AP_HAL_ChibiOS/SoftSigReader.cpp @@ -68,6 +68,9 @@ bool SoftSigReader::attach_capture_timer(ICUDriver* icu_drv, icuchannel_t chan, icucfg.mode = ICU_INPUT_ACTIVE_LOW; icucfg.dier = STM32_TIM_DIER_CC2DE; } +#ifdef HAL_RCIN_IS_INVERTED + icucfg.mode = (icucfg.mode==ICU_INPUT_ACTIVE_LOW)?ICU_INPUT_ACTIVE_HIGH:ICU_INPUT_ACTIVE_LOW; +#endif icuStart(_icu_drv, &icucfg); //Extended Timer Setup to enable DMA transfer //selected offset for TIM_CCR1 and for two words diff --git a/libraries/AP_HAL_ChibiOS/hwdef/revo-mini/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/revo-mini/hwdef.dat index c40aa8eed0..1cb312fa1e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/revo-mini/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/revo-mini/hwdef.dat @@ -28,10 +28,11 @@ STDOUT_BAUDRATE 57600 I2C_ORDER I2C1 # order of UARTs (and USB) -UART_ORDER OTG1 USART1 USART6 USART3 +UART_ORDER OTG1 USART1 USART3 -# we need to add support for N channels for RCIN -# PB14 TIM8_CH2 TIM8 RCIN PULLDOWN LOW DMA_CH0 +# rcinput is PC6, which is the 3rd "PWM IN" pin (the yellow wire on a +# revolution board) +PC6 TIM8_CH1 TIM8 RCIN FLOAT LOW DMA_CH0 # analog pins PC3 VDD_5V_SENS ADC1 @@ -46,11 +47,11 @@ PB5 LED_BLUE OUTPUT LOW GPIO(0) PB6 LED_YELLOW OUTPUT LOW GPIO(1) # optional PB4 LED_RED OUTPUT LOW GPIO(2) -# GPS port -PC6 USART6_TX USART6 -PC7 USART6_RX USART6 +# GPS port disabled (PC6 used for RCIN) +#PC6 USART6_TX USART6 +#PC7 USART6_RX USART6 -# flexi port, telem2 (can also be I2C2) +# flexi port, setup as GPS PB10 USART3_TX USART3 PB11 USART3_RX USART3 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index d2bdfb7e43..db2721a290 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -540,6 +540,10 @@ def write_PWM_config(f): a = rc_in.label.split('_') chan_str = a[1][2:] timer_str = a[0][3:] + if chan_str[-1] == 'N': + # it is an inverted channel + f.write('#define HAL_RCIN_IS_INVERTED\n') + chan_str = chan_str[:-1] if not is_int(chan_str) or not is_int(timer_str): error("Bad timer channel %s" % rc_in.label) if int(chan_str) not in [1, 2]: @@ -873,7 +877,10 @@ def build_peripheral_list(): if type.startswith('SDIO'): peripherals.append(type) if type.startswith('TIM') and p.has_extra('RCIN'): - peripherals.append(p.label) + label = p.label + if label[-1] == 'N': + label = label[:-1] + peripherals.append(label) done.add(type) return peripherals