From 30015b862c5aa61b8fbf31760570ac3a469f2df6 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Thu, 19 Jan 2023 14:29:58 +0100 Subject: [PATCH] drivers/foc: add ioctl interface that turn off all PWM switches --- drivers/motor/foc/foc_dev.c | 63 ++++++++++++++++++++++++++++- drivers/motor/foc/foc_dummy.c | 17 ++++++++ include/nuttx/motor/foc/foc.h | 1 + include/nuttx/motor/foc/foc_lower.h | 5 +++ include/nuttx/motor/motor_ioctl.h | 1 + 5 files changed, 86 insertions(+), 1 deletion(-) diff --git a/drivers/motor/foc/foc_dev.c b/drivers/motor/foc/foc_dev.c index 81e34ee842..09c1a9b614 100644 --- a/drivers/motor/foc/foc_dev.c +++ b/drivers/motor/foc/foc_dev.c @@ -60,6 +60,7 @@ static int foc_params_set(FAR struct foc_dev_s *dev, static int foc_fault_clear(FAR struct foc_dev_s *dev); static int foc_info_get(FAR struct foc_dev_s *dev, FAR struct foc_info_s *info); +static int foc_pwm_off(FAR struct foc_dev_s *dev, bool off); static int foc_notifier(FAR struct foc_dev_s *dev, FAR foc_current_t *current, @@ -234,6 +235,9 @@ static int foc_close(FAR struct file *filep) * MTRIOC_GET_INFO: Get the FOC device info, * arg: struct foc_info_s pointer * + * MTRIOC_PWM_OFF: Force all PWM switches to the off state. + * arg: bool pointer + * ****************************************************************************/ static int foc_ioctl(FAR struct file *filep, int cmd, unsigned long arg) @@ -340,7 +344,7 @@ static int foc_ioctl(FAR struct file *filep, int cmd, unsigned long arg) case MTRIOC_GET_INFO: { - FAR struct foc_info_s *info = (struct foc_info_s *)arg; + FAR struct foc_info_s *info = (FAR struct foc_info_s *)arg; DEBUGASSERT(info != NULL); @@ -353,6 +357,21 @@ static int foc_ioctl(FAR struct file *filep, int cmd, unsigned long arg) break; } + case MTRIOC_PWM_OFF: + { + FAR bool *off = (FAR bool *)arg; + + DEBUGASSERT(off != NULL); + + ret = foc_pwm_off(dev, *off); + if (ret != OK) + { + mtrerr("MTRIOC_PWM_OFF failed %d\n", ret); + } + + break; + } + /* Not supported */ default: @@ -384,6 +403,7 @@ static int foc_lower_ops_assert(FAR struct foc_lower_ops_s *ops) DEBUGASSERT(ops->setup); DEBUGASSERT(ops->shutdown); DEBUGASSERT(ops->start); + DEBUGASSERT(ops->pwm_off); DEBUGASSERT(ops->ioctl); DEBUGASSERT(ops->bind); DEBUGASSERT(ops->fault_clear); @@ -503,6 +523,13 @@ static int foc_start(FAR struct foc_dev_s *dev) goto errout; } + if (!dev->state.pwm_off) + { + /* Make sure that PWM is enabled if pwm_off was not called before */ + + ret = foc_pwm_off(dev, false); + } + /* Start the FOC */ ret = FOC_OPS_START(dev, true); @@ -537,6 +564,10 @@ static int foc_stop(FAR struct foc_dev_s *dev) memset(&d_zero, 0, CONFIG_MOTOR_FOC_PHASES * sizeof(foc_duty_t)); + /* Make sure that PWM is disabled */ + + ret = foc_pwm_off(dev, true); + /* Reset duty cycle */ ret = FOC_OPS_DUTY(dev, d_zero); @@ -685,6 +716,16 @@ static int foc_params_set(FAR struct foc_dev_s *dev, DEBUGASSERT(dev); DEBUGASSERT(params); + /* If PWM switches are turned off, the change of duty cycle has no + * effect. + */ + + if (dev->state.pwm_off) + { + ret = -EPERM; + goto errout; + } + #ifdef CONFIG_MOTOR_FOC_TRACE FOC_OPS_TRACE(dev, FOC_TRACE_PARAMS, true); #endif @@ -697,6 +738,7 @@ static int foc_params_set(FAR struct foc_dev_s *dev, FOC_OPS_TRACE(dev, FOC_TRACE_PARAMS, false); #endif +errout: return ret; } @@ -718,6 +760,25 @@ static int foc_info_get(FAR struct foc_dev_s *dev, return OK; } +/**************************************************************************** + * Name: foc_info_get + * + * Description: + * Force all PWM swichtes to the off state + * + ****************************************************************************/ + +static int foc_pwm_off(FAR struct foc_dev_s *dev, bool off) +{ + DEBUGASSERT(dev); + + /* Update device state */ + + dev->state.pwm_off = off; + + return FOC_OPS_PWMOFF(dev, off); +} + /**************************************************************************** * Name: foc_notifier * diff --git a/drivers/motor/foc/foc_dummy.c b/drivers/motor/foc/foc_dummy.c index c7fa2e09dc..75582439e3 100644 --- a/drivers/motor/foc/foc_dummy.c +++ b/drivers/motor/foc/foc_dummy.c @@ -107,6 +107,7 @@ static int foc_dummy_shutdown(FAR struct foc_dev_s *dev); static int foc_dummy_start(FAR struct foc_dev_s *dev, bool state); static int foc_dummy_pwm_duty_set(FAR struct foc_dev_s *dev, FAR foc_duty_t *duty); +static int foc_pwm_off(struct foc_dev_s *dev, bool off); static int foc_dummy_ioctl(FAR struct foc_dev_s *dev, int cmd, unsigned long arg); static int foc_dummy_bind(FAR struct foc_dev_s *dev, @@ -146,6 +147,7 @@ static struct foc_lower_ops_s g_foc_dummy_ops = foc_dummy_setup, foc_dummy_shutdown, foc_dummy_pwm_duty_set, + foc_pwm_off, foc_dummy_start, foc_dummy_ioctl, foc_dummy_bind, @@ -518,6 +520,21 @@ static int foc_dummy_pwm_duty_set(FAR struct foc_dev_s *dev, return OK; } +/**************************************************************************** + * Name: foc_pwm_off + * + * Description: + * Set the 3-phase bridge switches in off state. + * + ****************************************************************************/ + +static int foc_pwm_off(struct foc_dev_s *dev, bool off) +{ + mtrinfo("[PWM_OFF] %d\n", off); + + return OK; +} + /**************************************************************************** * Name: foc_dummy_hw_config_get * diff --git a/include/nuttx/motor/foc/foc.h b/include/nuttx/motor/foc/foc.h index bd6de77f9f..ba1d197dd9 100644 --- a/include/nuttx/motor/foc/foc.h +++ b/include/nuttx/motor/foc/foc.h @@ -83,6 +83,7 @@ struct foc_cfg_s struct foc_state_s { + bool pwm_off; /* PWM switches disabled */ uint8_t fault; /* Fault state */ foc_current_t curr[CONFIG_MOTOR_FOC_PHASES]; /* Phase current feedback */ #ifdef CONFIG_MOTOR_FOC_BEMF_SENSE diff --git a/include/nuttx/motor/foc/foc_lower.h b/include/nuttx/motor/foc/foc_lower.h index 2ad2e16822..5eee69340e 100644 --- a/include/nuttx/motor/foc/foc_lower.h +++ b/include/nuttx/motor/foc/foc_lower.h @@ -45,6 +45,7 @@ #define FOC_OPS_SETUP(d) (d)->lower->ops->setup(d) #define FOC_OPS_SHUTDOWN(d) (d)->lower->ops->shutdown(d) #define FOC_OPS_START(d, s) (d)->lower->ops->start(d, s) +#define FOC_OPS_PWMOFF(d, o) (d)->lower->ops->pwm_off(d, o) #define FOC_OPS_DUTY(d, x) (d)->lower->ops->pwm_duty_set(d, x) #define FOC_OPS_IOCTL(d, c, a) (d)->lower->ops->ioctl(d, c, a) #define FOC_OPS_BIND(d, c) (d)->lower->ops->bind(d, c) @@ -110,6 +111,10 @@ struct foc_lower_ops_s CODE int (*pwm_duty_set)(FAR struct foc_dev_s *dev, FAR foc_duty_t *duty); + /* Force all PWM switches to the off state */ + + CODE int (*pwm_off)(FAR struct foc_dev_s *dev, bool off); + /* Lower-half start/stop */ CODE int (*start)(FAR struct foc_dev_s *dev, bool state); diff --git a/include/nuttx/motor/motor_ioctl.h b/include/nuttx/motor/motor_ioctl.h index 51220643f6..d026a29b6e 100644 --- a/include/nuttx/motor/motor_ioctl.h +++ b/include/nuttx/motor/motor_ioctl.h @@ -49,5 +49,6 @@ #define MTRIOC_SET_LIMITS _MTRIOC(9) #define MTRIOC_SET_FAULT _MTRIOC(10) #define MTRIOC_GET_FAULT _MTRIOC(11) +#define MTRIOC_PWM_OFF _MTRIOC(12) #endif /* __INCLUDE_NUTTX_MOTOR_MOTOR_IOCTL_H */