sergiotarxz/nuttx
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

stm32f7,stm32/foc: support for BEMF sensing

Browse Source 
stm32 version tested with b-g431b-esc1
stm32f7 version not tested on HW
This commit is contained in:
raiden00pl 2023-01-19 17:50:30 +01:00 committed by Alan Carvalho de Assis
parent 01d84408e6
commit bfdb7f8909
4 changed files with 636 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

409
arch/arm/src/stm32/stm32_foc.c
View File

@ -599,6 +599,76 @@
# endif
#endif
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
# warning not tested on HW
/* Additional checks for BEMF sensing */
# if defined(CONFIG_STM32_FOC_FOC0) && defined(CONFIG_STM32_FOC_FOC1)
# error BEMF sensing supported only for one FOC instance enabled
# endif
# if defined(CONFIG_STM32_FOC_FOC0_ADC2) || defined(CONFIG_STM32_FOC_FOC0_ADC3)
# error FOC must use ADC master
# endif
# if defined(CONFIG_STM32_FOC_FOC1_ADC2) || defined(CONFIG_STM32_FOC_FOC1_ADC3)
# error FOC must use ADC master
# endif
/* Additional ADC slave in use */
# if defined(CONFIG_STM32_FOC_FOC0_ADC1) || defined(CONFIG_STM32_FOC_FOC1_ADC1)
# define CONFIG_STM32_FOC_USE_ADC2
# endif
# if defined(CONFIG_STM32_FOC_FOC0_ADC3) || defined(CONFIG_STM32_FOC_FOC1_ADC3)
# define CONFIG_STM32_FOC_USE_ADC4
# endif
/* The number of required injected channels */
# ifdef CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND
# define FOC_VADC_INJ_CHAN_REQUIRED (CONFIG_MOTOR_FOC_PHASES + 1)
# else
# define FOC_VADC_INJ_CHAN_REQUIRED (CONFIG_MOTOR_FOC_PHASES)
# endif
/* Slave ADC2 */
# ifdef CONFIG_STM32_FOC_USE_ADC2
# ifndef CONFIG_STM32_ADC2
# error ADC2 not supported !
# endif
# ifndef ADC2_HAVE_JEXTCFG
# error ADC2 must support JEXTCFG
# endif
# if CONFIG_STM32_ADC2_ANIOC_TRIGGER != 1
# error CONFIG_STM32_ADC2_ANIOC_TRIGGER must be 1
# endif
# if CONFIG_STM32_ADC2_INJECTED_CHAN != FOC_VADC_INJ_CHAN_REQUIRED
# error Invalid configuration for ADC2 injected channels
# endif
# endif
/* Slave ADC4 */
# ifdef CONFIG_STM32_FOC_USE_ADC4
# ifndef CONFIG_STM32_ADC4
# error ADC4 not supported !
# endif
# ifndef ADC4_HAVE_JEXTCFG
# error ADC4 must support JEXTCFG
# endif
# if CONFIG_STM32_ADC4_ANIOC_TRIGGER != 1
# error CONFIG_STM32_ADC4_ANIOC_TRIGGER must be 1
# endif
# if CONFIG_STM32_ADC4_INJECTED_CHAN != FOC_VADC_INJ_CHAN_REQUIRED
# error Invalid configuration for ADC4 injected channels
# endif
# endif
#endif
/* Helper macros ************************************************************/
/* Get arch-specific FOC private part */
@ -622,7 +692,8 @@
/* Get ADC device */
#define ADC_FROM_FOC_DEV_GET(d) (STM32_FOC_DEV_FROM_DEV_GET(d)->adc)
#define ADC_FROM_FOC_DEV_GET(d) (STM32_FOC_DEV_FROM_DEV_GET(d)->adc)
#define VADC_FROM_FOC_DEV_GET(d) (STM32_FOC_DEV_FROM_DEV_GET(d)->vadc)
/* Define PWM all outputs */
@ -698,6 +769,11 @@ struct stm32_foc_dev_s
/* Interrupt handler for FOC device */
int (*adc_isr)(struct foc_dev_s *dev);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
struct adc_dev_s *vadc_dev; /* ADC device reference (voltage ) */
struct stm32_adc_dev_s *vadc; /* STM32 ADC device reference (voltage) */
#endif
};
/* STM32 FOC common data */
@ -719,6 +795,11 @@ struct stm32_foc_data_s
uint32_t adcint_cntr; /* ADC interrupt counter */
uint32_t curr_offset[CONFIG_MOTOR_FOC_SHUNTS]; /* ADC current offset */
int16_t curr_raw[CONFIG_MOTOR_FOC_SHUNTS]; /* ADC current RAW */
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
foc_voltage_t volt[CONFIG_MOTOR_FOC_PHASES]; /* Voltage */
uint32_t volt_offset[CONFIG_MOTOR_FOC_PHASES]; /* ADC voltage offset */
int16_t volt_raw[CONFIG_MOTOR_FOC_PHASES]; /* ADC voltage RAW */
#endif
};
/* STM32 FOC private */
@ -786,6 +867,9 @@ static int stm32_foc_worker_handler(struct foc_dev_s *dev);
static void stm32_foc_curr_get(struct foc_dev_s *dev,
int16_t *curr, int shunts);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
static void stm32_foc_volt_get(struct foc_dev_s *dev, int16_t *volt);
#endif
static int stm32_foc_notifier_cfg(struct foc_dev_s *dev, uint32_t freq);
static int stm32_foc_pwm_cfg(struct foc_dev_s *dev, uint32_t freq);
static int stm32_foc_adc_cfg(struct foc_dev_s *dev);
@ -1069,7 +1153,7 @@ static int stm32_foc_pwm_start(struct foc_dev_s *dev, bool state)
if (!dev->state.pwm_off)
{
/* Configure outputs state */
/* Enable PWM outputs */
PWM_ALL_OUTPUTS_ENABLE(pwm, state);
}
@ -1320,6 +1404,9 @@ static int stm32_foc_setup(struct foc_dev_s *dev)
struct stm32_foc_board_s *board = STM32_FOC_BOARD_FROM_DEV_GET(dev);
struct stm32_foc_priv_s *priv = STM32_FOC_PRIV_FROM_DEV_GET(dev);
struct stm32_adc_dev_s *adc = ADC_FROM_FOC_DEV_GET(dev);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
struct stm32_adc_dev_s *vadc = VADC_FROM_FOC_DEV_GET(dev);
#endif
struct adc_sample_time_s stime;
int ret = OK;
@ -1345,6 +1432,30 @@ static int stm32_foc_setup(struct foc_dev_s *dev)
STM32_ADC_SETUP(foc_dev->adc);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* Setup slave ADC */
STM32_ADC_SETUP(foc_dev->vadc);
/* Disable interrupts for slave ADC */
STM32_ADC_DISABLEINT(foc_dev->vadc);
/* Disable master and slave ADC */
STM32_ADC_ENABLE(foc_dev->adc, false);
STM32_ADC_ENABLE(foc_dev->vadc, false);
/* Configure dual injected simultaneous only mode */
STM32_ADC_MULTICFG(foc_dev->vadc, ADC_MULTIMODE_ISM2);
/* Enable master and slave ADC */
STM32_ADC_ENABLE(foc_dev->adc, true);
STM32_ADC_ENABLE(foc_dev->vadc, true);
#endif
#ifdef FOC_ADC_HAVE_CMN
/* Lock ADC common data */
@ -1396,6 +1507,18 @@ static int stm32_foc_setup(struct foc_dev_s *dev)
STM32_ADC_SAMPLETIME_SET(adc, &stime);
STM32_ADC_SAMPLETIME_WRITE(adc);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* Configure sample times for BEMF channels */
memset(&stime, 0, sizeof(struct adc_sample_time_s));
stime.channels_nbr = board->data->vadc_cfg->nchan;
stime.channel = board->data->vadc_cfg->stime;
STM32_ADC_SAMPLETIME_SET(vadc, &stime);
STM32_ADC_SAMPLETIME_WRITE(vadc);
#endif
/* Set the priority of the ADC interrupt vector */
ret = up_prioritize_irq(foc_dev->adc_irq, NVIC_SYSH_PRIORITY_DEFAULT);
@ -1441,6 +1564,9 @@ static int stm32_foc_setup(struct foc_dev_s *dev)
/* Dump ADC regs */
STM32_ADC_DUMP_REGS(adc);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
STM32_ADC_DUMP_REGS(vadc);
#endif
errout:
return ret;
@ -1719,10 +1845,22 @@ static int stm32_foc_worker_handler(struct foc_dev_s *dev)
priv->data.curr_raw,
priv->data.curr);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* Get raw voltage samples */
stm32_foc_volt_get(dev, priv->data.volt_raw);
/* Get BEMF voltages */
ret = board->ops->voltage_get(dev,
priv->data.volt_raw,
priv->data.volt);
#endif
/* Call upper-half worker callback */
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
# error BEMF sensing not supported yet
priv->cb->notifier(dev, priv->data.curr, priv->data.volt);
#else
priv->cb->notifier(dev, priv->data.curr, NULL);
#endif
@ -1746,6 +1884,9 @@ static int stm32_foc_calibration_start(struct foc_dev_s *dev)
struct stm32_foc_board_s *board = STM32_FOC_BOARD_FROM_DEV_GET(dev);
struct stm32_pwm_dev_s *pwm = PWM_FROM_FOC_DEV_GET(dev);
struct stm32_adc_dev_s *adc = ADC_FROM_FOC_DEV_GET(dev);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
struct stm32_adc_dev_s *vadc = VADC_FROM_FOC_DEV_GET(dev);
#endif
uint8_t i = 0;
uint8_t ch = 0;
int ret = OK;
@ -1846,6 +1987,36 @@ static int stm32_foc_calibration_start(struct foc_dev_s *dev)
STM32_ADC_OFFSET_SET(adc, ch, i, priv->data.curr_offset[i]);
}
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* TODO: BEMF sensing calibartion */
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{
priv->data.volt_offset[i] = 0;
}
/* Clear last ADC data */
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{
priv->data.volt_raw[i] = 0;
}
/* Set ADC hardware offset for voltage channels (only injected channels) */
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{
/* Get channel */
ch = board->data->vadc_cfg->chan[board->data->vadc_cfg->regch + i];
/* Write offset */
STM32_ADC_OFFSET_SET(vadc, ch, i, priv->data.volt_offset[i]);
}
#endif
mtrinfo("ADC offset calibration - DONE!\n");
errout:
@ -1943,7 +2114,7 @@ static int stm32_foc_pwm_off(struct foc_dev_s *dev, bool off)
PWM_MODE_UPDATE(pwm, STM32_PWM_CHAN4, PWM_MODE_HSHI_LSLO);
#endif
/* Disable complementary output */
/* Disable complementary outputs */
PWM_OUTPUTS_ENABLE(pwm, PMW_OUTPUTS_ALL_COMP, false);
}
@ -2025,6 +2196,61 @@ static void stm32_foc_curr_get(struct foc_dev_s *dev,
}
}
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/****************************************************************************
* Name: stm32_foc_volt_get
*
* Description:
* Get voltage samples from ADC
*
****************************************************************************/
static void stm32_foc_volt_get(struct foc_dev_s *dev, int16_t *volt)
{
struct stm32_foc_priv_s *priv = STM32_FOC_PRIV_FROM_DEV_GET(dev);
struct stm32_adc_dev_s *vadc = VADC_FROM_FOC_DEV_GET(dev);
int i = 0;
DEBUGASSERT(dev);
DEBUGASSERT(priv);
DEBUGASSERT(vadc);
DEBUGASSERT(volt);
/* Make sure the conversion is complete.
* It is possible that the ADC master sequence will end in front of
* the slave sequence. In that case we just busy-wait.
* In the worst case scenario the slave conversion is one channel behind
* the master conversion (2 current channels vs 3 voltage channels).
*
* Another solution is to make sure that both conversions has the same
* length, but this makes the code much more complex.
*/
while ((FOC_ADC_ISR_FOC & STM32_ADC_INT_GET(vadc)) == 0);
/* Clear status */
STM32_ADC_INT_ACK(vadc, FOC_ADC_ISR_FOC);
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{
/* Get raw voltage samples.
* We have ADC offset enabled for injected channels so this
* gives us signed values.
* NOTE: ADC value is 11 bits + sign.
*/
#ifdef CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND
/* Ignore first channel */
volt[i] = (int16_t)STM32_ADC_INJDATA_GET(vadc, (i + 1));
#else
volt[i] = (int16_t)STM32_ADC_INJDATA_GET(vadc, i);
#endif
}
}
#endif
/****************************************************************************
* Name: stm32_foc_notifier_cfg
*
@ -2167,6 +2393,91 @@ void stm32_foc_trace(struct foc_dev_s *dev, int type, bool state)
}
#endif
/****************************************************************************
* Name: stm32_foc_adc_init
*
* Description:
* Initialize ADC instance
*
****************************************************************************/
struct adc_dev_s *stm32_foc_adc_init(struct stm32_foc_adc_s *adc_cfg)
{
struct adc_dev_s *adc_dev = NULL;
int i = 0;
#ifdef CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND
uint8_t *adc_chan = NULL;
uint8_t adc_nchan = 0;
#endif
DEBUGASSERT(adc_cfg);
DEBUGASSERT(adc_cfg != NULL);
DEBUGASSERT(adc_cfg->pins != NULL);
DEBUGASSERT(adc_cfg->chan != NULL);
/* Configure pins as analog inputs for the selected channels */
for (i = 0; i < adc_cfg->nchan; i++)
{
stm32_configgpio(adc_cfg->pins[i]);
}
/* STM32G4 ADC channel 0 unwanted conversion workaround */
#ifdef CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND
/* Add one dummy channel to conversion */
adc_nchan = (adc_cfg->nchan + 1);
/* Allocate memory for the extended list of channels */
adc_chan = zalloc(adc_nchan);
if (adc_chan == NULL)
{
goto errout;
}
/* Copy regular channels first */
for (i = 0; i < adc_cfg->regch; i += 1)
{
adc_chan[i] = adc_cfg->chan[i];
}
/* Add dummy channel at the beginning of injected channels */
adc_chan[adc_cfg->regch] = 0;
/* Copy injected channels */
for (i = (adc_cfg->regch + 1); i < adc_nchan; i += 1)
{
adc_chan[i] = adc_cfg->chan[i - 1];
}
#endif /* CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND */
/* Get the ADC interface */
#ifdef CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND
adc_dev = stm32_adcinitialize(adc_cfg->intf,
adc_chan,
adc_nchan);
free(adc_chan);
#else
adc_dev = stm32_adcinitialize(adc_cfg->intf,
adc_cfg->chan,
adc_cfg->nchan);
#endif
return adc_dev;
#ifdef CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND
errout:
return NULL;
#endif
}
/****************************************************************************
* Public Functions
****************************************************************************/
@ -2204,11 +2515,6 @@ stm32_foc_initialize(int inst, struct stm32_foc_board_s *board)
uint8_t pwm_inst = 0;
uint8_t adc_inst = 0;
uint32_t pwmfzbit = 0;
int i = 0;
#ifdef CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND
uint8_t *adc_chan = NULL;
uint8_t adc_nchan = 0;
#endif
DEBUGASSERT(board != NULL);
DEBUGASSERT(board->ops != NULL);
@ -2227,11 +2533,6 @@ stm32_foc_initialize(int inst, struct stm32_foc_board_s *board)
DEBUGASSERT(board->ops->trace);
#endif
/* Get ADC configuration from board data */
adc_cfg = board->data->adc_cfg;
DEBUGASSERT(adc_cfg);
/* Get FOC instance configuration */
switch (inst)
@ -2327,86 +2628,62 @@ stm32_foc_initialize(int inst, struct stm32_foc_board_s *board)
goto errout;
}
/* Configure pins as analog inputs for the selected channels */
/* Get ADC configuration */
DEBUGASSERT(adc_cfg != NULL);
DEBUGASSERT(adc_cfg->pins != NULL);
DEBUGASSERT(adc_cfg->chan != NULL);
for (i = 0; i < adc_cfg->nchan; i++)
{
stm32_configgpio(adc_cfg->pins[i]);
}
adc_cfg = board->data->adc_cfg;
/* Make sure that we are using the appropriate ADC interface */
if (adc_inst != adc_cfg->intf)
{
mtrerr("Configuration doesn't match %d, %d\n",
mtrerr("FOC ADC configuration doesn't match %d, %d\n",
adc_inst, adc_cfg->intf);
set_errno(EINVAL);
goto errout;
}
/* STM32G4 ADC channel 0 unwanted conversion workaround */
/* Get ADC instance */
#ifdef CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND
/* Add one dummy channel to conversion */
adc_nchan = (adc_cfg->nchan + 1);
/* Allocate memory for the extended list of channels */
adc_chan = zalloc(adc_nchan);
if (adc_chan == NULL)
foc_dev->adc_dev = stm32_foc_adc_init(adc_cfg);
if (foc_dev->adc_dev == NULL)
{
mtrerr("Failed to initialize FOC ADC%d interface\n", adc_cfg->intf);
set_errno(EINVAL);
goto errout;
}
/* Copy regular channels first */
/* Get ADC private part */
for (i = 0; i < adc_cfg->regch; i += 1)
foc_dev->adc = (struct stm32_adc_dev_s *)foc_dev->adc_dev->ad_priv;
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* Get ADC configuration */
adc_cfg = board->data->vadc_cfg;
/* Make sure that we are using the slave ADC */
if (adc_inst != adc_cfg->intf - 1)
{
adc_chan[i] = adc_cfg->chan[i];
mtrerr("BEMF ADC must be the first slave instance of the main ADC!");
set_errno(EINVAL);
goto errout;
}
/* Add dummy channel at the beginning of injected channels */
/* Get ADC instance */
adc_chan[adc_cfg->regch] = 0;
/* Copy injected channels */
for (i = (adc_cfg->regch + 1); i < adc_nchan; i += 1)
foc_dev->vadc_dev = stm32_foc_adc_init(adc_cfg);
if (foc_dev->vadc_dev == NULL)
{
adc_chan[i] = adc_cfg->chan[i - 1];
}
#endif /* CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND */
/* Get the ADC interface */
#ifdef CONFIG_STM32_FOC_G4_ADCCHAN0_WORKAROUND
foc_dev->adc_dev = stm32_adcinitialize(adc_inst,
adc_chan,
adc_nchan);
free(adc_chan);
#else
foc_dev->adc_dev = stm32_adcinitialize(adc_inst,
adc_cfg->chan,
adc_cfg->nchan);
#endif
if (foc_dev->adc_dev == NULL)
{
mtrerr("Failed to get ADC%d interface\n", adc_cfg->intf);
mtrerr("Failed to initialize BEMF ADC%d interface\n", adc_cfg->intf);
set_errno(EINVAL);
goto errout;
}
/* Get ADC private part */
foc_dev->adc = (struct stm32_adc_dev_s *)foc_dev->adc_dev->ad_priv;
foc_dev->vadc = (struct stm32_adc_dev_s *)foc_dev->vadc_dev->ad_priv;
#endif
/* Froze timer and reset outputs when core is halted.
* TODO: move this to stm32_pwm.c and configure from Kconfig

13
arch/arm/src/stm32/stm32_foc.h
View File

@ -106,6 +106,13 @@ struct stm32_foc_board_ops_s
int (*current_get)(struct foc_dev_s *dev, int16_t *curr_raw,
foc_current_t *curr);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* Get BEMF voltage */
int (*voltage_get)(struct foc_dev_s *dev, int16_t *volt_raw,
foc_voltage_t *volt);
#endif
#ifdef CONFIG_MOTOR_FOC_TRACE
/* FOC trace interface setup */
@ -125,6 +132,12 @@ struct stm32_foc_board_data_s
struct stm32_foc_adc_s *adc_cfg;
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* BEMF voltage ADC configuration */
struct stm32_foc_adc_s *vadc_cfg;
#endif
/* PWM deadtime register value */
uint8_t pwm_dt;

289
arch/arm/src/stm32f7/stm32_foc.c
View File

@ -426,6 +426,49 @@
# endif
#endif
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* Additional checks for BEMF sensing */
# if defined(CONFIG_STM32_FOC_FOC0) && defined(CONFIG_STM32_FOC_FOC1)
# error BEMF sensing supported only for one FOC instance enabled
# endif
# if defined(CONFIG_STM32_FOC_FOC0_ADC2) || defined(CONFIG_STM32_FOC_FOC0_ADC3)
# error FOC must use ADC master
# endif
# if defined(CONFIG_STM32_FOC_FOC1_ADC2) || defined(CONFIG_STM32_FOC_FOC1_ADC3)
# error FOC must use ADC master
# endif
/* Additional ADC slave in use */
# if defined(CONFIG_STM32_FOC_FOC0_ADC1) || defined(CONFIG_STM32_FOC_FOC1_ADC1)
# define CONFIG_STM32_FOC_USE_ADC2
# endif
/* The number of required injected channels */
# define FOC_VADC_INJ_CHAN_REQUIRED (CONFIG_MOTOR_FOC_PHASES)
/* Slave ADC2 */
# ifdef CONFIG_STM32_FOC_USE_ADC2
# ifndef CONFIG_STM32_ADC2
# error ADC2 not supported !
# endif
# ifndef ADC2_HAVE_JEXTCFG
# error ADC2 must support JEXTCFG
# endif
# if CONFIG_STM32_ADC2_ANIOC_TRIGGER != 1
# error CONFIG_STM32_ADC2_ANIOC_TRIGGER must be 1
# endif
# if CONFIG_STM32_ADC2_INJECTED_CHAN != FOC_VADC_INJ_CHAN_REQUIRED
# error Invalid configuration for ADC2 injected channels
# endif
# endif
#endif
/* Helper macros ************************************************************/
/* Get arch-specific FOC private part */
@ -449,7 +492,8 @@
/* Get ADC device */
#define ADC_FROM_FOC_DEV_GET(d) (STM32_FOCDEV_FROM_DEV_GET(d)->adc)
#define ADC_FROM_FOC_DEV_GET(d) (STM32_FOCDEV_FROM_DEV_GET(d)->adc)
#define VADC_FROM_FOC_DEV_GET(d) (STM32_FOC_DEV_FROM_DEV_GET(d)->vadc)
/* Define PWM all outputs */
@ -525,6 +569,11 @@ struct stm32_foc_dev_s
/* Interrupt handler for FOC device */
int (*adc_isr)(struct foc_dev_s *dev);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
struct adc_dev_s *vadc_dev; /* ADC device reference (voltage) */
struct stm32_adc_dev_s *vadc; /* STM32 ADC device reference (voltage) */
#endif
};
/* STM32 FOC common data */
@ -546,6 +595,11 @@ struct stm32_foc_data_s
uint32_t adcint_cntr; /* ADC interrupt counter */
uint32_t curr_offset[CONFIG_MOTOR_FOC_SHUNTS]; /* ADC current offset */
int16_t curr_raw[CONFIG_MOTOR_FOC_SHUNTS]; /* ADC current RAW */
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
foc_voltage_t volt[CONFIG_MOTOR_FOC_PHASES]; /* Voltage */
uint32_t volt_offset[CONFIG_MOTOR_FOC_PHASES]; /* ADC voltage offset */
int16_t volt_raw[CONFIG_MOTOR_FOC_PHASES]; /* ADC voltage RAW */
#endif
};
/* STM32 FOC private */
@ -611,6 +665,9 @@ static int stm32_foc_worker_handler(struct foc_dev_s *dev);
static void stm32_foc_curr_get(struct foc_dev_s *dev,
int16_t *curr, int shunts);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
static void stm32_foc_volt_get(struct foc_dev_s *dev, int16_t *volt);
#endif
static int stm32_foc_notifier_cfg(struct foc_dev_s *dev, uint32_t freq);
static int stm32_foc_pwm_cfg(struct foc_dev_s *dev, uint32_t freq);
static int stm32_foc_adc_cfg(struct foc_dev_s *dev);
@ -869,7 +926,7 @@ static int stm32_foc_pwm_start(struct foc_dev_s *dev, bool state)
if (!dev->state.pwm_off)
{
/* Configure outputs state */
/* Enable PWM outputs */
PWM_ALL_OUTPUTS_ENABLE(pwm, state);
}
@ -1120,6 +1177,9 @@ static int stm32_foc_setup(struct foc_dev_s *dev)
struct stm32_foc_board_s *board = STM32_FOCBOARD_FROM_DEV_GET(dev);
struct stm32_foc_priv_s *priv = STM32_FOCPRIV_FROM_DEV_GET(dev);
struct stm32_adc_dev_s *adc = ADC_FROM_FOC_DEV_GET(dev);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
struct stm32_adc_dev_s *vadc = VADC_FROM_FOC_DEV_GET(dev);
#endif
struct adc_sample_time_s stime;
int ret = OK;
@ -1143,6 +1203,30 @@ static int stm32_foc_setup(struct foc_dev_s *dev)
STM32_ADC_SETUP(foc_dev->adc);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* Setup slave ADC */
STM32_ADC_SETUP(foc_dev->vadc);
/* Disable interrupts for slave ADC */
STM32_ADC_DISABLEINT(foc_dev->vadc);
/* Disable master and slave ADC */
STM32_ADC_ENABLE(foc_dev->adc, false);
STM32_ADC_ENABLE(foc_dev->vadc, false);
/* Configure dual injected simultaneous only mode */
STM32_ADC_MULTICFG(foc_dev->vadc, ADC_MULTIMODE_ISM2);
/* Enable master and slave ADC */
STM32_ADC_ENABLE(foc_dev->adc, true);
STM32_ADC_ENABLE(foc_dev->vadc, true);
#endif
/* Lock ADC common data */
ret = nxmutex_lock(&priv->adc_cmn->lock);
@ -1192,6 +1276,18 @@ static int stm32_foc_setup(struct foc_dev_s *dev)
STM32_ADC_SAMPLETIME_SET(adc, &stime);
STM32_ADC_SAMPLETIME_WRITE(adc);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* Configure sample times for BEMF channels */
memset(&stime, 0, sizeof(struct adc_sample_time_s));
stime.channels_nbr = board->data->vadc_cfg->nchan;
stime.channel = board->data->vadc_cfg->stime;
STM32_ADC_SAMPLETIME_SET(vadc, &stime);
STM32_ADC_SAMPLETIME_WRITE(vadc);
#endif
/* Set the priority of the ADC interrupt vector */
ret = up_prioritize_irq(foc_dev->adc_irq, NVIC_SYSH_PRIORITY_DEFAULT);
@ -1237,6 +1333,9 @@ static int stm32_foc_setup(struct foc_dev_s *dev)
/* Dump ADC regs */
STM32_ADC_DUMP_REGS(adc);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
STM32_ADC_DUMP_REGS(vadc);
#endif
errout:
return ret;
@ -1509,10 +1608,22 @@ static int stm32_foc_worker_handler(struct foc_dev_s *dev)
priv->data.curr_raw,
priv->data.curr);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* Get raw voltage samples */
stm32_foc_volt_get(dev, priv->data.volt_raw);
/* Get BEMF voltages */
ret = board->ops->voltage_get(dev,
priv->data.volt_raw,
priv->data.volt);
#endif
/* Call upper-half worker callback */
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
# error BEMF sensing not supported yet
priv->cb->notifier(dev, priv->data.curr, priv->data.volt);
#else
priv->cb->notifier(dev, priv->data.curr, NULL);
#endif
@ -1536,6 +1647,9 @@ static int stm32_foc_calibration_start(struct foc_dev_s *dev)
struct stm32_foc_board_s *board = STM32_FOCBOARD_FROM_DEV_GET(dev);
struct stm32_pwm_dev_s *pwm = PWM_FROM_FOC_DEV_GET(dev);
struct stm32_adc_dev_s *adc = ADC_FROM_FOC_DEV_GET(dev);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
struct stm32_adc_dev_s *vadc = VADC_FROM_FOC_DEV_GET(dev);
#endif
uint8_t i = 0;
uint8_t ch = 0;
int ret = OK;
@ -1597,6 +1711,36 @@ static int stm32_foc_calibration_start(struct foc_dev_s *dev)
stm32_foc_adc_start(dev, true);
stm32_foc_pwm_start(dev, true);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* TODO: BEMF sensing calibartion */
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{
priv->data.volt_offset[i] = 0;
}
/* Clear last ADC data */
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{
priv->data.volt_raw[i] = 0;
}
/* Set ADC hardware offset for voltage channels (only injected channels) */
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{
/* Get channel */
ch = board->data->vadc_cfg->chan[board->data->vadc_cfg->regch + i];
/* Write offset */
STM32_ADC_OFFSET_SET(vadc, ch, i, priv->data.volt_offset[i]);
}
#endif
/* Wait for calibration done semaphore
* All work is done in adc_calibration_handler
*/
@ -1733,7 +1877,7 @@ static int stm32_foc_pwm_off(struct foc_dev_s *dev, bool off)
PWM_MODE_UPDATE(pwm, STM32_PWM_CHAN4, PWM_MODE_HSHI_LSLO);
#endif
/* Disable complementary output */
/* Disable complementary outputs */
PWM_OUTPUTS_ENABLE(pwm, PMW_OUTPUTS_ALL_COMP, false);
}
@ -1809,6 +1953,55 @@ static void stm32_foc_curr_get(struct foc_dev_s *dev,
}
}
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/****************************************************************************
* Name: stm32_foc_volt_get
*
* Description:
* Get voltage samples from ADC
*
****************************************************************************/
static void stm32_foc_volt_get(struct foc_dev_s *dev, int16_t *volt)
{
struct stm32_foc_priv_s *priv = STM32_FOC_PRIV_FROM_DEV_GET(dev);
struct stm32_adc_dev_s *vadc = VADC_FROM_FOC_DEV_GET(dev);
int i = 0;
DEBUGASSERT(dev);
DEBUGASSERT(priv);
DEBUGASSERT(vadc);
DEBUGASSERT(volt);
/* Make sure the conversion is complete.
* It is possible that the ADC master sequence will end in front of
* the slave sequence. In that case we just busy-wait.
* In the worst case scenario the slave conversion is one channel behind
* the master conversion (2 current channels vs 3 voltage channels).
*
* Another solution is to make sure that both conversions has the same
* length, but this makes the code much more complex.
*/
while ((FOC_ADC_ISR_FOC & STM32_ADC_INT_GET(vadc)) == 0);
/* Clear status */
STM32_ADC_INT_ACK(vadc, FOC_ADC_ISR_FOC);
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{
/* Get raw voltage samples.
* We have ADC offset enabled for injected channels so this
* gives us signed values.
* NOTE: ADC value is 11 bits + sign.
*/
volt[i] = (int16_t)STM32_ADC_INJDATA_GET(vadc, i);
}
}
#endif
/****************************************************************************
* Name: stm32_foc_notifier_cfg
*
@ -1951,6 +2144,40 @@ void stm32_foc_trace(struct foc_dev_s *dev, int type, bool state)
}
#endif
/****************************************************************************
* Name: stm32_foc_adc_init
*
* Description:
* Initialize ADC instance
*
****************************************************************************/
struct adc_dev_s *stm32_foc_adc_init(struct stm32_foc_adc_s *adc_cfg)
{
struct adc_dev_s *adc_dev = NULL;
int i = 0;
DEBUGASSERT(adc_cfg);
DEBUGASSERT(adc_cfg != NULL);
DEBUGASSERT(adc_cfg->pins != NULL);
DEBUGASSERT(adc_cfg->chan != NULL);
/* Configure pins as analog inputs for the selected channels */
for (i = 0; i < adc_cfg->nchan; i++)
{
stm32_configgpio(adc_cfg->pins[i]);
}
/* Get the ADC interface */
adc_dev = stm32_adc_initialize(adc_cfg->intf,
adc_cfg->chan,
adc_cfg->nchan);
return adc_dev;
}
/****************************************************************************
* Public Functions
****************************************************************************/
@ -1986,7 +2213,6 @@ stm32_foc_initialize(int inst, struct stm32_foc_board_s *board)
uint8_t pwm_inst = 0;
uint8_t adc_inst = 0;
uint32_t pwmfzbit = 0;
int i = 0;
DEBUGASSERT(board != NULL);
DEBUGASSERT(board->ops != NULL);
@ -2099,43 +2325,62 @@ stm32_foc_initialize(int inst, struct stm32_foc_board_s *board)
goto errout;
}
/* Configure pins as analog inputs for the selected channels */
/* Get ADC configuration */
DEBUGASSERT(adc_cfg != NULL);
DEBUGASSERT(adc_cfg->pins != NULL);
DEBUGASSERT(adc_cfg->chan != NULL);
for (i = 0; i < adc_cfg->nchan; i++)
{
stm32_configgpio(adc_cfg->pins[i]);
}
adc_cfg = board->data->adc_cfg;
/* Make sure that we are using the appropriate ADC interface */
if (adc_inst != adc_cfg->intf)
{
mtrerr("Configuration doesn't match %d, %d\n",
mtrerr("FOC ADC configuration doesn't match %d, %d\n",
adc_inst, adc_cfg->intf);
set_errno(EINVAL);
goto errout;
}
/* Get the ADC interface */
foc_dev->adc_dev = stm32_adc_initialize(adc_inst,
adc_cfg->chan,
adc_cfg->nchan);
/* Get ADC instance */
foc_dev->adc_dev = stm32_foc_adc_init(adc_cfg);
if (foc_dev->adc_dev == NULL)
{
mtrerr("Failed to get ADC%d interface\n", adc_cfg->intf);
mtrerr("Failed to initialize FOC ADC%d interface\n", adc_cfg->intf);
set_errno(EINVAL);
goto errout;
}
/* Get ADC private part */
foc_dev->adc = (struct stm32_adc_dev_s *)foc_dev->adc_dev->ad_priv;
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* Get ADC configuration */
adc_cfg = board->data->vadc_cfg;
/* Make sure that we are using the slave ADC */
if (adc_inst != adc_cfg->intf - 1)
{
mtrerr("BEMF ADC must be the first slave instance of the main ADC!");
set_errno(EINVAL);
goto errout;
}
/* Get ADC instance */
foc_dev->vadc_dev = stm32_foc_adc_init(adc_cfg);
if (foc_dev->vadc_dev == NULL)
{
mtrerr("Failed to initialize BEMF ADC%d interface\n", adc_cfg->intf);
set_errno(EINVAL);
goto errout;
}
/* Get ADC private part */
foc_dev->adc = (struct stm32_adc_dev_s *)foc_dev->adc_dev->ad_priv;
foc_dev->vadc = (struct stm32_adc_dev_s *)foc_dev->vadc_dev->ad_priv;
#endif
/* Froze timer and reset outputs when core is halted.
* TODO: move this to stm32_pwm.c and configure from Kconfig

13
arch/arm/src/stm32f7/stm32_foc.h
View File

@ -106,6 +106,13 @@ struct stm32_foc_board_ops_s
int (*current_get)(struct foc_dev_s *dev, int16_t *curr_raw,
foc_current_t *curr);
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* Get BEMF voltage */
int (*voltage_get)(struct foc_dev_s *dev, int16_t *volt_raw,
foc_voltage_t *volt);
#endif
#ifdef CONFIG_MOTOR_FOC_TRACE
/* FOC trace interface setup */
@ -125,6 +132,12 @@ struct stm32_foc_board_data_s
struct stm32_foc_adc_s *adc_cfg;
#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE
/* BEMF voltage ADC configuration */
struct stm32_foc_adc_s *vadc_cfg;
#endif
/* PWM deadtime register value */
uint8_t pwm_dt;