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

drivers: add generic upper-half driver for Field Oriented Control (FOC)

Browse Source
This commit is contained in:
raiden00pl 2021-03-29 12:18:07 +02:00 committed by Alan Carvalho de Assis
parent 3a97f51a8d
commit e4c27dfdd6
13 changed files with 1426 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
Documentation/components/drivers/character/foc.rst Normal file
View File

@ -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.

1
Documentation/components/drivers/character/index.rst
View File

@ -64,4 +64,5 @@ Character device drivers have these properties:
watchdog.rst
keypad.rst
note.rst
foc.rst

1
drivers/Kconfig
View File

@ -130,3 +130,4 @@ source drivers/syslog/Kconfig
source drivers/platform/Kconfig
source drivers/rf/Kconfig
source drivers/rc/Kconfig
source drivers/motor/Kconfig

1
drivers/Makefile
View File

@ -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

14
drivers/motor/Kconfig Normal file
View File

@ -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

35
drivers/motor/Make.defs Normal file
View File

@ -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)

31
drivers/motor/foc/Kconfig Normal file
View File

@ -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

29
drivers/motor/foc/Make.defs Normal file
View File

@ -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}

909
drivers/motor/foc/foc_dev.c Normal file
View File

@ -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 <nuttx/config.h>
#include <stdio.h>
#include <fcntl.h>
#include <debug.h>
#include <errno.h>
#include <assert.h>
#include <nuttx/motor/motor_ioctl.h>
#include <nuttx/motor/foc/foc_lower.h>
/****************************************************************************
* 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;
}

6
include/nuttx/fs/ioctl.h
View File

@ -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 */

158
include/nuttx/motor/foc/foc.h Normal file
View File

@ -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 <nuttx/config.h>
#include <nuttx/compiler.h>
#include <stdbool.h>
#include <semaphore.h>
#include <fixedmath.h>
/****************************************************************************
* 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 */

144
include/nuttx/motor/foc/foc_lower.h Normal file
View File

@ -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 <nuttx/config.h>
#include <nuttx/motor/foc/foc.h>
/****************************************************************************
* 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 */

49
include/nuttx/motor/motor_ioctl.h Normal file
View File

@ -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 <nuttx/config.h>
#include <nuttx/fs/ioctl.h>
/****************************************************************************
* 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 */