From d7e7990548dc6668e5204f6978b8bc7c3d7b148d Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sat, 21 Jan 2023 21:47:35 +0100 Subject: [PATCH] boards/b-g431b-esc1: support for BEMF sensing --- boards/arm/stm32/b-g431b-esc1/README.txt | 9 +- .../arm/stm32/b-g431b-esc1/src/b-g431b-esc1.h | 5 + boards/arm/stm32/b-g431b-esc1/src/stm32_foc.c | 98 +++++++++++++++++++ 3 files changed, 110 insertions(+), 2 deletions(-) diff --git a/boards/arm/stm32/b-g431b-esc1/README.txt b/boards/arm/stm32/b-g431b-esc1/README.txt index f6ecb02ec5..8b355f6da5 100644 --- a/boards/arm/stm32/b-g431b-esc1/README.txt +++ b/boards/arm/stm32/b-g431b-esc1/README.txt @@ -48,9 +48,13 @@ Configuration Sub-directories ENCO_Z/HALL_H3 TIM4_CH3 PB8 BUTTON GPIO_PC10 PC10 PWM PA15 - CAN_RX PA11 - CAN_TX PB9 + CAN_RX FDCAN1_RX PA11 + CAN_TX FDCAN1_TX PB9 CAN_TERM PC14 + GPIO_BEMF PB5 + BEMF1 ADC2_IN17 PA4 + BEMF2 ADC2_IN5 PC4 + BEMF3 ADC2_IN14 PB11 Current shunt resistance = 0.003 PGA gain = 16 @@ -59,6 +63,7 @@ Configuration Sub-directories Vbus min = ? Vbus max = 25V Iout max = 40A peak + BEMF sense gain = 2.2k/(10k+2.2k) = 0.18 IPHASE_RATIO = 1/(R_shunt*gain) = -36.47 VBUS_RATIO = 1/VBUS_gain = 10.4 diff --git a/boards/arm/stm32/b-g431b-esc1/src/b-g431b-esc1.h b/boards/arm/stm32/b-g431b-esc1/src/b-g431b-esc1.h index 8b7cbb62ea..f55861b1f0 100644 --- a/boards/arm/stm32/b-g431b-esc1/src/b-g431b-esc1.h +++ b/boards/arm/stm32/b-g431b-esc1/src/b-g431b-esc1.h @@ -103,6 +103,11 @@ #define GPIO_CANTERM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz| \ GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN14) +/* GPIO_BEMF */ + +#define GPIO_GPIOBEMF (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz| \ + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5) + /**************************************************************************** * Public Data ****************************************************************************/ diff --git a/boards/arm/stm32/b-g431b-esc1/src/stm32_foc.c b/boards/arm/stm32/b-g431b-esc1/src/stm32_foc.c index 0887fdc1dc..3fc3086cd8 100644 --- a/boards/arm/stm32/b-g431b-esc1/src/stm32_foc.c +++ b/boards/arm/stm32/b-g431b-esc1/src/stm32_foc.c @@ -138,6 +138,7 @@ /* ADC sample time */ #define CURRENT_SAMPLE_TIME ADC_SMPR_2p5 +#define VOLTAGE_SAMPLE_TIME ADC_SMPR_2p5 #define VBUS_SAMPLE_TIME ADC_SMPR_640p5 #define POT_SAMPLE_TIME ADC_SMPR_640p5 @@ -160,6 +161,14 @@ #define ADC1_REGULAR (BG431BESC1_FOC_VBUS + BG431BESC1_FOC_POT) #define ADC1_NCHANNELS (ADC1_INJECTED + ADC1_REGULAR) +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE +/* ADC2 channels used for BEMF sensing */ + +# define ADC2_INJECTED (CONFIG_MOTOR_FOC_PHASES) +# define ADC2_REGULAR (0) +# define ADC2_NCHANNELS (ADC2_INJECTED + ADC2_REGULAR) +#endif + /* Check ADC1 configuration */ #ifdef CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND @@ -203,6 +212,11 @@ static int board_foc_pwm_start(struct foc_dev_s *dev, bool state); static int board_foc_current_get(struct foc_dev_s *dev, int16_t *curr_raw, foc_current_t *curr); +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE +static int board_foc_voltage_get(struct foc_dev_s *dev, + int16_t *volt_raw, + foc_voltage_t *volt); +#endif #ifdef CONFIG_MOTOR_FOC_TRACE static int board_foc_trace_init(struct foc_dev_s *dev); static void board_foc_trace(struct foc_dev_s *dev, int type, bool state); @@ -300,6 +314,52 @@ static struct stm32_foc_adc_s g_adc_cfg = .intf = 1 }; +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE +static uint8_t g_adc2_chan[] = +{ + 17, /* ADC2 INJ1 - PHASE 1 */ + 5, /* ADC2 INJ2 - PHASE 2 */ + 14, /* ADC2 INJ3 - PHASE 3 */ +}; + +static uint32_t g_adc2_pins[] = +{ + GPIO_ADC2_IN17, + GPIO_ADC2_IN5, + GPIO_ADC2_IN14, +}; + +/* ADC2 sample time configuration */ + +static adc_channel_t g_adc2_stime[] = +{ + { + .channel = 17, + .sample_time = VOLTAGE_SAMPLE_TIME + }, + { + .channel = 5, + .sample_time = VOLTAGE_SAMPLE_TIME + }, + { + .channel = 14, + .sample_time = VOLTAGE_SAMPLE_TIME + }, +}; + +/* Board specific ADC configuration for BEMF */ + +static struct stm32_foc_adc_s g_vadc_cfg = +{ + .chan = g_adc2_chan, + .pins = g_adc2_pins, + .stime = g_adc2_stime, + .nchan = ADC2_NCHANNELS, + .regch = ADC2_REGULAR, + .intf = 2 +}; +#endif + /* Board specific ops */ static struct stm32_foc_board_ops_s g_stm32_foc_board_ops = @@ -310,6 +370,9 @@ static struct stm32_foc_board_ops_s g_stm32_foc_board_ops = .fault_clear = board_foc_fault_clear, .pwm_start = board_foc_pwm_start, .current_get = board_foc_current_get, +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE + .voltage_get = board_foc_voltage_get, +#endif #ifdef CONFIG_MOTOR_FOC_TRACE .trace_init = board_foc_trace_init, .trace = board_foc_trace @@ -321,6 +384,9 @@ static struct stm32_foc_board_ops_s g_stm32_foc_board_ops = static struct stm32_foc_board_data_s g_stm32_foc_board_data = { .adc_cfg = &g_adc_cfg, +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE + .vadc_cfg = &g_vadc_cfg, +#endif .duty_max = (MAX_DUTY_B16), .pwm_dt = (PWM_DEADTIME), .pwm_dt_ns = (PWM_DEADTIME_NS) @@ -374,6 +440,9 @@ static int board_foc_setup(struct foc_dev_s *dev) stm32_configgpio(GPIO_OPAMP3_VINM0); stm32_configgpio(GPIO_OPAMP3_VINP0); stm32_configgpio(GPIO_OPAMP3_VOUT); +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE + stm32_configgpio(GPIO_GPIOBEMF); +#endif /* Configure OPAMP inputs */ @@ -405,6 +474,12 @@ static int board_foc_setup(struct foc_dev_s *dev) putreg32(regval, STM32_OPAMP2_CSR); putreg32(regval, STM32_OPAMP3_CSR); +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE + /* Keep GPIO_BEMF low to create BEMF voltage divider */ + + stm32_gpiowrite(GPIO_GPIOBEMF, false); +#endif + return OK; } @@ -484,6 +559,29 @@ static int board_foc_current_get(struct foc_dev_s *dev, return OK; } +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE +/**************************************************************************** + * Name: board_foc_voltage_get + ****************************************************************************/ + +static int board_foc_voltage_get(struct foc_dev_s *dev, + int16_t *volt_raw, + foc_voltage_t *volt) +{ + DEBUGASSERT(dev); + DEBUGASSERT(volt_raw); + DEBUGASSERT(volt); + + /* Get voltages */ + + volt[0] = volt_raw[0]; + volt[1] = volt_raw[1]; + volt[2] = volt_raw[2]; + + return OK; +} +#endif + #ifdef CONFIG_MOTOR_FOC_TRACE /**************************************************************************** * Name: board_foc_trace_init