diff --git a/boards/arm/stm32/b-g431b-esc1/Kconfig b/boards/arm/stm32/b-g431b-esc1/Kconfig index ef2d7cf720a..54c75faa436 100644 --- a/boards/arm/stm32/b-g431b-esc1/Kconfig +++ b/boards/arm/stm32/b-g431b-esc1/Kconfig @@ -5,4 +5,16 @@ if ARCH_BOARD_B_G431B_ESC1 +if STM32_FOC + +config BOARD_STM32_BG431BESC1_FOC_VBUS + bool "B-G431B-ESC1 board VBUS sense" + default n + +config BOARD_STM32_BG431BESC1_FOC_POT + bool "B-G431B-ESC1 board POT support" + default n + +endif # STM32_FOC + endif # ARCH_BOARD_B_G431B_ESC1 diff --git a/boards/arm/stm32/b-g431b-esc1/README.txt b/boards/arm/stm32/b-g431b-esc1/README.txt index 7567661b9ea..4b846366bc1 100644 --- a/boards/arm/stm32/b-g431b-esc1/README.txt +++ b/boards/arm/stm32/b-g431b-esc1/README.txt @@ -18,3 +18,44 @@ Configuration Sub-directories --- Configures the NuttShell (nsh) located at apps/examples/nsh. The Configuration enables the serial interfaces on USART2. + + foc_f32 and foc_b16: + --------------------- + FOC examples based on hardware on board. + + Pin configuration: + + Board Function Chip Function Chip Pin Number + -------------- -------------- --------------- + Phase U high TIM1_CH1 PA8 + Phase U low TIM1_CH1N PC13 + Phase V high TIM1_CH2 PA9 + Phase V low TIM1_CH2N PA12 + Phase W high TIM1_CH3 PA10 + Phase W low TIM1_CH3N PB15 + Current U + OPAMP1_VINP PA1 + Current U - OPAMP1_VINM PA3 + Current V + OPAMP2_VINP PA7 + Current V - OPAMP2_VINM PA5 + Current W + OPAMP3_VINP PB0 + Current W - OPAMP3_VINM PB2 + Temperature PB14 + VBUS ADC1_IN1 PA0 + POT ADC1_IN11 PB12 + LED GPIO_PC6 PC6 + ENCO_A/HALL_H1 PB6 + ENCO_B/HALL_H2 PB7 + ENCO_Z/HALL_H3 PB8 + BUTTON GPIO_PC10 PC10 + PWM PA15 + + Current shunt resistance = 0.003 + PGA gain = 16 + Current sense gain = -9.14 (inverted current) + Vbus sense gain = 18k/(18k+169k) = 0.0962 + Vbus min = ? + Vbus max = 25V + Iout max = 40A peak + + IPHASE_RATIO = 1/(R_shunt*gain) = -36.47 + VBUS_RATIO = 1/VBUS_gain = 10.4 diff --git a/boards/arm/stm32/b-g431b-esc1/configs/foc_b16/defconfig b/boards/arm/stm32/b-g431b-esc1/configs/foc_b16/defconfig new file mode 100644 index 00000000000..25a638866de --- /dev/null +++ b/boards/arm/stm32/b-g431b-esc1/configs/foc_b16/defconfig @@ -0,0 +1,89 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +CONFIG_ADC=y +CONFIG_ADC_FIFOSIZE=3 +CONFIG_ANALOG=y +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD="b-g431b-esc1" +CONFIG_ARCH_BOARD_B_G431B_ESC1=y +CONFIG_ARCH_BUTTONS=y +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32G431C=y +CONFIG_ARCH_INTERRUPTSTACK=1024 +CONFIG_ARCH_IRQBUTTONS=y +CONFIG_ARMV7M_LIBM=y +CONFIG_BOARD_LOOPSPERMSEC=8499 +CONFIG_BOARD_STM32_BG431BESC1_FOC_POT=y +CONFIG_BOARD_STM32_BG431BESC1_FOC_VBUS=y +CONFIG_BUILTIN=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEFAULT_TASK_STACKSIZE=1024 +CONFIG_EXAMPLES_FOC=y +CONFIG_EXAMPLES_FOC_ADC_MAX=4095 +CONFIG_EXAMPLES_FOC_ADC_VREF=3300 +CONFIG_EXAMPLES_FOC_CONTROL_STACKSIZE=2048 +CONFIG_EXAMPLES_FOC_FIXED16_INST=1 +CONFIG_EXAMPLES_FOC_HAVE_BUTTON=y +CONFIG_EXAMPLES_FOC_IPHASE_ADC=-2939 +CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ=10000 +CONFIG_EXAMPLES_FOC_PWM_FREQ=20000 +CONFIG_EXAMPLES_FOC_RAMP_ACC=200000 +CONFIG_EXAMPLES_FOC_RAMP_DEC=200000 +CONFIG_EXAMPLES_FOC_RAMP_THR=10000 +CONFIG_EXAMPLES_FOC_VBUS_ADC=y +CONFIG_EXAMPLES_FOC_VBUS_SCALE=10400 +CONFIG_EXAMPLES_FOC_VEL_ADC=y +CONFIG_INDUSTRY_FOC=y +CONFIG_INDUSTRY_FOC_FIXED16=y +CONFIG_INPUT=y +CONFIG_INPUT_BUTTONS=y +CONFIG_INPUT_BUTTONS_LOWER=y +CONFIG_INTELHEX_BINARY=y +CONFIG_LIBM=y +CONFIG_MAX_TASKS=8 +CONFIG_MOTOR=y +CONFIG_MOTOR_FOC=y +CONFIG_MOTOR_FOC_SHUNTS=2 +CONFIG_MQ_MAXMSGSIZE=5 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_PREALLOC_TIMERS=4 +CONFIG_RAM_SIZE=22528 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_RR_INTERVAL=200 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_START_DAY=14 +CONFIG_START_MONTH=10 +CONFIG_START_YEAR=2014 +CONFIG_STM32_ADC1_ANIOC_TRIGGER=1 +CONFIG_STM32_ADC1_DMA=y +CONFIG_STM32_ADC1_DMA_CFG=1 +CONFIG_STM32_ADC1_INJECTED_CHAN=3 +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_DMAMUX1=y +CONFIG_STM32_FOC=y +CONFIG_STM32_FOC_FOC0=y +CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND=y +CONFIG_STM32_FOC_HAS_PWM_COMPLEMENTARY=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_TIM1_CH1MODE=0 +CONFIG_STM32_TIM1_CH2MODE=0 +CONFIG_STM32_TIM1_CH3MODE=0 +CONFIG_STM32_TIM1_MODE=2 +CONFIG_STM32_USART2=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=0 +CONFIG_USART2_SERIAL_CONSOLE=y +CONFIG_USART2_TXDMA=y +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/arm/stm32/b-g431b-esc1/configs/foc_f32/defconfig b/boards/arm/stm32/b-g431b-esc1/configs/foc_f32/defconfig new file mode 100644 index 00000000000..7cceaa5c19e --- /dev/null +++ b/boards/arm/stm32/b-g431b-esc1/configs/foc_f32/defconfig @@ -0,0 +1,90 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +CONFIG_ADC=y +CONFIG_ADC_FIFOSIZE=3 +CONFIG_ANALOG=y +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD="b-g431b-esc1" +CONFIG_ARCH_BOARD_B_G431B_ESC1=y +CONFIG_ARCH_BUTTONS=y +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32G431C=y +CONFIG_ARCH_INTERRUPTSTACK=1024 +CONFIG_ARCH_IRQBUTTONS=y +CONFIG_ARMV7M_LIBM=y +CONFIG_BOARD_LOOPSPERMSEC=8499 +CONFIG_BOARD_STM32_BG431BESC1_FOC_POT=y +CONFIG_BOARD_STM32_BG431BESC1_FOC_VBUS=y +CONFIG_BUILTIN=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEFAULT_TASK_STACKSIZE=1024 +CONFIG_EXAMPLES_FOC=y +CONFIG_EXAMPLES_FOC_ADC_MAX=4095 +CONFIG_EXAMPLES_FOC_ADC_VREF=3300 +CONFIG_EXAMPLES_FOC_CONTROL_STACKSIZE=2048 +CONFIG_EXAMPLES_FOC_FLOAT_INST=1 +CONFIG_EXAMPLES_FOC_HAVE_BUTTON=y +CONFIG_EXAMPLES_FOC_IPHASE_ADC=-2939 +CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ=10000 +CONFIG_EXAMPLES_FOC_PWM_FREQ=20000 +CONFIG_EXAMPLES_FOC_RAMP_ACC=200000 +CONFIG_EXAMPLES_FOC_RAMP_DEC=200000 +CONFIG_EXAMPLES_FOC_RAMP_THR=10000 +CONFIG_EXAMPLES_FOC_VBUS_ADC=y +CONFIG_EXAMPLES_FOC_VBUS_SCALE=10400 +CONFIG_EXAMPLES_FOC_VEL_ADC=y +CONFIG_INDUSTRY_FOC=y +CONFIG_INDUSTRY_FOC_FLOAT=y +CONFIG_INPUT=y +CONFIG_INPUT_BUTTONS=y +CONFIG_INPUT_BUTTONS_LOWER=y +CONFIG_INTELHEX_BINARY=y +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBM=y +CONFIG_MAX_TASKS=8 +CONFIG_MOTOR=y +CONFIG_MOTOR_FOC=y +CONFIG_MOTOR_FOC_SHUNTS=2 +CONFIG_MQ_MAXMSGSIZE=5 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_PREALLOC_TIMERS=4 +CONFIG_RAM_SIZE=22528 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_RR_INTERVAL=200 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_START_DAY=14 +CONFIG_START_MONTH=10 +CONFIG_START_YEAR=2014 +CONFIG_STM32_ADC1_ANIOC_TRIGGER=1 +CONFIG_STM32_ADC1_DMA=y +CONFIG_STM32_ADC1_DMA_CFG=1 +CONFIG_STM32_ADC1_INJECTED_CHAN=3 +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_DMAMUX1=y +CONFIG_STM32_FOC=y +CONFIG_STM32_FOC_FOC0=y +CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND=y +CONFIG_STM32_FOC_HAS_PWM_COMPLEMENTARY=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_TIM1_CH1MODE=0 +CONFIG_STM32_TIM1_CH2MODE=0 +CONFIG_STM32_TIM1_CH3MODE=0 +CONFIG_STM32_TIM1_MODE=2 +CONFIG_STM32_USART2=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=0 +CONFIG_USART2_SERIAL_CONSOLE=y +CONFIG_USART2_TXDMA=y +CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/arm/stm32/b-g431b-esc1/include/board.h b/boards/arm/stm32/b-g431b-esc1/include/board.h index 6a09afebd03..67946a45992 100644 --- a/boards/arm/stm32/b-g431b-esc1/include/board.h +++ b/boards/arm/stm32/b-g431b-esc1/include/board.h @@ -120,6 +120,45 @@ #define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK #define STM32_PCLK2_FREQUENCY STM32_HCLK_FREQUENCY +/* APB2 timers 1, 8, 20 and 15-17 will receive PCLK2. */ + +/* Timers driven from APB2 will be PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB1_TIM15_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB1_TIM16_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB1_TIM17_CLKIN (STM32_PCLK2_FREQUENCY) + +/* APB1 timers 2-7 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (STM32_PCLK1_FREQUENCY) + +/* USB divider -- Divide PLL clock by 1.5 */ + +#define STM32_CFGR_USBPRE 0 + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + */ + +#define BOARD_TIM1_FREQUENCY (STM32_PCLK2_FREQUENCY) +#define BOARD_TIM2_FREQUENCY (STM32_PCLK1_FREQUENCY) +#define BOARD_TIM3_FREQUENCY (STM32_PCLK1_FREQUENCY) +#define BOARD_TIM4_FREQUENCY (STM32_PCLK1_FREQUENCY) +#define BOARD_TIM5_FREQUENCY (STM32_PCLK1_FREQUENCY) +#define BOARD_TIM6_FREQUENCY (STM32_PCLK1_FREQUENCY) +#define BOARD_TIM7_FREQUENCY (STM32_PCLK1_FREQUENCY) +#define BOARD_TIM8_FREQUENCY (STM32_PCLK2_FREQUENCY) +#define BOARD_TIM15_FREQUENCY (STM32_PCLK2_FREQUENCY) +#define BOARD_TIM16_FREQUENCY (STM32_PCLK2_FREQUENCY) +#define BOARD_TIM17_FREQUENCY (STM32_PCLK2_FREQUENCY) +#define BOARD_TIM20_FREQUENCY (STM32_PCLK2_FREQUENCY) + /* LED definitions **********************************************************/ /* The B-G431B-ESC1 has four user LEDs. @@ -184,4 +223,21 @@ /* Pin Multiplexing Disambiguation ******************************************/ +/* TIM1 configuration *******************************************************/ + +#define GPIO_TIM1_CH1NOUT GPIO_TIM1_CH1NOUT_4 /* TIM1 CH1N - PC13 - U low */ +#define GPIO_TIM1_CH2NOUT GPIO_TIM1_CH2NOUT_1 /* TIM1 CH2N - PA12 - V low */ +#define GPIO_TIM1_CH3NOUT GPIO_TIM1_CH3NOUT_3 /* TIM1 CH3N - PB15 - W low */ + +/* DMA channels *************************************************************/ + +/* ADC */ + +#define ADC1_DMA_CHAN DMAMAP_DMA12_ADC1_0 /* DMA1 */ + +/* USART2 */ + +#define DMACHAN_USART2_TX DMAMAP_DMA12_USART2TX_0 /* DMA1 */ +#define DMACHAN_USART2_RX DMAMAP_DMA12_USART2RX_0 /* DMA1 */ + #endif /* __BOARDS_ARM_STM32_B_G431B_ESC1_INCLUDE_BOARD_H */ diff --git a/boards/arm/stm32/b-g431b-esc1/src/Make.defs b/boards/arm/stm32/b-g431b-esc1/src/Make.defs index 3ea4c9a0b57..c1a08b29ef9 100644 --- a/boards/arm/stm32/b-g431b-esc1/src/Make.defs +++ b/boards/arm/stm32/b-g431b-esc1/src/Make.defs @@ -37,6 +37,10 @@ ifeq ($(CONFIG_LIB_BOARDCTL),y) CSRCS += stm32_appinit.c endif +ifeq ($(CONFIG_STM32_FOC),y) +CSRCS += stm32_foc.c +endif + DEPPATH += --dep-path board VPATH += :board CFLAGS += $(shell $(INCDIR) "$(CC)" $(TOPDIR)$(DELIM)arch$(DELIM)$(CONFIG_ARCH)$(DELIM)src$(DELIM)board$(DELIM)board) 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 9bc75f30c0c..be372df84da 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 @@ -98,4 +98,28 @@ int stm32_bringup(void); +/**************************************************************************** + * Name: stm32_adc_setup + * + * Description: + * Initialize ADC and register the ADC driver. + * + ****************************************************************************/ + +#ifdef CONFIG_ADC +int stm32_adc_setup(void); +#endif + +/**************************************************************************** + * Name: stm32_foc_setup + * + * Description: + * Initialize FOC peripheral for the board. + * + ****************************************************************************/ + +#ifdef CONFIG_STM32_FOC +int stm32_foc_setup(void); +#endif + #endif /* __BOARDS_ARM_STM32_B_G431B_ESC1_SRC_B_G431B_ESC1_H */ diff --git a/boards/arm/stm32/b-g431b-esc1/src/stm32_bringup.c b/boards/arm/stm32/b-g431b-esc1/src/stm32_bringup.c index 3eebb58c39b..edd66adcf88 100644 --- a/boards/arm/stm32/b-g431b-esc1/src/stm32_bringup.c +++ b/boards/arm/stm32/b-g431b-esc1/src/stm32_bringup.c @@ -92,6 +92,26 @@ int stm32_bringup(void) } #endif +#ifdef CONFIG_STM32_FOC + /* Initialize and register the FOC device - must be before ADC setup */ + + ret = stm32_foc_setup(); + if (ret < 0) + { + syslog(LOG_ERR, "ERROR: stm32_foc_setup failed: %d\n", ret); + } +#endif + +#ifdef CONFIG_ADC + /* Initialize ADC and register the ADC driver. */ + + ret = stm32_adc_setup(); + if (ret < 0) + { + syslog(LOG_ERR, "ERROR: stm32_adc_setup failed: %d\n", ret); + } +#endif + UNUSED(ret); return OK; } diff --git a/boards/arm/stm32/b-g431b-esc1/src/stm32_buttons.c b/boards/arm/stm32/b-g431b-esc1/src/stm32_buttons.c index ceb320ee6a0..b3ebf9a58aa 100644 --- a/boards/arm/stm32/b-g431b-esc1/src/stm32_buttons.c +++ b/boards/arm/stm32/b-g431b-esc1/src/stm32_buttons.c @@ -74,11 +74,11 @@ uint32_t board_button_initialize(void) uint32_t board_buttons(void) { - /* Check the state of the USER button. A HIGH value means that the key is + /* Check the state of the USER button. A LOW value means that the key is * pressed. */ - return stm32_gpioread(GPIO_BTN_USER) ? BUTTON_USER_BIT : 0; + return stm32_gpioread(GPIO_BTN_USER) ? 0 : BUTTON_USER_BIT; } /**************************************************************************** diff --git a/boards/arm/stm32/b-g431b-esc1/src/stm32_foc.c b/boards/arm/stm32/b-g431b-esc1/src/stm32_foc.c new file mode 100644 index 00000000000..7f36182efc3 --- /dev/null +++ b/boards/arm/stm32/b-g431b-esc1/src/stm32_foc.c @@ -0,0 +1,601 @@ +/**************************************************************************** + * boards/arm/stm32/b-g431b-esc1/src/stm32_foc.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include + +#include + +#include "hardware/stm32g4xxxx_opamp.h" + +#include "stm32_foc.h" + +#include "arm_arch.h" + +#include "b-g431b-esc1.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* We don't use phase 2 feedback as it is no connected to ADC1 */ + +#if CONFIG_MOTOR_FOC_SHUNTS != 2 +# error Only 2-shunts configuration is supported +#endif + +/* Configuration specific for L6387ED: + * 1. PWM channels must have positive polarity + * 2. PWM complementary channels must have positive polarity + */ + +#ifndef CONFIG_STM32_FOC_HAS_PWM_COMPLEMENTARY +# error +#endif + +#if CONFIG_STM32_TIM1_CH1POL != 0 +# error +#endif +#if CONFIG_STM32_TIM1_CH2POL != 0 +# error +#endif +#if CONFIG_STM32_TIM1_CH3POL != 0 +# error +#endif +#if CONFIG_STM32_TIM1_CH1NPOL != 0 +# error +#endif +#if CONFIG_STM32_TIM1_CH2NPOL != 0 +# error +#endif +#if CONFIG_STM32_TIM1_CH3NPOL != 0 +# error +#endif + +/* SYSCFG must be enabled for OPAMP */ + +#ifndef CONFIG_STM32_SYSCFG +# error +#endif + +/* Aux ADC needs DMA enabled and workaround for G4 ADC CHAN0 enabled */ + +#ifdef CONFIG_ADC +# ifndef CONFIG_STM32_ADC1_DMA +# error +# endif +# ifndef CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND +# error +# endif +#endif + +/* REVISIT: */ + +#define PWM_DEADTIME (20) +#define PWM_DEADTIME_NS (500) + +/* Devpath for FOC driver */ + +#define FOC_DEVPATH "/dev/foc0" + +/* Board parameters: + * Current shunt resistance = 0.003 + * PGA gain = 16 + * Current sense gain = -9.14 (inverted current) + * Vbus sense gain = 0.0962 + * Vbus min = 7V + * Vbus max = 25V (6S LiPo battery pack) + * Iout max = 40A peak + * IPHASE_RATIO = 1/(R_shunt*gain) = -36.47 + * ADC_REF_VOLTAGE = 3.3 + * ADC_VAL_MAX = 4095 + * ADC_TO_VOLT = ADC_REF_VOLTAGE / ADC_VAL_MAX + * IPHASE_ADC = IPHASE_RATIO * ADC_TO_VOLT = -0.02939 + * VBUS_RATIO = 1/VBUS_gain = 10.4 + */ + +/* OPAMP gain */ + +#define CURRENT_PGA_GAIN 16 + +/* Center-aligned PWM duty cycle limits */ + +#define MAX_DUTY_B16 ftob16(0.95f) + +/* ADC sample time */ + +#define CURRENT_SAMPLE_TIME ADC_SMPR_2p5 +#define VBUS_SAMPLE_TIME ADC_SMPR_640p5 +#define POT_SAMPLE_TIME ADC_SMPR_640p5 + +/* ADC1 channels used in this example */ + +#define ADC1_INJECTED (CONFIG_MOTOR_FOC_SHUNTS) + +#ifdef CONFIG_BOARD_STM32_BG431BESC1_FOC_VBUS +# define BG431BESC1_FOC_VBUS 1 +#else +# define BG431BESC1_FOC_VBUS 0 +#endif + +#ifdef CONFIG_BOARD_STM32_BG431BESC1_FOC_POT +# define BG431BESC1_FOC_POT 1 +#else +# define BG431BESC1_FOC_POT 0 +#endif + +#define ADC1_REGULAR (BG431BESC1_FOC_VBUS + BG431BESC1_FOC_POT) +#define ADC1_NCHANNELS (ADC1_INJECTED + ADC1_REGULAR) + +/* Check ADC1 configuration */ + +#ifdef CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND +# if ADC1_INJECTED != (CONFIG_STM32_ADC1_INJECTED_CHAN - 1) +# error +# endif +#else +# if ADC1_INJECTED != CONFIG_STM32_ADC1_INJECTED_CHAN +# error +# endif +#endif + +#if CONFIG_STM32_ADC1_RESOLUTION != 0 +# error +#endif + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Protototypes + ****************************************************************************/ + +static int board_foc_setup(FAR struct foc_dev_s *dev); +static int board_foc_shutdown(FAR struct foc_dev_s *dev); +static int board_foc_calibration(FAR struct foc_dev_s *dev, bool state); +static int board_foc_fault_clear(FAR struct foc_dev_s *dev); +static int board_foc_pwm_start(FAR struct foc_dev_s *dev, bool state); +static int board_foc_current_get(FAR struct foc_dev_s *dev, + FAR int16_t *curr_raw, + FAR foc_current_t *curr); +#ifdef CONFIG_MOTOR_FOC_TRACE +static int board_foc_trace_init(FAR struct foc_dev_s *dev); +static void board_foc_trace(FAR struct foc_dev_s *dev, int type, bool state); +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* OPAMP confuguration: + * - connected with ADC through output pin (OPAINTOEN=0) + * - Current U+ - OPAMP1_VINP0 (PA1) + * - Current U- - OPAMP1_VINP0 (PA3) + * - Current V+ - OPAMP2_VINP0 (PA7) + * - Current V- - OPAMP2_VINP0 (PA5) + * - Current W+ - OPAMP3_VINP0 (PB0) + * - Current W- - OPAMP3_VINP0 (PB2) + * + * ADC configration: + * - Current Phase V -> ADC1 INJ1 -> ADC1_IN3 (OPAMP1_VOUT/PA2) + * - Current Phase U -> Not used, no ADC1 connection + * - Current Phase W -> ADC1 INJ2 -> ADC1_IN12 (OPAMP3_VOUT/PB12) + * optional: + * - VBUS -> ADC1 REG -> ADC1_IN1 (PA0) + * - POT -> ADC1 REG -> ADC1_IN11 (PB12) + * + * TIM1 PWM configuration: + * - Phase U high -> TIM1_CH1 (PA8) + * - Phase U low -> TIM1_CH1N (PC13) + * - Phase V high -> TIM1_CH2 (PA9) + * - Phase V low -> TIM1_CH2N (PA12) + * - Phase W high -> TIM1_CH3 (PA10) + * - Phase W low -> TIM1_CH3N (PB15) + */ + +static uint8_t g_adc1_chan[] = +{ +#ifdef CONFIG_BOARD_STM32_BG431BESC1_FOC_VBUS + 1, /* ADC1 REG - VBUS */ +#endif +#ifdef CONFIG_BOARD_STM32_BG431BESC1_FOC_POT + 11, /* ADC1 REG - POT */ +#endif + 3, /* ADC1 INJ1 - PHASE 1 */ + 12, /* ADC1 INJ2 - PHASE 3 */ +}; + +static uint32_t g_adc1_pins[] = +{ +#ifdef CONFIG_BOARD_STM32_BG431BESC1_FOC_VBUS + GPIO_ADC1_IN1, +#endif +#ifdef CONFIG_BOARD_STM32_BG431BESC1_FOC_POT + GPIO_ADC1_IN11, +#endif + GPIO_ADC1_IN3, + GPIO_ADC1_IN12, +}; + +/* ADC1 sample time configuration */ + +static adc_channel_t g_adc1_stime[] = +{ +#ifdef CONFIG_BOARD_STM32_BG431BESC1_FOC_VBUS + { + .channel = 1, + .sample_time = VBUS_SAMPLE_TIME + }, +#endif +#ifdef CONFIG_BOARD_STM32_BG431BESC1_FOC_POT + { + .channel = 11, + .sample_time = POT_SAMPLE_TIME + }, +#endif + { + .channel = 3, + .sample_time = CURRENT_SAMPLE_TIME + }, + { + .channel = 12, + .sample_time = CURRENT_SAMPLE_TIME + }, +}; + +/* Board specific ADC configuration for FOC */ + +static struct stm32_foc_adc_s g_adc_cfg = +{ + .chan = g_adc1_chan, + .pins = g_adc1_pins, + .stime = g_adc1_stime, + .nchan = ADC1_NCHANNELS, + .regch = ADC1_REGULAR, + .intf = 1 +}; + +/* Board specific ops */ + +static struct stm32_foc_board_ops_s g_stm32_foc_board_ops = +{ + .setup = board_foc_setup, + .shutdown = board_foc_shutdown, + .calibration = board_foc_calibration, + .fault_clear = board_foc_fault_clear, + .pwm_start = board_foc_pwm_start, + .current_get = board_foc_current_get, +#ifdef CONFIG_MOTOR_FOC_TRACE + .trace_init = board_foc_trace_init, + .trace = board_foc_trace +#endif +}; + +/* Board specific data */ + +static struct stm32_foc_board_data_s g_stm32_foc_board_data = +{ + .adc_cfg = &g_adc_cfg, + .duty_max = (MAX_DUTY_B16), + .pwm_dt = (PWM_DEADTIME), + .pwm_dt_ns = (PWM_DEADTIME_NS) +}; + +/* Board specific configuration */ + +static struct stm32_foc_board_s g_stm32_foc_board = +{ + .data = &g_stm32_foc_board_data, + .ops = &g_stm32_foc_board_ops, +}; + +/* Global pointer to the upper FOC driver */ + +static FAR struct foc_dev_s *g_foc_dev = NULL; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_foc_setup + ****************************************************************************/ + +static int board_foc_setup(FAR struct foc_dev_s *dev) +{ + uint32_t regval = 0; + + DEBUGASSERT(dev); + + UNUSED(dev); + + /* OPAMP1/2/3 pins: + * OPAMP1_VINM - PA3 (VINM0) + * OPAMP1_VINP - PA1 (VINP0) + * OPAMP2_VINM - PA5 (VINM0) + * OPAMP2_VINP - PA7 (VINP0) + * OPAMP3_VINM - PB2 (VINM0) + * OPAMP3_VINP - PB0 (VINP0) + */ + + /* Configure GPIO */ + + stm32_configgpio(GPIO_OPAMP1_VINM0); + stm32_configgpio(GPIO_OPAMP1_VINP0); + stm32_configgpio(GPIO_OPAMP1_VOUT); + stm32_configgpio(GPIO_OPAMP2_VINM0); + stm32_configgpio(GPIO_OPAMP2_VINP0); + stm32_configgpio(GPIO_OPAMP2_VOUT); + stm32_configgpio(GPIO_OPAMP3_VINM0); + stm32_configgpio(GPIO_OPAMP3_VINP0); + stm32_configgpio(GPIO_OPAMP3_VOUT); + + /* Configure OPAMP inputs */ + + regval += (OPAMP_CSR_VPSEL_VINP0 | OPAMP_CSR_VMSEL_PGA); + + /* PGA mode, non-inverting configuration with external bias on VINM0 */ + +#if CURRENT_PGA_GAIN == 16 + regval += ((0b01011 << OPAMP_CSR_PGAGAIN_SHIFT) & OPAMP_CSR_PGAGAIN_MASK); +#else +# error Not supported +#endif + + /* Enable high-speed mode */ + + regval += OPAMP_CSR_OPAHSM; + + /* Write configuration */ + + putreg32(regval, STM32_OPAMP1_CSR); + putreg32(regval, STM32_OPAMP2_CSR); + putreg32(regval, STM32_OPAMP3_CSR); + + /* Enable OPAMPs in separate write */ + + regval += OPAMP_CSR_OPAMPEN; + + putreg32(regval, STM32_OPAMP1_CSR); + putreg32(regval, STM32_OPAMP2_CSR); + putreg32(regval, STM32_OPAMP3_CSR); + + return OK; +} + +/**************************************************************************** + * Name: board_foc_shutdown + ****************************************************************************/ + +static int board_foc_shutdown(FAR struct foc_dev_s *dev) +{ + DEBUGASSERT(dev); + + UNUSED(dev); + + return OK; +} + +/**************************************************************************** + * Name: board_foc_calibration + ****************************************************************************/ + +static int board_foc_calibration(FAR struct foc_dev_s *dev, bool state) +{ + DEBUGASSERT(dev); + + UNUSED(dev); + + return OK; +} + +/**************************************************************************** + * Name: board_foc_fault_clear + ****************************************************************************/ + +static int board_foc_fault_clear(FAR struct foc_dev_s *dev) +{ + DEBUGASSERT(dev); + + UNUSED(dev); + + return OK; +} + +/**************************************************************************** + * Name: board_foc_pwm_start + ****************************************************************************/ + +static int board_foc_pwm_start(FAR struct foc_dev_s *dev, bool state) +{ + DEBUGASSERT(dev); + + UNUSED(dev); + + return OK; +} + +/**************************************************************************** + * Name: board_foc_current_get + ****************************************************************************/ + +static int board_foc_current_get(FAR struct foc_dev_s *dev, + FAR int16_t *curr_raw, + FAR foc_current_t *curr) +{ + DEBUGASSERT(dev); + DEBUGASSERT(curr_raw); + DEBUGASSERT(curr); + + /* Get currents */ + + curr[0] = curr_raw[0]; + curr[2] = curr_raw[1]; + + /* Phase 2 reconstruction */ + + curr[1] = -(curr_raw[0] + curr_raw[1]); + + return OK; +} + +#ifdef CONFIG_MOTOR_FOC_TRACE +/**************************************************************************** + * Name: board_foc_trace_init + ****************************************************************************/ + +static int board_foc_trace_init(FAR struct foc_dev_s *dev) +{ + DEBUGASSERT(dev); + + UNUSED(dev); + + /* Not supported */ + + return -1; +} + +/**************************************************************************** + * Name: board_foc_trace + ****************************************************************************/ + +static void board_foc_trace(FAR struct foc_dev_s *dev, int type, bool state) +{ + DEBUGASSERT(dev); + + UNUSED(dev); +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_foc_setup + * + * Description: + * Initialize FOC driver. + * + * This function should be call by board_app_initialize(). + * + * Returned Value: + * 0 on success, a negated errno value on failure + * + ****************************************************************************/ + +int stm32_foc_setup(void) +{ + FAR struct foc_dev_s *foc = NULL; + int ret = OK; + + /* Initialize only once */ + + if (g_foc_dev == NULL) + { + /* Initialize arch specific FOC lower-half */ + + foc = stm32_foc_initialize(0, &g_stm32_foc_board); + if (foc == NULL) + { + ret = -errno; + mtrerr("Failed to initialize STM32 FOC: %d\n", ret); + goto errout; + } + + DEBUGASSERT(foc->lower); + + /* Register FOC device */ + + ret = foc_register(FOC_DEVPATH, foc); + if (ret < 0) + { + mtrerr("Failed to register FOC device: %d\n", ret); + goto errout; + } + + /* Store pointer to driver */ + + g_foc_dev = foc; + } + +errout: + return ret; +} + +#ifdef CONFIG_ADC +/**************************************************************************** + * Name: stm32_adc_setup + * + * Description: + * Initialize ADC and register the ADC driver. + * + ****************************************************************************/ + +int stm32_adc_setup(void) +{ + FAR struct adc_dev_s *adc = NULL; + int ret = OK; + static bool initialized = false; + + /* Initialize only once */ + + if (initialized == false) + { + if (g_foc_dev == NULL) + { + mtrerr("Failed to get g_foc_dev device\n"); + ret = -EACCES; + goto errout; + } + + /* Register regular channel ADC */ + + adc = stm32_foc_adcget(g_foc_dev); + if (adc == NULL) + { + mtrerr("Failed to get ADC device: %d\n", ret); + goto errout; + } + + ret = adc_register("/dev/adc0", adc); + if (ret < 0) + { + mtrerr("adc_register failed: %d\n", ret); + goto errout; + } + + initialized = true; + } + +errout: + return ret; +} +#endif