From 91d43edffd1ae38a12335dfd3bcfa35f7e44fbc3 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sun, 15 Jan 2023 14:41:02 +0100 Subject: [PATCH] drivers/foc: support for BEMF sensing --- arch/arm/src/stm32/stm32_foc.c | 6 +++++- arch/arm/src/stm32f7/stm32_foc.c | 6 +++++- drivers/motor/foc/Kconfig | 4 ++++ drivers/motor/foc/foc_dev.c | 18 ++++++++++++++++-- drivers/motor/foc/foc_dummy.c | 12 +++++++++++- include/nuttx/motor/foc/foc.h | 6 +++++- include/nuttx/motor/foc/foc_lower.h | 10 ++++++---- 7 files changed, 52 insertions(+), 10 deletions(-) diff --git a/arch/arm/src/stm32/stm32_foc.c b/arch/arm/src/stm32/stm32_foc.c index 836921a3d1..15f2f0b118 100644 --- a/arch/arm/src/stm32/stm32_foc.c +++ b/arch/arm/src/stm32/stm32_foc.c @@ -1717,7 +1717,11 @@ static int stm32_foc_worker_handler(struct foc_dev_s *dev) /* Call upper-half worker callback */ - priv->cb->notifier(dev, priv->data.curr); +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE +# error BEMF sensing not supported yet +#else + priv->cb->notifier(dev, priv->data.curr, NULL); +#endif } return ret; diff --git a/arch/arm/src/stm32f7/stm32_foc.c b/arch/arm/src/stm32f7/stm32_foc.c index be44c8aa40..8f60485d8a 100644 --- a/arch/arm/src/stm32f7/stm32_foc.c +++ b/arch/arm/src/stm32f7/stm32_foc.c @@ -1507,7 +1507,11 @@ static int stm32_foc_worker_handler(struct foc_dev_s *dev) /* Call upper-half worker callback */ - priv->cb->notifier(dev, priv->data.curr); +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE +# error BEMF sensing not supported yet +#else + priv->cb->notifier(dev, priv->data.curr, NULL); +#endif } return ret; diff --git a/drivers/motor/foc/Kconfig b/drivers/motor/foc/Kconfig index 16ab50ad2d..3a36301053 100644 --- a/drivers/motor/foc/Kconfig +++ b/drivers/motor/foc/Kconfig @@ -22,6 +22,10 @@ config MOTOR_FOC_SHUNTS Number of shunts supported (or other types of current sensors). Any current reconstruction must be done on the lower-half side. +config MOTOR_FOC_BEMF_SENSE + bool "FOC support for Back-EMF sensing" + default n + config MOTOR_FOC_TRACE bool "FOC trace support" default n diff --git a/drivers/motor/foc/foc_dev.c b/drivers/motor/foc/foc_dev.c index 65c0f53018..81e34ee842 100644 --- a/drivers/motor/foc/foc_dev.c +++ b/drivers/motor/foc/foc_dev.c @@ -62,7 +62,8 @@ static int foc_info_get(FAR struct foc_dev_s *dev, FAR struct foc_info_s *info); static int foc_notifier(FAR struct foc_dev_s *dev, - FAR foc_current_t *current); + FAR foc_current_t *current, + FAR foc_voltage_t *voltage); /**************************************************************************** * Private Data @@ -726,7 +727,8 @@ static int foc_info_get(FAR struct foc_dev_s *dev, ****************************************************************************/ static int foc_notifier(FAR struct foc_dev_s *dev, - FAR foc_current_t *current) + FAR foc_current_t *current, + FAR foc_voltage_t *voltage) { int ret = OK; int sval = 0; @@ -743,6 +745,18 @@ static int foc_notifier(FAR struct foc_dev_s *dev, current, sizeof(foc_current_t) * CONFIG_MOTOR_FOC_PHASES); +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE + /* Copy voltage */ + + memcpy(&dev->state.volt, + voltage, + sizeof(foc_voltage_t) * CONFIG_MOTOR_FOC_PHASES); +#else + /* If BEMF sampling is not enabled then voltage must be NULL */ + + DEBUGASSERT(voltage == NULL); +#endif + /* Check if the previous cycle was handled */ ret = nxsem_get_value(&dev->statesem, &sval); diff --git a/drivers/motor/foc/foc_dummy.c b/drivers/motor/foc/foc_dummy.c index f85c48523e..c7fa2e09dc 100644 --- a/drivers/motor/foc/foc_dummy.c +++ b/drivers/motor/foc/foc_dummy.c @@ -79,6 +79,12 @@ struct foc_dummy_data_s foc_current_t current[CONFIG_MOTOR_FOC_PHASES]; +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE + /* BEMF voltage */ + + foc_voltage_t volt[CONFIG_MOTOR_FOC_PHASES]; +#endif + /* FOC worker loop helpers */ bool state; @@ -464,7 +470,11 @@ static void foc_dummy_notifier_handler(FAR struct foc_dev_s *dev) { /* Call FOC notifier */ - sim->cb->notifier(dev, sim->current); +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE + sim->cb->notifier(dev, sim->current, sim->voltage); +#else + sim->cb->notifier(dev, sim->current, NULL); +#endif } /* Increase counter */ diff --git a/include/nuttx/motor/foc/foc.h b/include/nuttx/motor/foc/foc.h index bee45811e8..bd6de77f9f 100644 --- a/include/nuttx/motor/foc/foc.h +++ b/include/nuttx/motor/foc/foc.h @@ -60,9 +60,10 @@ enum foc_fault_e FOC_FAULT_BOARD = (1 << 3), /* Board-specific fault */ }; -/* Phase current as signed 32-bit integer */ +/* Phase current and BEMF voltage as signed 32-bit integer */ typedef int32_t foc_current_t; +typedef int32_t foc_voltage_t; /* Phase duty cycle as unsigned fixed16. * We use range [0.0 to 1.0] so this gives us a 16-bit resolution. @@ -84,6 +85,9 @@ struct foc_state_s { uint8_t fault; /* Fault state */ foc_current_t curr[CONFIG_MOTOR_FOC_PHASES]; /* Phase current feedback */ +#ifdef CONFIG_MOTOR_FOC_BEMF_SENSE + foc_voltage_t volt[CONFIG_MOTOR_FOC_PHASES]; /* BEMF voltage feedback */ +#endif }; /* Input data to the FOC device */ diff --git a/include/nuttx/motor/foc/foc_lower.h b/include/nuttx/motor/foc/foc_lower.h index b154e3c504..2ad2e16822 100644 --- a/include/nuttx/motor/foc/foc_lower.h +++ b/include/nuttx/motor/foc/foc_lower.h @@ -77,13 +77,15 @@ struct foc_callbacks_s /* FOC notifier callback * * Description: - * Deliver the phase current samples and wake up the thread waiting. - * Must be called by lower-half logic at a frequency determined by - * configuration (notifier_freq in foc_cfg_s). + * Deliver the phase current samples (and optional BEMF voltages) + * and wake up the thread waiting. Must be called by lower-half + * logic at a frequency determined by configuration (notifier_freq + * in foc_cfg_s). */ CODE int (*notifier)(FAR struct foc_dev_s *dev, - FAR foc_current_t *current); + FAR foc_current_t *current, + FAR foc_voltage_t *voltage); }; /* Lower-half FOC operations */