diff --git a/Documentation/components/drivers/character/foc.rst b/Documentation/components/drivers/character/foc.rst new file mode 100644 index 00000000000..1a1612acb75 --- /dev/null +++ b/Documentation/components/drivers/character/foc.rst @@ -0,0 +1,48 @@ +==================== +FOC Driver Interface +==================== + +Field Oriented Control (FOC) is a common technique to control +either synchronous or asynchronous alternating current machines. +The main goal of FOC is to control direct current (Id) and +quadrature current (Iq) in powered device. + +The device on the kernel side is responsible for the following: + +#. update PWM duty cycles +#. return ADC current samples +#. synchronize user-space with PWM events + +The Nuttx FOC driver is split into two parts: + +#. An "upper half", generic driver that provides the common FOC + interface to application level code, +#. A "lower half", platform-specific driver that implemets + the low-level logic to implement the FOC functionality + +Files supporting FOC can be found in the following locations: + +- ``include/nuttx/motor/foc/foc.h``. + "Upper-half" FOC interface available for the user-space. +- ``include/nuttx/motor/foc/foc_lower.h``. + "Lower-half" FOC interface. +- ``drivers/motor/foc/foc_dev.c``. + The generic "upper half" FOC driver. + +The majority of the functionality available to the application +is implemented in driver ioctl calls. Supported ioctl commands: + +- ``MTRIOC_START`` - Start the FOC device, arg: none. +- ``MTRIOC_STOP`` - Stop the FOC device, arg: none. +- ``MTRIOC_GET_STATE`` - Get the FOC device state, + arg: ``struct foc_state_s`` pointer. + This is a blocking operation that is used to synchronize the user space + application with ADC samples. +- ``MTRIOC_CLEAR_FAULT`` - Clear the FOC device fault state, + arg: none. +- ``MTRIOC_SET_PARAMS`` - Set the FOC device operation parameters, + arg: ``struct foc_params_s`` pointer. +- ``MTRIOC_SET_CONFIG`` - Set the FOC device configuration, + arg: ``struct foc_cfg_s`` pointer. +- ``MTRIOC_GET_INFO`` - Get the FOC device info, + arg: ``struct foc_info_s`` pointer. diff --git a/Documentation/components/drivers/character/index.rst b/Documentation/components/drivers/character/index.rst index d72b6ccf615..f0411bb7149 100644 --- a/Documentation/components/drivers/character/index.rst +++ b/Documentation/components/drivers/character/index.rst @@ -64,4 +64,5 @@ Character device drivers have these properties: watchdog.rst keypad.rst note.rst + foc.rst diff --git a/drivers/Kconfig b/drivers/Kconfig index 5a6950381a2..bf77c8dea9a 100644 --- a/drivers/Kconfig +++ b/drivers/Kconfig @@ -130,3 +130,4 @@ source drivers/syslog/Kconfig source drivers/platform/Kconfig source drivers/rf/Kconfig source drivers/rc/Kconfig +source drivers/motor/Kconfig diff --git a/drivers/Makefile b/drivers/Makefile index f63932fa57f..88c5339aa17 100644 --- a/drivers/Makefile +++ b/drivers/Makefile @@ -29,6 +29,7 @@ include audio/Make.defs include bch/Make.defs include can/Make.defs include crypto/Make.defs +include motor/Make.defs include i2c/Make.defs include i2s/Make.defs include input/Make.defs diff --git a/drivers/motor/Kconfig b/drivers/motor/Kconfig new file mode 100644 index 00000000000..a6a6f2f1f6b --- /dev/null +++ b/drivers/motor/Kconfig @@ -0,0 +1,14 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +menuconfig MOTOR + bool "Motor control drivers" + default n + +if MOTOR + +source "drivers/motor/foc/Kconfig" + +endif # MOTOR diff --git a/drivers/motor/Make.defs b/drivers/motor/Make.defs new file mode 100644 index 00000000000..5a05409b923 --- /dev/null +++ b/drivers/motor/Make.defs @@ -0,0 +1,35 @@ +############################################################################ +# drivers/motor/Make.defs +# +# 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. +# +############################################################################ + +# Add FOC driver + +ifeq ($(CONFIG_MOTOR_FOC),y) +include motor$(DELIM)foc$(DELIM)Make.defs +endif + +# Include motor drivers in the build + +MOTOR_DEPPATH := --dep-path motor +MOTOR_VPATH := :motor +MOTOR_CFLAGS := ${shell $(INCDIR) "$(CC)" $(TOPDIR)$(DELIM)drivers$(DELIM)motr} + +DEPPATH += $(MOTOR_DEPPATH) +VPATH += $(MOTOR_VPATH) +CFLAGS += $(MOTOR_CFLAGS) \ No newline at end of file diff --git a/drivers/motor/foc/Kconfig b/drivers/motor/foc/Kconfig new file mode 100644 index 00000000000..75bb91eb221 --- /dev/null +++ b/drivers/motor/foc/Kconfig @@ -0,0 +1,31 @@ +menuconfig MOTOR_FOC + bool "FOC (Field Oriented Controller) driver support" + default n + ---help--- + Enables building of the "upper-half" FOC driver. + +if MOTOR_FOC + +config MOTOR_FOC_INST + int "FOC instances" + default 1 + +config MOTOR_FOC_PHASES + int "FOC phases number" + default 3 + +config MOTOR_FOC_SHUNTS + int "FOC number of shunts" + range 1 3 + default 3 + ---help--- + Number of shunts supported (or other types of current sensors). + Any current recontruction must be done on the lower-half side. + +config MOTOR_FOC_TRACE + bool "FOC trace support" + default n + ---help--- + Enables FOC driver trace interface. + +endif #MOTOR_FOC diff --git a/drivers/motor/foc/Make.defs b/drivers/motor/foc/Make.defs new file mode 100644 index 00000000000..c557e20f146 --- /dev/null +++ b/drivers/motor/foc/Make.defs @@ -0,0 +1,29 @@ +############################################################################ +# drivers/motor/foc/Make.defs +# +# 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. +# +############################################################################ + +# Include FOC driver into the build + +CSRCS += foc_dev.c + +# Include FOC driver build support + +DEPPATH += --dep-path motor$(DELIM)foc +VPATH += :motor$(DELIM)foc +CFLAGS += ${shell $(INCDIR) $(INCDIROPT) "$(CC)" $(TOPDIR)$(DELIM)drivers$(DELIM)motor$(DELIM)foc} \ No newline at end of file diff --git a/drivers/motor/foc/foc_dev.c b/drivers/motor/foc/foc_dev.c new file mode 100644 index 00000000000..41de5953330 --- /dev/null +++ b/drivers/motor/foc/foc_dev.c @@ -0,0 +1,909 @@ +/**************************************************************************** + * drivers/motor/foc/foc_dev.c + * Upper-half FOC controller logic + * + * 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 +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int foc_open(FAR struct file *filep); +static int foc_close(FAR struct file *filep); +static int foc_ioctl(FAR struct file *filep, int cmd, unsigned long arg); + +static int foc_lower_ops_assert(FAR struct foc_lower_ops_s *ops); + +static int foc_setup(FAR struct foc_dev_s *dev); +static int foc_shutdown(FAR struct foc_dev_s *dev); +static int foc_stop(FAR struct foc_dev_s *dev); +static int foc_start(FAR struct foc_dev_s *dev); +static int foc_cfg_set(FAR struct foc_dev_s *dev, FAR struct foc_cfg_s *cfg); +static int foc_state_get(FAR struct foc_dev_s *dev, + FAR struct foc_state_s *state); +static int foc_params_set(FAR struct foc_dev_s *dev, + FAR struct foc_params_s *params); +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_notifier(FAR struct foc_dev_s *dev, + FAR foc_current_t *current); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* Device counter */ + +static uint8_t g_devno_cntr = 0; + +/* File operations */ + +static const struct file_operations g_foc_fops = +{ + foc_open, /* open */ + foc_close, /* close */ + NULL, /* read */ + NULL, /* write */ + NULL, /* seek */ + foc_ioctl, /* ioctl */ + NULL /* poll */ +#ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS + , NULL /* unlink */ +#endif +}; + +/* FOC callbacks from the lower-half implementation to this driver */ + +static struct foc_callbacks_s g_foc_callbacks = +{ + .notifier = foc_notifier, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_open + * + * Description: + * This function is called whenever the foc device is opened. + * + ****************************************************************************/ + +static int foc_open(FAR struct file *filep) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct foc_dev_s *dev = inode->i_private; + uint8_t tmp = 0; + int ret = OK; + irqstate_t flags; + + /* Non-blocking operations not supported */ + + if (filep->f_oflags & O_NONBLOCK) + { + ret = -EPERM; + goto errout; + } + + /* If the port is the middle of closing, wait until the close is finished */ + + ret = nxsem_wait(&dev->closesem); + if (ret >= 0) + { + /* Increment the count of references to the device. If this the first + * time that the driver has been opened for this device, then + * initialize the device. + */ + + tmp = dev->ocount + 1; + if (tmp == 0) + { + /* More than 255 opens; uint8_t overflows to zero */ + + ret = -EMFILE; + } + else + { + /* Check if this is the first time that the driver has been opened + */ + + if (tmp == 1) + { + /* Yes.. perform one time driver setup */ + + flags = enter_critical_section(); + + ret = foc_setup(dev); + if (ret == OK) + { + /* Save the new open count on success */ + + dev->ocount = tmp; + } + + leave_critical_section(flags); + } + else + { + /* Save the incremented open count */ + + dev->ocount = tmp; + } + } + + nxsem_post(&dev->closesem); + } + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_close + * + * Description: + * This routine is called when the foc device is closed. + * + ****************************************************************************/ + +static int foc_close(FAR struct file *filep) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct foc_dev_s *dev = inode->i_private; + int ret = 0; + irqstate_t flags; + + ret = nxsem_wait(&dev->closesem); + if (ret >= 0) + { + /* Decrement the references to the driver. If the reference count will + * decrement to 0, then uninitialize the driver. + */ + + if (dev->ocount > 1) + { + dev->ocount--; + nxsem_post(&dev->closesem); + } + else + { + /* There are no more references to the port */ + + dev->ocount = 0; + + /* Shutdown the device */ + + flags = enter_critical_section(); + ret = foc_shutdown(dev); + leave_critical_section(flags); + + nxsem_post(&dev->closesem); + } + } + + return ret; +} + +/**************************************************************************** + * Name: foc_ioctl + * + * Description: + * Supported IOCTLs: + * + * MTRIOC_START: Start the FOC device, + * arg: none + * + * MTRIOC_STOP: Stop the FOC device, + * arg: none + * + * MTRIOC_GET_STATE: Get the FOC device state, + * arg: struct foc_state_s pointer + * This is a blocking operation that is used to + * synchronize the user space application with + * a FOC worker. + * + * MTRIOC_CLEAR_FAULT: Clear the FOC device fault state, + * arg: none + * + * MTRIOC_SET_PARAMS: Set the FOC device operation parameters, + * arg: struct foc_params_s pointer + * + * MTRIOC_SET_CONFIG: Set the FOC device configuration, + * arg: struct foc_cfg_s pointer + * + * MTRIOC_GET_INFO: Get the FOC device info, + * arg: struct foc_info_s pointer + * + ****************************************************************************/ + +static int foc_ioctl(FAR struct file *filep, int cmd, unsigned long arg) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct foc_dev_s *dev = inode->i_private; + int ret = 0; + irqstate_t flags; + + flags = enter_critical_section(); + + switch (cmd) + { + /* Start the FOC device */ + + case MTRIOC_START: + { + ret = foc_start(dev); + if (ret != OK) + { + pwrerr("ERROR: MTRIOC_START failed %d\n", ret); + } + + break; + } + + /* Stop the FOC device */ + + case MTRIOC_STOP: + { + ret = foc_stop(dev); + if (ret != OK) + { + pwrerr("ERROR: MTRIOC_STOP failed %d\n", ret); + } + + break; + } + + /* Get device state */ + + case MTRIOC_GET_STATE: + { + FAR struct foc_state_s *state = (FAR struct foc_state_s *)arg; + + DEBUGASSERT(state != NULL); + + ret = foc_state_get(dev, state); + if (ret != OK) + { + pwrerr("ERROR: MTRIOC_GET_STATE failed %d\n", ret); + } + + break; + } + + /* Clear fault state */ + + case MTRIOC_CLEAR_FAULT: + { + DEBUGASSERT(arg == 0); + + ret = foc_fault_clear(dev); + if (ret != OK) + { + pwrerr("ERROR: MTRIOC_CLEAR_FAULT failed %d\n", ret); + } + + break; + } + + /* Set device parameters */ + + case MTRIOC_SET_PARAMS: + { + FAR struct foc_params_s *params = (FAR struct foc_params_s *)arg; + + DEBUGASSERT(params != NULL); + + ret = foc_params_set(dev, params); + if (ret != OK) + { + pwrerr("ERROR: MTRIOC_SET_PARAMS failed %d\n", ret); + } + + break; + } + + /* Set the device configuration */ + + case MTRIOC_SET_CONFIG: + { + FAR struct foc_cfg_s *cfg = (FAR struct foc_cfg_s *)arg; + + DEBUGASSERT(cfg != NULL); + + ret = foc_cfg_set(dev, cfg); + if (ret != OK) + { + pwrerr("ERROR: MTRIOC_SET_CONFIG failed %d\n", ret); + } + + break; + } + + /* Get the FOC device info */ + + case MTRIOC_GET_INFO: + { + FAR struct foc_info_s *info = (struct foc_info_s *)arg; + + DEBUGASSERT(info != NULL); + + ret = foc_info_get(dev, info); + if (ret != OK) + { + pwrerr("ERROR: MTRIOC_GET_INFO failed %d\n", ret); + } + + break; + } + + /* Not supported */ + + default: + { + pwrinfo("Forwarding unrecognized cmd: %d arg: %ld\n", cmd, arg); + + /* Call lower-half logic */ + + ret = FOC_OPS_IOCTL(dev, cmd, arg); + + break; + } + } + + leave_critical_section(flags); + + return ret; +} + +/**************************************************************************** + * Name: foc_lower_ops_assert + * + * Description: + * Assert the lower-half FOC operations + * + ****************************************************************************/ + +static int foc_lower_ops_assert(FAR struct foc_lower_ops_s *ops) +{ + DEBUGASSERT(ops->configure); + DEBUGASSERT(ops->setup); + DEBUGASSERT(ops->shutdown); + DEBUGASSERT(ops->start); + DEBUGASSERT(ops->ioctl); + DEBUGASSERT(ops->bind); + DEBUGASSERT(ops->fault_clear); +#ifdef CONFIG_MOTOR_FOC_TRACE + DEBUGASSERT(ops->trace); +#endif + + UNUSED(ops); + + return OK; +} + +/**************************************************************************** + * Name: foc_lower_bind + * + * Description: + * Bind the upper-half with the lower-half FOC logic + * + ****************************************************************************/ + +static int foc_lower_bind(FAR struct foc_dev_s *dev) +{ + DEBUGASSERT(dev); + DEBUGASSERT(g_foc_callbacks.notifier); + + return FOC_OPS_BIND(dev, &g_foc_callbacks); +} + +/**************************************************************************** + * Name: foc_setup + * + * Description: + * Setup the FOC device + * + ****************************************************************************/ + +static int foc_setup(FAR struct foc_dev_s *dev) +{ + int ret = OK; + + DEBUGASSERT(dev); + + pwrinfo("FOC SETUP\n"); + + /* Reset device data */ + + memset(&dev->cfg, 0, sizeof(struct foc_cfg_s)); + memset(&dev->state, 0, sizeof(struct foc_state_s)); + + /* Bind the upper-half with the lower-half FOC logic */ + + ret = foc_lower_bind(dev); + if (ret < 0) + { + pwrerr("ERROR: foc_lower_bind failed %d\n", ret); + set_errno(EINVAL); + goto errout; + } + + /* Call lower-half setup */ + + ret = FOC_OPS_SETUP(dev); + if (ret < 0) + { + pwrerr("FOC_OPS_SETUP failed %d\n", ret); + goto errout; + } + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_shutdown + * + * Description: + * Shutdown the FOC device + * + ****************************************************************************/ + +int foc_shutdown(FAR struct foc_dev_s *dev) +{ + int ret = OK; + + DEBUGASSERT(dev); + + pwrinfo("FOC SHUTDOWN\n"); + + /* Call the lower-half shutdown */ + + ret = FOC_OPS_SHUTDOWN(dev); + + return ret; +} + +/**************************************************************************** + * Name: foc_start + * + * Description: + * Start the FOC device + * + ****************************************************************************/ + +static int foc_start(FAR struct foc_dev_s *dev) +{ + int ret = OK; + + DEBUGASSERT(dev); + + pwrinfo("FOC START\n"); + + /* Reset the notifier semaphore */ + + ret = nxsem_reset(&dev->statesem, 0); + if (ret < 0) + { + pwrerr("ERROR: nxsem_reset failed %d\n", ret); + goto errout; + } + + /* Start the FOC */ + + ret = FOC_OPS_START(dev, true); + if (ret < 0) + { + pwrerr("ERROR: FOC_OPS_START failed %d !\n", ret); + goto errout; + } + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_stop + * + * Description: + * Stop the FOC device + * + ****************************************************************************/ + +static int foc_stop(FAR struct foc_dev_s *dev) +{ + foc_duty_t d_zero[CONFIG_MOTOR_FOC_PHASES]; + int ret = OK; + + DEBUGASSERT(dev); + + pwrinfo("FOC STOP\n"); + + /* Zero duty cycle */ + + memset(&d_zero, 0, CONFIG_MOTOR_FOC_PHASES * sizeof(foc_duty_t)); + + /* Reset duty cycle */ + + ret = FOC_OPS_DUTY(dev, d_zero); + if (ret < 0) + { + pwrerr("ERROR: FOC_OPS_DUTY failed %d\n", ret); + } + + /* Stop the FOC */ + + ret = FOC_OPS_START(dev, false); + if (ret < 0) + { + pwrerr("ERROR: FOC_OPS_START failed %d\n", ret); + } + + /* Reset device data */ + + memset(&dev->state, 0, sizeof(struct foc_state_s)); + + return ret; +} + +/**************************************************************************** + * Name: foc_cfg_set + * + * Description: + * Set the FOC device configuration + * + ****************************************************************************/ + +static int foc_cfg_set(FAR struct foc_dev_s *dev, FAR struct foc_cfg_s *cfg) +{ + int ret = OK; + + DEBUGASSERT(dev); + DEBUGASSERT(cfg); + + DEBUGASSERT(cfg->pwm_freq > 0); + DEBUGASSERT(cfg->notifier_freq > 0); + + /* Copy common configuration */ + + memcpy(&dev->cfg, cfg, sizeof(struct foc_cfg_s)); + + pwrinfo("FOC %" PRIu8 " PWM=%" PRIu32 " notifier=%" PRIu32 "\n", + dev->devno, dev->cfg.pwm_freq, dev->cfg.notifier_freq); + + /* Call arch configuration */ + + ret = FOC_OPS_CONFIGURE(dev, &dev->cfg); + if (ret < 0) + { + pwrerr("FOC_OPS_CONFIGURE failed %d\n", ret); + goto errout; + } + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_state_get + * + * Description: + * Get the FOC device state + * + ****************************************************************************/ + +static int foc_state_get(FAR struct foc_dev_s *dev, + FAR struct foc_state_s *state) +{ + int ret = OK; + + DEBUGASSERT(dev); + DEBUGASSERT(state); + + /* Signal trace */ + +#ifdef CONFIG_MOTOR_FOC_TRACE + FOC_OPS_TRACE(dev, FOC_TRACE_STATE, true); +#endif + + /* Wait for notification if blocking */ + + ret = nxsem_wait_uninterruptible(&dev->statesem); + if (ret < 0) + { + goto errout; + } + +#ifdef CONFIG_MOTOR_FOC_TRACE + FOC_OPS_TRACE(dev, FOC_TRACE_STATE, false); +#endif + + /* Copy state */ + + memcpy(state, &dev->state, sizeof(struct foc_state_s)); + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_fault_clear + * + * Description: + * Clear the FOC device fault state + * + ****************************************************************************/ + +static int foc_fault_clear(FAR struct foc_dev_s *dev) +{ + int ret = OK; + + /* Call lower-half logic */ + + ret = FOC_OPS_FAULT_CLEAR(dev); + if (ret < 0) + { + pwrerr("ERROR: FOC_OPS_FAULT_CLEAR failed %d\n", ret); + goto errout; + } + + /* Clear all faults */ + + dev->state.fault = FOC_FAULT_NONE; + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_params_set + * + * Description: + * Set the FOC device parameters + * + ****************************************************************************/ + +static int foc_params_set(FAR struct foc_dev_s *dev, + FAR struct foc_params_s *params) +{ + int ret = OK; + + DEBUGASSERT(dev); + DEBUGASSERT(params); + +#ifdef CONFIG_MOTOR_FOC_TRACE + FOC_OPS_TRACE(dev, FOC_TRACE_PARAMS, true); +#endif + + /* Set new duty */ + + ret = FOC_OPS_DUTY(dev, params->duty); + +#ifdef CONFIG_MOTOR_FOC_TRACE + FOC_OPS_TRACE(dev, FOC_TRACE_PARAMS, false); +#endif + + return ret; +} + +/**************************************************************************** + * Name: foc_info_get + * + * Description: + * Get the FOC device info + * + ****************************************************************************/ + +static int foc_info_get(FAR struct foc_dev_s *dev, + FAR struct foc_info_s *info) +{ + /* Copy data from device */ + + memcpy(info, &dev->info, sizeof(struct foc_info_s)); + + return OK; +} + +/**************************************************************************** + * Name: foc_notifier + * + * Description: + * Notify the user-space and provide the phase current samples + * + ****************************************************************************/ + +static int foc_notifier(FAR struct foc_dev_s *dev, + FAR foc_current_t *current) +{ + int ret = OK; + int sval = 0; + + DEBUGASSERT(dev != NULL); + +#ifdef CONFIG_MOTOR_FOC_TRACE + FOC_OPS_TRACE(dev, FOC_TRACE_NOTIFIER, true); +#endif + + /* Disable pre-emption until all of the waiting threads have been + * restarted. This is necessary to assure that the sval behaves as + * expected in the following while loop + */ + + sched_lock(); + + /* Copy currents */ + + memcpy(&dev->state.curr, + current, + sizeof(foc_current_t) * CONFIG_MOTOR_FOC_PHASES); + + /* Check if the previous cycle was handled */ + + ret = nxsem_get_value(&dev->statesem, &sval); + if (ret != OK) + { + ret = -EINVAL; + } + else + { + if (sval < -dev->ocount) + { + /* This is a critical fault */ + + DEBUGASSERT(0); + + /* Set timeout fault if not in debug mode */ + + dev->state.fault |= FOC_FAULT_TIMEOUT; + + /* Reset semaphore */ + + nxsem_reset(&dev->statesem, 0); + } + else + { + /* Loop until all of the waiting threads have been restarted. */ + + while (sval < 0) + { + /* Post semaphore */ + + nxsem_post(&dev->statesem); + + /* Increment the semaphore count (as was done by the + * above post). + */ + + sval += 1; + } + } + } + + /* Now we can let the restarted threads run */ + + sched_unlock(); + +#ifdef CONFIG_MOTOR_FOC_TRACE + FOC_OPS_TRACE(dev, FOC_TRACE_NOTIFIER, false); +#endif + + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_register + * + * Description: + * Register the FOC character device as 'path' + * + * Input Parameters: + * path - The full path to the driver to register + * dev - An instance of the FOC device + * + ****************************************************************************/ + +int foc_register(FAR const char *path, FAR struct foc_dev_s *dev) +{ + int ret = OK; + + DEBUGASSERT(path != NULL); + DEBUGASSERT(dev != NULL); + + /* Lower-half must be initialized */ + + DEBUGASSERT(dev->lower); + DEBUGASSERT(dev->lower->ops); + DEBUGASSERT(dev->lower->data); + + /* Check if the device instance is supported by the driver */ + + if (dev->devno > CONFIG_MOTOR_FOC_INST) + { + pwrerr("ERROR: unsupported foc devno %d\n\n", dev->devno); + set_errno(EINVAL); + ret = ERROR; + goto errout; + } + + /* Reset counter */ + + dev->ocount = 0; + + /* Store device number */ + + dev->devno = g_devno_cntr; + + /* Assert the lower-half interface */ + + ret = foc_lower_ops_assert(dev->lower->ops); + if (ret < 0) + { + goto errout; + } + + /* Initialize semaphores */ + + nxsem_init(&dev->closesem, 0, 1); + nxsem_init(&dev->statesem, 0, 0); + nxsem_set_protocol(&dev->statesem, SEM_PRIO_NONE); + + /* Register the FOC character driver */ + + ret = register_driver(path, &g_foc_fops, 0444, dev); + if (ret < 0) + { + nxsem_destroy(&dev->closesem); + set_errno(ret); + ret = ERROR; + goto errout; + } + + /* Increase device counter */ + + g_devno_cntr += 1; + +errout: + return ret; +} diff --git a/include/nuttx/fs/ioctl.h b/include/nuttx/fs/ioctl.h index bb5a70ae4b8..bceba81d1a5 100644 --- a/include/nuttx/fs/ioctl.h +++ b/include/nuttx/fs/ioctl.h @@ -86,6 +86,7 @@ #define _RCIOCBASE (0x2e00) /* Remote Control device ioctl commands */ #define _HIMEMBASE (0x2f00) /* Himem device ioctl commands*/ #define _EFUSEBASE (0x3000) /* Efuse device ioctl commands*/ +#define _MTRIOBASE (0x3100) /* Motor device ioctl commands*/ #define _WLIOCBASE (0x8b00) /* Wireless modules ioctl network commands */ /* boardctl() commands share the same number space */ @@ -541,6 +542,11 @@ #define _EFUSEIOCVALID(c) (_IOC_TYPE(c) == _EFUSEBASE) #define _EFUSEIOC(nr) _IOC(_EFUSEBASE, nr) +/* Motor drivers ************************************************************/ + +#define _MTRIOCVALID(c) (_IOC_TYPE(c) == _MTRIOBASE) +#define _MTRIOC(nr) _IOC(_MTRIOBASE, nr) + /* Wireless driver network ioctl definitions ********************************/ /* (see nuttx/include/wireless/wireless.h */ diff --git a/include/nuttx/motor/foc/foc.h b/include/nuttx/motor/foc/foc.h new file mode 100644 index 00000000000..a1f4a07ece6 --- /dev/null +++ b/include/nuttx/motor/foc/foc.h @@ -0,0 +1,158 @@ +/**************************************************************************** + * include/nuttx/motor/foc/foc.h + * + * 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. + * + ****************************************************************************/ + +#ifndef __INCLUDE_NUTTX_MOTOR_FOC_FOC_H +#define __INCLUDE_NUTTX_MOTOR_FOC_FOC_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define FOCDUTY_FROM_FLOAT(d) (ftob16(d)) +#define FOCDUTY_FROM_FIXED16(d) (d) + +#define FOCDUTY_TO_FLOAT(d) (b16tof(d)) +#define FOCDUTY_TO_FIXED16(d) (d) + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* FOC device fault code */ + +enum foc_fault_e +{ + FOC_FAULT_NONE = (0), /* No fault */ + FOC_FAULT_TIMEOUT = (1 << 1), /* Timeout fault */ + FOC_FAULT_ARCH = (1 << 2), /* Arch-specific fault */ + FOC_FAULT_BOARD = (1 << 3), /* Board-specific fault */ +}; + +/* Phase current as signed 32-bit integer */ + +typedef int32_t foc_current_t; + +/* Phase duty cycle as unsigned fixed16. + * We use range [0.0 to 1.0] so this gives us a 16-bit resolution. + */ + +typedef ub16_t foc_duty_t; + +/* FOC device configuration */ + +struct foc_cfg_s +{ + uint32_t pwm_freq; /* FOC PWM frequency */ + uint32_t notifier_freq; /* FOC notifier frequency */ +}; + +/* Output data from the FOC device */ + +struct foc_state_s +{ + uint8_t fault; /* Fault state */ + foc_current_t curr[CONFIG_MOTOR_FOC_PHASES]; /* Phase current feedback */ +}; + +/* Input data to the FOC device */ + +struct foc_params_s +{ + foc_duty_t duty[CONFIG_MOTOR_FOC_PHASES]; /* PWM duty cycle for phases */ +}; + +/* Hardware specific configuration */ + +struct foc_hw_config_s +{ + uint32_t pwm_dt_ns; /* PWM dead-time in nano seconds */ + foc_duty_t pwm_max; /* Maximum PWM duty cycle */ +}; + +/* FOC driver info */ + +struct foc_info_s +{ + struct foc_hw_config_s hw_cfg; /* Hardware specific configuration */ +}; + +/* FOC device upper-half */ + +struct foc_lower_s; +struct foc_typespec_s; +struct foc_dev_s +{ + /* Fields managed by common upper-half FOC logic **************************/ + + uint8_t devno; /* FOC device instance number */ + uint8_t ocount; /* The number of times the device + * has been opened + */ + sem_t closesem; /* Locks out new opens while close + * is in progress + */ + sem_t statesem; /* Notifier semaphore */ + + /* Fields provided by lower-half foc logic ********************************/ + + FAR struct foc_lower_s *lower; /* Reference to the FOC lower-half */ + + /* FOC device specific data ***********************************************/ + + struct foc_info_s info; /* Device info */ + struct foc_cfg_s cfg; /* FOC common configuration */ + + /* FOC device input/output data *******************************************/ + + struct foc_state_s state; /* FOC device state */ +}; + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +int foc_register(FAR const char *path, FAR struct foc_dev_s *dev); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_NUTTX_MOTOR_FOC_FOC_H */ diff --git a/include/nuttx/motor/foc/foc_lower.h b/include/nuttx/motor/foc/foc_lower.h new file mode 100644 index 00000000000..b154e3c5043 --- /dev/null +++ b/include/nuttx/motor/foc/foc_lower.h @@ -0,0 +1,144 @@ +/**************************************************************************** + * include/nuttx/motor/foc/foc_lower.h + * + * 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. + * + ****************************************************************************/ + +#ifndef __INCLUDE_NUTTX_MOTOR_FOC_FOC_LOWER_H +#define __INCLUDE_NUTTX_MOTOR_FOC_FOC_LOWER_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Only for kernel side */ + +#ifndef __KERNEL__ +# error +#endif + +/* Helper macros */ + +#define FOC_OPS_CONFIGURE(d, c) (d)->lower->ops->configure(d, c) +#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_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) +#define FOC_OPS_FAULT_CLEAR(d) (d)->lower->ops->fault_clear(d) +#ifdef CONFIG_MOTOR_FOC_TRACE +# define FOC_OPS_TRACE(d, t, s) (d)->lower->ops->trace(d, t, s) +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +#ifdef CONFIG_MOTOR_FOC_TRACE +/* FOC trace type */ + +enum foc_trace_type_e +{ + FOC_TRACE_NONE = 0, /* Not used */ + FOC_TRACE_PARAMS = 1, /* In foc_params_set() */ + FOC_TRACE_STATE = 2, /* In foc_state_get() */ + FOC_TRACE_NOTIFIER = 3, /* In foc_notifier() */ + FOC_TRACE_LOWER = 4 /* Reserved for lower-half code */ +}; +#endif + +/* Upper-half FOC callbacks */ + +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). + */ + + CODE int (*notifier)(FAR struct foc_dev_s *dev, + FAR foc_current_t *current); +}; + +/* Lower-half FOC operations */ + +struct foc_lower_ops_s +{ + /* Lower-half configuration */ + + CODE int (*configure)(FAR struct foc_dev_s *dev, + FAR struct foc_cfg_s *cfg); + + /* Lower-half setup */ + + CODE int (*setup)(FAR struct foc_dev_s *dev); + + /* Lower-half shutdwon */ + + CODE int (*shutdown)(FAR struct foc_dev_s *dev); + + /* Set the PWM duty cycles */ + + CODE int (*pwm_duty_set)(FAR struct foc_dev_s *dev, + FAR foc_duty_t *duty); + + /* Lower-half start/stop */ + + CODE int (*start)(FAR struct foc_dev_s *dev, bool state); + + /* Lower-half IOCTL */ + + CODE int (*ioctl)(FAR struct foc_dev_s *dev, int cmd, + unsigned long arg); + + /* Bind the upper-half driver with the lower-half logic */ + + CODE int (*bind)(FAR struct foc_dev_s *dev, + FAR struct foc_callbacks_s *cb); + + /* Lower-half fault clear */ + + CODE int (*fault_clear)(FAR struct foc_dev_s *dev); + +#ifdef CONFIG_MOTOR_FOC_TRACE + /* FOC trace */ + + CODE void (*trace)(FAR struct foc_dev_s *dev, int type, bool state); +#endif +}; + +/* Lower-half FOC data - must be provided by lower-half implementation */ + +struct foc_lower_s +{ + FAR struct foc_lower_ops_s *ops; /* The FOC lower-half operations */ + FAR void *data; /* The FOC lower-half data */ +}; + +#endif /* __INCLUDE_NUTTX_MOTOR_FOC_FOC_LOWER_H */ diff --git a/include/nuttx/motor/motor_ioctl.h b/include/nuttx/motor/motor_ioctl.h new file mode 100644 index 00000000000..725c51c55e6 --- /dev/null +++ b/include/nuttx/motor/motor_ioctl.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * include/nuttx/motor/motor_ioctl.h + * NuttX Motor-Related IOCTLs definitions + * + * 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. + * + ****************************************************************************/ + +#ifndef __INCLUDE_NUTTX_MOTOR_MOTOR_IOCTL_H +#define __INCLUDE_NUTTX_MOTOR_MOTOR_IOCTL_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* All foc-related IOCTL commands must be defined in this header file + * in order to assure that every IOCTL command is unique and will not be + * aliased. + */ + +#define MTRIOC_START _MTRIOC(1) +#define MTRIOC_STOP _MTRIOC(2) +#define MTRIOC_GET_STATE _MTRIOC(3) +#define MTRIOC_CLEAR_FAULT _MTRIOC(4) +#define MTRIOC_SET_PARAMS _MTRIOC(5) +#define MTRIOC_SET_CONFIG _MTRIOC(6) +#define MTRIOC_GET_INFO _MTRIOC(7) + +#endif /* __INCLUDE_NUTTX_MOTOR_MOTOR_IOCTL_H */