diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig index 1e5aa2a71a..94d29122d7 100644 --- a/drivers/power/Kconfig +++ b/drivers/power/Kconfig @@ -276,6 +276,56 @@ config SMPS_HAVE_EFFICIENCY endif +config DRIVERS_MOTOR + bool "Motor driver" + default n + ---help--- + Enables building of a motor upper half driver. + +if DRIVERS_MOTOR + +config MOTOR_HAVE_POSITION + bool "Have position control" + default n + +config MOTOR_HAVE_DIRECTION + bool "Have direction control" + default n + +config MOTOR_HAVE_SPEED + bool "Have speed control" + default n + +config MOTOR_HAVE_TORQUE + bool "Have torque control (rotary motors)" + default n + +config MOTOR_HAVE_FORCE + bool "Have force control (linear motors)" + default n + +config MOTOR_HAVE_ACCELERATION + bool "Have acceleration control" + default n + +config MOTOR_HAVE_DECELERATION + bool "Have deceleration control" + default n + +config MOTOR_HAVE_INPUT_VOLTAGE + bool "Have input voltage protection" + default n + +config MOTOR_HAVE_INPUT_CURRENT + bool "Have input current protection" + default n + +config MOTOR_HAVE_INPUT_POWER + bool "Have input power protection" + default n + +endif + menuconfig POWER bool "Power Management Support" default n diff --git a/drivers/power/Make.defs b/drivers/power/Make.defs index 2b97dc22e4..f4fe4c1a6a 100644 --- a/drivers/power/Make.defs +++ b/drivers/power/Make.defs @@ -76,6 +76,18 @@ POWER_CFLAGS := ${shell $(INCDIR) $(INCDIROPT) "$(CC)" $(TOPDIR)$(DELIM)drivers$ endif +# Add motor driver + +ifeq ($(CONFIG_DRIVERS_MOTOR),y) + +CSRCS += motor.c + +POWER_DEPPATH := --dep-path power +POWER_VPATH := :power +POWER_CFLAGS := ${shell $(INCDIR) $(INCDIROPT) "$(CC)" $(TOPDIR)$(DELIM)drivers$(DELIM)power} + +endif + # Add battery charger drivers ifeq ($(CONFIG_BATTERY_CHARGER),y) diff --git a/drivers/power/motor.c b/drivers/power/motor.c new file mode 100644 index 0000000000..d050b85468 --- /dev/null +++ b/drivers/power/motor.c @@ -0,0 +1,558 @@ +/**************************************************************************** + * drivers/power/motor.c + * Upper-half, character driver for motor control + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Mateusz Szafoni + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int motor_open(FAR struct file *filep); +static int motor_close(FAR struct file *filep); +static ssize_t motor_read(FAR struct file *filep, FAR char *buffer, + size_t buflen); +static ssize_t motor_write(FAR struct file *filep, FAR const char *buffer, + size_t buflen); +static int motor_ioctl(FAR struct file *filep, int cmd, unsigned long arg); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static const struct file_operations motor_fops = +{ + motor_open, /* open */ + motor_close, /* close */ + motor_read, /* read */ + motor_write, /* write */ + NULL, /* seek */ + motor_ioctl /* ioctl */ +#ifndef CONFIG_DISABLE_POLL + , NULL /* poll */ +#endif +#ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS + , NULL /* unlink */ +#endif +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: motor_open + * + * Description: + * This function is called whenever the motor device is opened. + * + ****************************************************************************/ + +static int motor_open(FAR struct file *filep) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct motor_dev_s *dev = inode->i_private; + uint8_t tmp; + int ret; + + /* 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 hardware initialization. */ + + irqstate_t flags = enter_critical_section(); + ret = dev->ops->setup(dev); + if (ret == OK) + { + /* Save the new open count on success */ + + dev->ocount = tmp; + } + + leave_critical_section(flags); + } + } + + nxsem_post(&dev->closesem); + } + + return OK; +} + +/**************************************************************************** + * Name: motor_close + * + * Description: + * This routine is called when the motor device is closed. + * + ****************************************************************************/ + +static int motor_close(FAR struct file *filep) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct motor_dev_s *dev = inode->i_private; + irqstate_t flags; + int ret; + + 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; + + /* Free the IRQ and disable the motor device */ + + flags = enter_critical_section(); /* Disable interrupts */ + dev->ops->shutdown(dev); /* Disable the motor */ + leave_critical_section(flags); + + nxsem_post(&dev->closesem); + } + } + + return ret; +} + +/**************************************************************************** + * Name: motor_read + ****************************************************************************/ + +static ssize_t motor_read(FAR struct file *filep, FAR char *buffer, size_t buflen) +{ + return 1; +} + +/**************************************************************************** + * Name: motor_write + ****************************************************************************/ + +static ssize_t motor_write(FAR struct file *filep, FAR const char *buffer, size_t buflen) +{ + return 1; +} + +/**************************************************************************** + * Name: motor_ioctl +****************************************************************************/ + +static int motor_ioctl(FAR struct file *filep, int cmd, unsigned long arg) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct motor_dev_s *dev = inode->i_private; + FAR struct motor_s *motor = (FAR struct motor_s *)dev->priv; + int ret; + + switch (cmd) + { + case PWRIOC_START: + { + /* Allow motor start only when some limits available + * and strucutre is locked. + */ + + if ((motor->limits.lock == false) || + ( +#ifdef CONFIG_MOTOR_HAVE_POSITION + motor->limits.position <= 0.0 && +#endif +#ifdef CONFIG_MOTOR_HAVE_SPEED + motor->limits.speed <= 0.0 && +#endif +#ifdef CONFIG_MOTOR_HAVE_TORQUE + motor->limits.torque <= 0.0 && +#endif +#ifdef CONFIG_MOTOR_HAVE_FORCE + motor->limits.force <= 0.0 && +#endif +#ifdef CONFIG_MOTOR_HAVE_INPUT_VOLTAGE + motor->limits.v_in <= 0.0 && +#endif +#ifdef CONFIG_MOTOR_HAVE_INPUT_CURRENT + motor->limits.i_in <= 0.0 && +#endif +#ifdef CONFIG_MOTOR_HAVE_INPUT_POWER + motor->limits.p_in <= 0.0 && +#endif + 1)) + { + pwrerr("ERROR: motor limits data must be set" + " and locked before motor start\n"); + + ret = -EPERM; + goto errout; + } + + /* Check motor mode */ + + if (motor->opmode == MOTOR_OPMODE_INIT) + { + pwrerr("ERROR: motor operation mode not specified\n"); + + ret = -EPERM; + goto errout; + } + + /* REVISIT: do we need some parameters assertions here ? */ + + /* Finally, call start from lower-half driver */ + + ret = dev->ops->start(dev); + if (ret != OK) + { + pwrerr("ERROR: PWRIOC_START failed %d\n", ret); + } + break; + } + + case PWRIOC_STOP: + { + /* Call stop from lower-half driver */ + + ret = dev->ops->stop(dev); + if (ret != OK) + { + pwrerr("ERROR: PWRIOC_STOP failed %d\n", ret); + } + break; + } + + case PWRIOC_SET_MODE: + { + uint8_t mode = ((uint8_t)arg); + + ret = dev->ops->mode_set(dev, mode); + if (ret != OK) + { + pwrerr("ERROR: PWRIOC_SET_MODE failed %d\n", ret); + } + break; + } + + case PWRIOC_SET_LIMITS: + { + FAR struct motor_limits_s *limits = + (FAR struct motor_limits_s *)((uintptr_t)arg); + + if (motor->limits.lock == true) + { + pwrerr("ERROR: motor limits locked!\n"); + + ret = -EPERM; + goto errout; + } + + /* NOTE: this call must set the motor_limits_s structure */ + + ret = dev->ops->limits_set(dev, limits); + if (ret != OK) + { + pwrerr("ERROR: PWRIOC_SET_LIMITS failed %d\n", ret); + } + break; + } + + case PWRIOC_GET_STATE: + { + FAR struct motor_state_s *state = + (FAR struct motor_state_s *)((uintptr_t)arg); + + ret = dev->ops->state_get(dev, state); + if (ret != OK) + { + pwrerr("ERROR: PWRIOC_GET_STATE failed %d\n", ret); + } + break; + } + + case PWRIOC_SET_FAULT: + { + uint8_t fault = ((uint8_t)arg); + + ret = dev->ops->fault_set(dev, fault); + if (ret != OK) + { + pwrerr("ERROR: PWRIOC_SET_FAULT failed %d\n", ret); + } + break; + } + + case PWRIOC_GET_FAULT: + { + uint8_t *fault = ((uint8_t*)arg); + + ret = dev->ops->fault_get(dev, fault); + if (ret != OK) + { + pwrerr("ERROR: PWRIOC_GET_FAULT failed %d\n", ret); + } + break; + } + + case PWRIOC_CLEAN_FAULT: + { + uint8_t fault = ((uint8_t)arg); + + ret = dev->ops->fault_clean(dev, fault); + if (ret != OK) + { + pwrerr("ERROR: PWRIOC_CLEAN_FAULT failed %d\n", ret); + } + break; + } + + case PWRIOC_SET_PARAMS: + { + FAR struct motor_params_s *params = + (FAR struct motor_params_s *)((uintptr_t)arg); + + if (motor->param.lock == true) + { + pwrerr("ERROR: motor params locked!\n"); + + ret = -EPERM; + goto errout; + } + + if ((motor->limits.lock == false) || + ( +#ifdef CONFIG_MOTOR_HAVE_POSITION + motor->limits.position <= 0.0 && +#endif +#ifdef CONFIG_MOTOR_HAVE_SPEED + motor->limits.speed <= 0.0 && +#endif +#ifdef CONFIG_MOTOR_HAVE_TORQUE + motor->limits.torque <= 0.0 && +#endif +#ifdef CONFIG_MOTOR_HAVE_FORCE + motor->limits.force <= 0.0 && +#endif +#ifdef CONFIG_MOTOR_HAVE_INPUT_VOLTAGE + motor->limits.v_in <= 0.0 && +#endif +#ifdef CONFIG_MOTOR_HAVE_INPUT_CURRENT + motor->limits.i_in <= 0.0 && +#endif +#ifdef CONFIG_MOTOR_HAVE_INPUT_POWER + motor->limits.p_in <= 0.0 && +#endif + 1)) + { + pwrerr("ERROR: limits must be set prior to params!\n"); + + ret = -EPERM; + goto errout; + } + +#ifdef CONFIG_MOTOR_HAVE_POSITION + /* Check position configuration */ + + if (params->position < 0.0 || + params->position > motor->limits.position) + { + pwrerr("ERROR: params->position > limits.position: %.2f > %.2f\n", + params->position, motor->limits.position); + + ret = -EPERM; + goto errout; + } +#endif + +#ifdef CONFIG_MOTOR_HAVE_SPEED + /* Check speed configuration */ + + if (motor->limits.speed > 0.0 && params->speed > motor->limits.speed) + { + pwrerr("ERROR: params->speed > limits.speed: %.2f > %.2f\n", + params->speed, motor->limits.speed); + + ret = -EPERM; + goto errout; + } +#endif + +#ifdef CONFIG_MOTOR_HAVE_TORQUE + /* Check torque configuration */ + + if (motor->limits.torque > 0.0 && params->torque > motor->limits.torque) + { + pwrerr("ERROR: params->torque > limits.torque: %.2f > %.2f\n", + params->torque, motor->limits.torque); + + ret = -EPERM; + goto errout; + } +#endif + +#ifdef CONFIG_MOTOR_HAVE_FORCE + /* Check force configuration */ + + if (motor->limits.force > 0.0 && params->force > motor->limits.force) + { + pwrerr("ERROR: params->force > limits.force: %.2f > %.2f\n", + params->force, motor->limits.force); + + ret = -EPERM; + goto errout; + } +#endif + + ret = dev->ops->params_set(dev, params); + if (ret != OK) + { + pwrerr("ERROR: PWRIOC_SET_PARAMS failed %d\n", ret); + } + break; + } + + default: + { + pwrinfo("Forwarding unrecognized cmd: %d arg: %ld\n", cmd, arg); + ret = dev->ops->ioctl(dev, cmd, arg); + break; + } + } + +errout: + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: motor_register + ****************************************************************************/ + +int motor_register(FAR const char *path, FAR struct motor_dev_s *dev, FAR void *lower) +{ + int ret; + + DEBUGASSERT(path != NULL && dev != NULL && lower != NULL); + DEBUGASSERT(dev->ops != NULL); + + /* For safety reason, when some necessary low-level logic is not provided, + * system should fail before low-level hardware initialization, so: + * - all ops are checked here, before character driver registration + * - all ops must be provided, even if not used + */ + + DEBUGASSERT(dev->ops->setup != NULL); + DEBUGASSERT(dev->ops->shutdown != NULL); + DEBUGASSERT(dev->ops->stop != NULL); + DEBUGASSERT(dev->ops->start != NULL); + DEBUGASSERT(dev->ops->params_set != NULL); + DEBUGASSERT(dev->ops->mode_set != NULL); + DEBUGASSERT(dev->ops->limits_set != NULL); + DEBUGASSERT(dev->ops->fault_set != NULL); + DEBUGASSERT(dev->ops->state_get != NULL); + DEBUGASSERT(dev->ops->fault_get != NULL); + DEBUGASSERT(dev->ops->fault_clean != NULL); + DEBUGASSERT(dev->ops->ioctl != NULL); + + /* Initialize the motor device structure */ + + dev->ocount = 0; + + /* Initialize semaphores */ + + nxsem_init(&dev->closesem, 0, 1); + + /* Connect motor driver with lower level interface */ + + dev->lower = lower; + + /* Register the motor character driver */ + + ret = register_driver(path, &motor_fops, 0444, dev); + if (ret < 0) + { + nxsem_destroy(&dev->closesem); + } + + return ret; +} diff --git a/include/nuttx/power/motor.h b/include/nuttx/power/motor.h new file mode 100644 index 0000000000..30b8f8e628 --- /dev/null +++ b/include/nuttx/power/motor.h @@ -0,0 +1,341 @@ +/**************************************************************************** + * include/nuttx/power/motor.h + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Author: Mateusz Szafoni + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef __INCLUDE_NUTTX_DRIVERS_POWER_MOTOR_H +#define __INCLUDE_NUTTX_DRIVERS_POWER_MOTOR_H + +/* + * The motor driver is split into two parts: + * + * 1) An "upper half", generic driver that provides the common motor interface + * to application level code, and + * 2) A "lower half", platform-specific driver that implements the low-level + * functionality eg.: + * - timer controls to implement the PWM signals, + * - analog peripherals configuration such as ADC, DAC and comparators, + * - control algorithm for motor driver (eg. FOC control for BLDC) + * + * This 'upper-half' driver has been designed with flexibility in mind + * to support all kinds of electric motors and their applications. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include + +#ifdef CONFIG_DRIVERS_MOTOR + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* Motor driver operation modes */ + +enum motor_opmode_e +{ + MOTOR_OPMODE_INIT = 0, /* Initial mode */ + MOTOR_OPMODE_POSITION = 1, /* Position control mode */ + MOTOR_OPMODE_SPEED = 2, /* Speed control mode */ + MOTOR_OPMODE_TORQUE = 3, /* Torque control mode */ + MOTOR_OPMODE_FORCE = 4 /* Force control mode */ +}; + +/* Motor driver state */ + +enum motor_state_e +{ + MOTOR_STATE_INIT = 0, /* Initial state */ + MOTOR_STATE_IDLE = 1, /* IDLE state */ + MOTOR_STATE_RUN = 2, /* Run state */ + MOTOR_STATE_FAULT = 3, /* Fault state */ + MOTOR_STATE_CRITICAL = 4 /* Critical Fault state */ +}; + +/* Motor driver fault type */ + +enum motor_fault_e +{ + MOTOR_FAULT_OVERCURRENT = (1 << 0), /* Over-current Fault */ + MOTOR_FAULT_OVERVOLTAGE = (1 << 1), /* Over-voltage Fault */ + MOTOR_FAULT_OVERPOWER = (1 << 2), /* Over-power Fault */ + MOTOR_FAULT_OVERTEMP = (1 << 3), /* Over-temperature Fault */ + MOTOR_FAULT_LOCKED = (1 << 4), /* Motor locked Fault */ + MOTOR_FAULT_INVAL_PARAM = (1 << 5), /* Invalid parameter Fault */ + MOTOR_FAULT_OTHER = (1 << 6) /* Other Fault */ +}; + +/* This structure contains feedback data from motor driver */ + +struct motor_feedback_s +{ +#ifdef CONFIG_MOTOR_HAVE_POSITION + float position; /* Current motor position */ +#endif +#ifdef CONFIG_MOTOR_HAVE_SPEED + float speed; /* Current motor speed */ +#endif +#ifdef CONFIG_MOTOR_HAVE_TORQUE + float torque; /* Current motor torque (rotary motor) */ +#endif +#ifdef CONFIG_MOTOR_HAVE_FORCE + float force; /* Current motor force (linear motor) */ +#endif +#ifdef CONFIG_MOTOR_HAVE_INPUT_VOLTAGE + float v_in; /* Current input voltage */ +#endif +#ifdef CONFIG_MOTOR_HAVE_INPUT_CURRENT + float i_in; /* Current input current */ +#endif +#ifdef CONFIG_MOTOR_HAVE_INPUT_POWER + float p_in; /* Current input power */ +#endif +}; + +/* This structure describes motor driver state */ + +struct motor_state_s +{ + uint8_t state; /* Motor state */ + uint8_t fault; /* Motor faults state */ + struct motor_feedback_s fb; /* Feedback from motor */ +}; + +/* Motor absolute limits. Exceeding this limits should cause critical error + * This structure must be configured before motor params_set call. + * When limit is set to 0 then it is ignored. + */ + +struct motor_limits_s +{ + bool lock; /* This bit must be set after + * limits configuration. + */ +#ifdef CONFIG_MOTOR_HAVE_POSITION + float position; /* Maximum motor position */ +#endif +#ifdef CONFIG_MOTOR_HAVE_SPEED + float speed; /* Maximum motor speed */ +#endif +#ifdef CONFIG_MOTOR_HAVE_TORQUE + float torque; /* Maximum motor torque (rotary motor) */ +#endif +#ifdef CONFIG_MOTOR_HAVE_FORCE + float force; /* Maximum motor force (linear motor) */ +#endif +#ifdef CONFIG_MOTOR_HAVE_ACCELERATION + float acceleration; /* Maximum motor acceleration */ +#endif +#ifdef CONFIG_MOTOR_HAVE_DECELERATION + float deceleration; /* Maximum motor decelaration */ +#endif +#ifdef CONFIG_MOTOR_HAVE_INPUT_VOLTAGE + float v_in; /* Maximum input voltage */ +#endif +#ifdef CONFIG_MOTOR_HAVE_INPUT_CURRENT + float i_in; /* Maximum input current */ +#endif +#ifdef CONFIG_MOTOR_HAVE_INPUT_POWER + float p_in; /* Maximum input power */ +#endif +}; + +/* Motor parameters. + * NOTE: All parameters require not negative value. + */ + +struct motor_params_s +{ + bool lock; /* Lock this structure. Set this bit + * if there is no need to change motor + * parameter during run-time. + */ +#ifdef CONFIG_MOTOR_HAVE_DIRECTION + bool direction; /* Motor movement direction. We do not + * support negative values for parameters, + * so this flag can be used to allow movement + * in the positive and negative direction in + * a given coordinate system. + */ +#endif +#ifdef CONFIG_MOTOR_HAVE_POSITION + float position; /* Motor position */ +#endif +#ifdef CONFIG_MOTOR_HAVE_SPEED + float speed; /* Motor speed */ +#endif +#ifdef CONFIG_MOTOR_HAVE_TORQUE + float torque; /* Motor torque (rotary motor)*/ +#endif +#ifdef CONFIG_MOTOR_HAVE_FORCE + float force; /* Motor force (linear motor) */ +#endif +#ifdef CONFIG_MOTOR_HAVE_ACCELERATION + float acceleration; /* Motor acceleration */ +#endif +#ifdef CONFIG_MOTOR_HAVE_DECELERATION + float deceleartion; /* Motor deceleration */ +#endif +}; + +/* Motor private data structure */ + +struct motor_s +{ + uint8_t opmode; /* Motor operation mode */ + uint8_t opflags; /* Motor operation flags */ + struct motor_limits_s limits; /* Motor absolute limits */ + struct motor_params_s param; /* Motor settings */ + struct motor_state_s state; /* Motor state */ + FAR void *priv; /* Private data */ +}; + +/* Motor operations used to call from the upper-half, generic motor driver + * into lower-half, platform-specific logic. + */ + +struct motor_dev_s; +struct motor_ops_s +{ + /* Configure motor */ + + CODE int (*setup)(FAR struct motor_dev_s *dev); + + /* Disable motor */ + + CODE int (*shutdown)(FAR struct motor_dev_s *dev); + + /* Stop motor */ + + CODE int (*stop)(FAR struct motor_dev_s *dev); + + /* Start motor */ + + CODE int (*start)(FAR struct motor_dev_s *dev); + + /* Set motor parameters */ + + CODE int (*params_set)(FAR struct motor_dev_s *dev, + FAR struct motor_params_s *param); + + /* Set motor operation mode */ + + CODE int (*mode_set)(FAR struct motor_dev_s *dev, uint8_t mode); + + /* Set motor limts */ + + CODE int (*limits_set)(FAR struct motor_dev_s *dev, + FAR struct motor_limits_s *limits); + + /* Set motor fault */ + + CODE int (*fault_set)(FAR struct motor_dev_s *dev, uint8_t fault); + + /* Get motor state */ + + CODE int (*state_get)(FAR struct motor_dev_s *dev, + FAR struct motor_state_s *state); + + /* Get current fault state */ + + CODE int (*fault_get)(FAR struct motor_dev_s *dev, FAR uint8_t *fault); + + /* Clean fault state */ + + CODE int (*fault_clean)(FAR struct motor_dev_s *dev, uint8_t fault); + + /* Lower-half logic may support platform-specific ioctl commands */ + + CODE int (*ioctl)(FAR struct motor_dev_s *dev, int cmd, unsigned long arg); +}; + +/* */ + +struct motor_dev_s +{ + /* Fields managed by common upper half motor logic */ + + uint8_t ocount; /* The number of times the device + * has been opened + */ + sem_t closesem; /* Locks out new opens while close + * is in progress + */ + + /* Fields provided by lower half motor logic */ + + FAR const struct motor_ops_s *ops; /* Arch-specific operations */ + FAR void *priv; /* Reference to motor private data */ + FAR void *lower; /* Reference to lower level drivers */ +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Name: motor_register + ****************************************************************************/ + +int motor_register(FAR const char *path, FAR struct motor_dev_s *dev, + FAR void *lower); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* CONFIG_DRIVERS_MOTOR */ +#endif /* __INCLUDE_NUTTX_DRIVERS_POWER_MOTOR_H */