From 051dc82e20dbf5fe1fee8c35b5d5e2322c2188a8 Mon Sep 17 00:00:00 2001 From: Andrey G Date: Wed, 26 Oct 2022 22:33:47 +0300 Subject: [PATCH] SENT improvements (#4702) * smt32_common.mk: reduce copy-paste * SENT: sentPins is not used * hw: stm32: add ICU helper * hw: stm32: icu helpers: also return timer base clock * SENT: icu: use helper to get ICU & channel and AF * SENT: icu: use CPU ticks for pulse measurements * hw: stm32: icu: cleanup --- firmware/hw_layer/drivers/sent/sent.cpp | 5 +- .../hw_layer/drivers/sent/sent_hw_icu.cpp | 83 +++--- firmware/hw_layer/ports/mpu_util.h | 4 + firmware/hw_layer/ports/stm32/port_mpu_util.h | 12 + firmware/hw_layer/ports/stm32/stm32_common.mk | 27 +- firmware/hw_layer/ports/stm32/stm32_icu.cpp | 241 ++++++++++++++++++ 6 files changed, 326 insertions(+), 46 deletions(-) create mode 100644 firmware/hw_layer/ports/stm32/stm32_icu.cpp diff --git a/firmware/hw_layer/drivers/sent/sent.cpp b/firmware/hw_layer/drivers/sent/sent.cpp index 120a775572..9632681586 100644 --- a/firmware/hw_layer/drivers/sent/sent.cpp +++ b/firmware/hw_layer/drivers/sent/sent.cpp @@ -63,6 +63,9 @@ #define MsgGetSig1(msg) (((msg) >> (1 * 4)) & 0xfff) #define MsgGetCrc(msg) MsgGetNibble(msg, 7) +/* convert CPU ticks to float Us */ +#define TicksToUs(ticks) ((float)(ticks) * 1000.0 * 1000.0 / CORE_CLOCK) + typedef enum { SENT_STATE_CALIB = 0, @@ -521,7 +524,7 @@ void sent_channel::Info(void) uint8_t stat; uint16_t sig0, sig1; - efiPrintf("Unit time %d timer ticks", tickPerUnit); + efiPrintf("Unit time %d CPU ticks %f uS", tickPerUnit, TicksToUs(tickPerUnit)); efiPrintf("Total pulses %d", pulseCounter); if (GetSignals(&stat, &sig0, &sig1) == 0) { diff --git a/firmware/hw_layer/drivers/sent/sent_hw_icu.cpp b/firmware/hw_layer/drivers/sent/sent_hw_icu.cpp index 010e5031cd..6feea57ad3 100644 --- a/firmware/hw_layer/drivers/sent/sent_hw_icu.cpp +++ b/firmware/hw_layer/drivers/sent/sent_hw_icu.cpp @@ -22,49 +22,56 @@ #if (HAL_USE_ICU == TRUE) /* TODO: get at runtime */ -#define SENT_ICU_FREQ (CORE_CLOCK / 2) // == CPU freq / 2 - -/* TODO: implement helper to get AF from GPIO for TIM2 capture */ -#define SENT_INPUT_AF PAL_MODE_ALTERNATE(1) - -/* TODO: implement helper to get ICU and channel from GPIO */ -#define SENT_ICU_UNIT ICUD2 /* TIM2 */ -#define SENT_ICU_CHANNEL ICU_CHANNEL_2 +/* Max timer clock for most timers on STM32 is CPU clock / 2 */ +#define SENT_TIMER_CLOCK_DIV 2 +#define SENT_ICU_FREQ (CORE_CLOCK / SENT_TIMER_CLOCK_DIV) // == CPU freq / 2 /* ICU callbacks */ static void icuperiodcb_in1(ICUDriver *icup) { - SENT_ISR_Handler(0, icuGetPeriodX(icup)); + SENT_ISR_Handler(0, icuGetPeriodX(icup) * SENT_TIMER_CLOCK_DIV); } /* ICU configs */ -static ICUConfig icucfg_in1 = +static ICUConfig icucfg[SENT_INPUT_COUNT] = { - .mode = ICU_INPUT_ACTIVE_LOW, - .frequency = SENT_ICU_FREQ, - .width_cb = NULL, - .period_cb = icuperiodcb_in1, - .overflow_cb = NULL, - .channel = SENT_ICU_CHANNEL, - .dier = 0U, - .arr = 0xFFFFFFFFU, + { + .mode = ICU_INPUT_ACTIVE_LOW, + .frequency = SENT_ICU_FREQ, + .width_cb = NULL, + .period_cb = icuperiodcb_in1, + .overflow_cb = NULL, + .channel = ICU_CHANNEL_1, /* will be overwriten on startSent() */ + .dier = 0U, + .arr = 0xFFFFFFFFU, + } }; -/* current config */ -static brain_input_pin_e sentPins[SENT_INPUT_COUNT]; - void startSent() { for (int i = 0; i < SENT_INPUT_COUNT; i++) { brain_input_pin_e sentPin = engineConfiguration->sentInputPins[i]; - if (isBrainPinValid(sentPin)) { - efiSetPadMode("SENT", sentPin, SENT_INPUT_AF); - - icuStart(&SENT_ICU_UNIT, &icucfg_in1); - icuStartCapture(&SENT_ICU_UNIT); - icuEnableNotifications(&SENT_ICU_UNIT); + if (!isBrainPinValid(sentPin)) { + continue; } + + ICUConfig *cfg = &icucfg[i]; + ICUDriver *icu; + iomode_t pinAF; + uint32_t baseClock; + + if (getIcuParams(sentPin, &pinAF, &icu, &cfg->channel, &baseClock) != true) { + /* this pin has no ICU functionality, of ICU driver is not enabled for TIM on this pin */ + /* throw error? */ + continue; + } + + efiSetPadMode("SENT", sentPin, PAL_MODE_ALTERNATE(pinAF)); + + icuStart(icu, cfg); + icuStartCapture(icu); + icuEnableNotifications(icu); } } @@ -73,13 +80,23 @@ void stopSent() for (int i = 0; i < SENT_INPUT_COUNT; i++) { brain_input_pin_e sentPin = activeConfiguration.sentInputPins[i]; - if (isBrainPinValid(sentPin)) { - icuDisableNotifications(&SENT_ICU_UNIT); - icuStopCapture(&SENT_ICU_UNIT); - icuStop(&SENT_ICU_UNIT); - - efiSetPadUnused(sentPin); + if (!isBrainPinValid(sentPin)) { + continue; } + + ICUDriver *icu; + + if (getIcuParams(sentPin, NULL, &icu, NULL, NULL) != true) { + /* this pin has no ICU functionality, of ICU driver is not enabled for TIM on this pin */ + /* throw error? */ + continue; + } + + icuDisableNotifications(icu); + icuStopCapture(icu); + icuStop(icu); + + efiSetPadUnused(sentPin); } } diff --git a/firmware/hw_layer/ports/mpu_util.h b/firmware/hw_layer/ports/mpu_util.h index 03ae588a2b..2090ce09ed 100644 --- a/firmware/hw_layer/ports/mpu_util.h +++ b/firmware/hw_layer/ports/mpu_util.h @@ -49,6 +49,10 @@ void initSpiCs(SPIConfig *spiConfig, brain_pin_e csPin); void turnOnSpi(spi_device_e device); #endif // HAL_USE_SPI +#if HAL_USE_ICU +bool getIcuParams(brain_pin_e hwPin, iomode_t *af_ptr, ICUDriver ** icu_ptr, icuchannel_t *channel_ptr, uint32_t *base_clock); +#endif + // MMC Card #if HAL_USE_MMC_SPI // HS = max 50MHz SPI diff --git a/firmware/hw_layer/ports/stm32/port_mpu_util.h b/firmware/hw_layer/ports/stm32/port_mpu_util.h index 8972dcd517..5c31c5608c 100644 --- a/firmware/hw_layer/ports/stm32/port_mpu_util.h +++ b/firmware/hw_layer/ports/stm32/port_mpu_util.h @@ -60,6 +60,18 @@ typedef enum { #define GPIO_AF_TIM9 3 #endif +#ifndef GPIO_AF_TIM12 +#define GPIO_AF_TIM12 9 +#endif + +#ifndef GPIO_AF_TIM13 +#define GPIO_AF_TIM13 9 +#endif + +#ifndef GPIO_AF_TIM14 +#define GPIO_AF_TIM14 9 +#endif + #ifndef ADC_CR2_SWSTART #define ADC_CR2_SWSTART ((uint32_t)0x40000000) #endif diff --git a/firmware/hw_layer/ports/stm32/stm32_common.mk b/firmware/hw_layer/ports/stm32/stm32_common.mk index 528e20f878..3b8de03a58 100644 --- a/firmware/hw_layer/ports/stm32/stm32_common.mk +++ b/firmware/hw_layer/ports/stm32/stm32_common.mk @@ -1,19 +1,22 @@ +HW_STM32_PORT_DIR = $(PROJECT_DIR)/hw_layer/ports/stm32 + HW_LAYER_EMS_CPP += \ - $(PROJECT_DIR)/hw_layer/ports/stm32/serial_over_usb/usbconsole.cpp \ - $(PROJECT_DIR)/hw_layer/ports/stm32/stm32_pins.cpp \ - $(PROJECT_DIR)/hw_layer/ports/stm32/stm32_common.cpp \ - $(PROJECT_DIR)/hw_layer/ports/stm32/backup_ram.cpp \ - $(PROJECT_DIR)/hw_layer/ports/stm32/microsecond_timer_stm32.cpp \ - $(PROJECT_DIR)/hw_layer/ports/stm32/osc_detector.cpp \ - $(PROJECT_DIR)/hw_layer/ports/stm32/flash_int.cpp \ - $(PROJECT_DIR)/hw_layer/ports/stm32/serial_over_usb/usbcfg.cpp \ + $(HW_STM32_PORT_DIR)/serial_over_usb/usbconsole.cpp \ + $(HW_STM32_PORT_DIR)/stm32_pins.cpp \ + $(HW_STM32_PORT_DIR)/stm32_common.cpp \ + $(HW_STM32_PORT_DIR)/stm32_icu.cpp \ + $(HW_STM32_PORT_DIR)/backup_ram.cpp \ + $(HW_STM32_PORT_DIR)/microsecond_timer_stm32.cpp \ + $(HW_STM32_PORT_DIR)/osc_detector.cpp \ + $(HW_STM32_PORT_DIR)/flash_int.cpp \ + $(HW_STM32_PORT_DIR)/serial_over_usb/usbcfg.cpp - -RUSEFIASM = $(PROJECT_DIR)/hw_layer/ports/stm32/rusEfiStartup.S +RUSEFIASM = \ + $(HW_STM32_PORT_DIR)/rusEfiStartup.S HW_INC += \ - $(PROJECT_DIR)/hw_layer/ports/stm32 \ - $(PROJECT_DIR)/hw_layer/ports/stm32/serial_over_usb + $(HW_STM32_PORT_DIR) \ + $(HW_STM32_PORT_DIR)/serial_over_usb ifeq ($(EFI_HAS_EXT_SDRAM), yes) USE_OPT += -Wl,--defsym=STM32_HAS_SDRAM=1 diff --git a/firmware/hw_layer/ports/stm32/stm32_icu.cpp b/firmware/hw_layer/ports/stm32/stm32_icu.cpp new file mode 100644 index 0000000000..42ddb18cc2 --- /dev/null +++ b/firmware/hw_layer/ports/stm32/stm32_icu.cpp @@ -0,0 +1,241 @@ +/** + * @file stm32_icu.cpp + * @brief Port implementation for the STM32 timer units in ICU mode found on the STM32F4 and STM32F7 + * + * @date October 20, 2022 + * @author Andrey Gusakov, (c) 2022 + * @author Andrey Belomutskiy, (c) 2012-2020 + */ + +#include "pch.h" + +#if HAL_USE_ICU + +#define RETURN_ICU_TRUE(icu, channel, af, clock) \ + do { \ + if (icu_ptr) { \ + if (*icu_ptr != (icu)) { \ + if (af_ptr) \ + *af_ptr = (af); \ + *icu_ptr = (icu); \ + if (channel_ptr) \ + *channel_ptr = (channel); \ + if (*clock_ptr) \ + *clock_ptr = clock; \ + return true; \ + } else { \ + /* if current icu is allready in *icu_ptr, continue and return another icu available on this pin, if any */ \ + } \ + } else { \ + /* called with null icu_ptr, just to know if icu is available on given pin */ \ + return true; \ + } \ + } while(0) + +#if (STM32_ICU_USE_TIM1 == TRUE) + #define RETURN_ICU1(channel) RETURN_ICU_TRUE(&ICUD1, channel, GPIO_AF_TIM1, STM32_TIMCLK1) +#else + #define RETURN_ICU1(channel) +#endif + +#if (STM32_ICU_USE_TIM2 == TRUE) + #define RETURN_ICU2(channel) RETURN_ICU_TRUE(&ICUD2, channel, GPIO_AF_TIM2, STM32_TIMCLK2) +#else + #define RETURN_ICU2(channel) +#endif + +#if (STM32_ICU_USE_TIM3 == TRUE) + #define RETURN_ICU3(channel) RETURN_ICU_TRUE(&ICUD3, channel, GPIO_AF_TIM3, STM32_TIMCLK3) +#else + #define RETURN_ICU3(channel) +#endif + +#if (STM32_ICU_USE_TIM4 == TRUE) + #define RETURN_ICU4(channel) RETURN_ICU_TRUE(&ICUD4, channel, GPIO_AF_TIM4, STM32_TIMCLK4) +#else + #define RETURN_ICU4(channel) +#endif + +#if (STM32_ICU_USE_TIM5 == TRUE) + #define RETURN_ICU5(channel) RETURN_ICU_TRUE(&ICUD5, channel, GPIO_AF_TIM5, STM32_TIMCLK5) +#else + #define RETURN_ICU5(channel) +#endif + +/* TIM6 is internal only */ + +/* TIM7 is internal only */ + +#if (STM32_ICU_USE_TIM8 == TRUE) + #define RETURN_ICU8(channel) RETURN_ICU_TRUE(&ICUD8, channel, GPIO_AF_TIM8, STM32_TIMCLK8) +#else + #define RETURN_ICU8(channel) +#endif + +#if (STM32_ICU_USE_TIM9 == TRUE) + #define RETURN_ICU9(channel) RETURN_ICU_TRUE(&ICUD9, channel, GPIO_AF_TIM9, STM32_TIMCLK9) +#else + #define RETURN_ICU9(channel) +#endif + +#if (STM32_ICU_USE_TIM10 == TRUE) + #define RETURN_ICU10(channel) RETURN_ICU_TRUE(&ICUD10, channel, GPIO_AF_TIM10, STM32_TIMCLK10) +#else + #define RETURN_ICU10(channel) +#endif + +#if (STM32_ICU_USE_TIM11 == TRUE) + #define RETURN_ICU11(channel) RETURN_ICU_TRUE(&ICUD11, channel, GPIO_AF_TIM11, STM32_TIMCLK11) +#else + #define RETURN_ICU11(channel) +#endif + +#if (STM32_ICU_USE_TIM12 == TRUE) + #define RETURN_ICU12(channel) RETURN_ICU_TRUE(&ICUD12, channel, GPIO_AF_TIM12, STM32_TIMCLK12) +#else + #define RETURN_ICU12(channel) +#endif + +#if (STM32_ICU_USE_TIM13 == TRUE) + #define RETURN_ICU13(channel) RETURN_ICU_TRUE(&ICUD13, channel, GPIO_AF_TIM13, STM32_TIMCLK13) +#else + #define RETURN_ICU13(channel) +#endif + +#if (STM32_ICU_USE_TIM14 == TRUE) + #define RETURN_ICU14(channel) RETURN_ICU_TRUE(&ICUD14, channel, GPIO_AF_TIM14, STM32_TIMCLK14) +#else + #define RETURN_ICU14(channel) +#endif + +/** + * ChibiOS limitation is that only channels #1 and #2 could be used for input capture + */ +bool getIcuParams(brain_pin_e hwPin, iomode_t *af_ptr, ICUDriver ** icu_ptr, icuchannel_t *channel_ptr, uint32_t *clock_ptr) +{ + switch (hwPin) { + case Gpio::A0: + RETURN_ICU5(ICU_CHANNEL_1); + return false; + case Gpio::A1: + RETURN_ICU2(ICU_CHANNEL_2); + RETURN_ICU5(ICU_CHANNEL_2); + return false; + case Gpio::A2: + RETURN_ICU9(ICU_CHANNEL_1); + return false; + case Gpio::A3: + RETURN_ICU9(ICU_CHANNEL_2); + return false; + case Gpio::A5: + RETURN_ICU2(ICU_CHANNEL_1); + return false; + case Gpio::A6: + RETURN_ICU3(ICU_CHANNEL_1); + RETURN_ICU13(ICU_CHANNEL_1); + return false; + case Gpio::A7: + RETURN_ICU3(ICU_CHANNEL_2); + RETURN_ICU14(ICU_CHANNEL_1); + return false; + case Gpio::A8: + RETURN_ICU1(ICU_CHANNEL_1); + return false; + case Gpio::A9: + RETURN_ICU1(ICU_CHANNEL_2); + return false; + case Gpio::A15: + RETURN_ICU2(ICU_CHANNEL_1); + return false; + case Gpio::B3: + RETURN_ICU2(ICU_CHANNEL_2); + return false; + case Gpio::B4: + RETURN_ICU3(ICU_CHANNEL_1); + return false; + case Gpio::B5: + RETURN_ICU3(ICU_CHANNEL_2); + return false; + case Gpio::B6: + RETURN_ICU4(ICU_CHANNEL_1); + return false; + case Gpio::B7: + RETURN_ICU4(ICU_CHANNEL_2); + return false; + case Gpio::B8: + RETURN_ICU10(ICU_CHANNEL_1); + return false; + case Gpio::B9: + RETURN_ICU11(ICU_CHANNEL_1); + return false; + case Gpio::B14: + RETURN_ICU12(ICU_CHANNEL_1); + return false; + case Gpio::B15: + RETURN_ICU12(ICU_CHANNEL_2); + return false; + case Gpio::C6: + RETURN_ICU3(ICU_CHANNEL_1); + RETURN_ICU8(ICU_CHANNEL_1); + return false; + case Gpio::C7: + RETURN_ICU3(ICU_CHANNEL_2); + RETURN_ICU8(ICU_CHANNEL_2); + return false; + case Gpio::D12: + RETURN_ICU4(ICU_CHANNEL_1); + return false; + case Gpio::D13: + RETURN_ICU4(ICU_CHANNEL_2); + return false; + case Gpio::E5: + RETURN_ICU9(ICU_CHANNEL_1); + return false; + case Gpio::E6: + RETURN_ICU9(ICU_CHANNEL_2); + return false; + case Gpio::E9: + RETURN_ICU1(ICU_CHANNEL_1); + return false; + case Gpio::E11: + RETURN_ICU1(ICU_CHANNEL_2); + return false; + case Gpio::F6: + RETURN_ICU10(ICU_CHANNEL_1); + return false; + case Gpio::F7: + RETURN_ICU11(ICU_CHANNEL_1); + return false; + case Gpio::F8: + RETURN_ICU13(ICU_CHANNEL_1); + return false; + case Gpio::F9: + RETURN_ICU14(ICU_CHANNEL_1); + return false; + case Gpio::H6: + RETURN_ICU12(ICU_CHANNEL_1); + return false; + case Gpio::H9: + RETURN_ICU12(ICU_CHANNEL_2); + return false; + case Gpio::H10: + RETURN_ICU5(ICU_CHANNEL_1); + return false; + case Gpio::H11: + RETURN_ICU5(ICU_CHANNEL_2); + return false; + case Gpio::I5: + RETURN_ICU8(ICU_CHANNEL_1); + return false; + case Gpio::I6: + RETURN_ICU8(ICU_CHANNEL_2); + return false; + + default: + return false; + } + + return false; +} + +#endif //HAL_USE_ICU