You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by xi...@apache.org on 2021/04/10 05:40:50 UTC
[incubator-nuttx-apps] branch master updated: Add FOC motor
controller example
This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx-apps.git
The following commit(s) were added to refs/heads/master by this push:
new 864a61a Add FOC motor controller example
864a61a is described below
commit 864a61a4314d0c053d727e4b2e502ed03502bc83
Author: raiden00pl <ra...@railab.me>
AuthorDate: Wed Mar 3 19:48:19 2021 +0100
Add FOC motor controller example
For now, only open-loop velocity control is supported.
---
examples/README.md | 5 +
examples/foc/Kconfig | 236 +++++++++
examples/foc/Make.defs | 24 +
examples/foc/Makefile | 49 ++
examples/foc/README.md | 83 +++
examples/foc/foc_adc.h | 119 +++++
examples/foc/foc_cfg.h | 82 +++
examples/foc/foc_debug.h | 60 +++
examples/foc/foc_device.c | 82 +++
examples/foc/foc_device.h | 52 ++
examples/foc/foc_fixed16_thr.c | 1018 ++++++++++++++++++++++++++++++++++++
examples/foc/foc_float_thr.c | 1017 ++++++++++++++++++++++++++++++++++++
examples/foc/foc_main.c | 1118 ++++++++++++++++++++++++++++++++++++++++
examples/foc/foc_mq.c | 118 +++++
examples/foc/foc_mq.h | 83 +++
examples/foc/foc_thr.h | 101 ++++
16 files changed, 4247 insertions(+)
diff --git a/examples/README.md b/examples/README.md
index 9fb6cb5..2de025c 100644
--- a/examples/README.md
+++ b/examples/README.md
@@ -361,6 +361,11 @@ Dependencies:
- `CONFIG_BUILD_PROTECTED=n` and `CONFIG_BUILD_KERNEL=n` – This test uses
internal OS interfaces and so is not available in the NUTTX kernel builds.
+## `foc` FOC motor controller
+
+A FOC motor controller based on the NuttX FOC driver and the NuttX FOC library.
+See `apps/foc/README.md` for more information.
+
## `flowc` Serial Hardware Flow Control
A simple test of serial hardware flow control.
diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
new file mode 100644
index 0000000..7154558
--- /dev/null
+++ b/examples/foc/Kconfig
@@ -0,0 +1,236 @@
+#
+# For a description of the syntax of this configuration file,
+# see the file kconfig-language.txt in the NuttX tools repository.
+#
+
+menuconfig EXAMPLES_FOC
+ tristate "FOC motor controller example"
+ depends on MOTOR_FOC
+ depends on INDUSTRY_FOC
+ ---help---
+ Enable the FOC motor controller example.
+ At the moment, this example implements a simple open-loop velocity controller.
+
+if EXAMPLES_FOC
+
+config EXAMPLES_FOC_DEVPATH
+ string "FOC device path prefix"
+ default "/dev/foc"
+ ---help---
+ The default path to the FOC device without the device minor number.
+ Default: /dev/foc
+
+config EXAMPLES_FOC_FLOAT_INST
+ int "FOC float instances"
+ depends on INDUSTRY_FOC_FLOAT
+ default 0
+
+config EXAMPLES_FOC_FIXED16_INST
+ int "FOC fixed16 instances"
+ depends on INDUSTRY_FOC_FIXED16
+ default 0
+
+config EXAMPLES_FOC_CONTROL_PRIO
+ int "FOC control thread priority"
+ default 255
+
+config EXAMPLES_FOC_CONTROL_STACKSIZE
+ int "FOC control thread stack size"
+ default 4096
+
+config EXAMPLES_FOC_VERBOSE
+ int "Enable verbose print for app"
+ default 1
+ range 0 2
+
+config EXAMPLES_FOC_PWM_FREQ
+ int "FOC PWM frequency"
+ default 10000
+ ---help---
+ Select the FOC PWM switching frequency
+
+config EXAMPLES_FOC_NOTIFIER_FREQ
+ int "FOC notifier frequency"
+ default EXAMPLES_FOC_PWM_FREQ
+ ---help---
+ Select the FOC notifier frequency
+
+config EXAMPLES_FOC_IPHASE_ADC
+ int "FOC phase current scale [x100000]"
+ default 0
+ ---help---
+ This parameter is used to get real currents from ADC RAW values
+
+config EXAMPLES_FOC_STATE_PRINT_FREQ
+ int "FOC example data printer frequency"
+ default 0
+ depends on INDUSTRY_FOC_HANDLER_PRINT
+ ---help---
+ Set 0 to disable FOC data print
+
+config EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ bool "FOC uses PMSM model"
+ depends on INDUSTRY_FOC_MODEL_PMSM
+ default n
+ ---help---
+ Use PMSM model instead of real hardware
+
+menu "FOC user input"
+
+config EXAMPLES_FOC_HAVE_ADC
+ bool
+ default n
+
+choice
+ prompt "FOC VBUS source"
+ default EXAMPLES_FOC_VBUS_CONST
+
+config EXAMPLES_FOC_VBUS_CONST
+ bool "Use hardcoded constant VBUS value"
+
+config EXAMPLES_FOC_VBUS_ADC
+ bool "Use VBUS provided by ADC interface"
+ depends on ADC
+ select EXAMPLES_FOC_HAVE_ADC
+
+endchoice # FOC VBUS interface
+
+if EXAMPLES_FOC_VBUS_CONST
+
+config EXAMPLES_FOC_VBUS_CONST_VALUE
+ int "FOC VBUS constant value"
+ default 12000
+
+endif # EXAMPLES_FOC_VBUS_CONST
+
+if EXAMPLES_FOC_HAVE_ADC
+
+config EXAMPLES_FOC_ADC_DEVPATH
+ string "FOC ADC interface path"
+ default "/dev/adc0"
+
+config EXAMPLES_FOC_ADC_VREF
+ int "FOC ADC reference votlage [x1000]"
+ default 0
+
+config EXAMPLES_FOC_ADC_MAX
+ int "FOC ADC aux maximum sample value"
+ default 0
+
+endif # EXAMPLES_FOC_HAVE_ADC
+
+if EXAMPLES_FOC_VBUS_ADC
+
+config EXAMPLES_FOC_VBUS_SCALE
+ int "FOC VBUS SCALE scale [x1000]"
+ default 0
+
+endif # EXAMPLES_FOC_VBUS_ADC
+
+choice
+ prompt "FOC velocity source"
+ default EXAMPLES_FOC_VEL_CONST
+
+config EXAMPLES_FOC_VEL_CONST
+ bool "Use hardcoded constant velocity value"
+
+config EXAMPLES_FOC_VEL_ADC
+ bool "Use ADC to control velocity"
+ select EXAMPLES_FOC_HAVE_ADC
+
+endchoice # FOC velocity interface
+
+if EXAMPLES_FOC_VEL_CONST
+
+config EXAMPLES_FOC_VEL_CONST_VALUE
+ int "FOC hardoced velocity value"
+ default 100000
+
+endif # EXAMPLES_FOC_VEL_CONST
+
+if EXAMPLES_FOC_VEL_ADC
+
+config EXAMPLES_FOC_VEL_ADC_MAX
+ int "FOC maximum velocity from ADC [x1000]"
+ default 100000
+
+endif # EXAMPLES_FOC_VEL_ADC
+
+config EXAMPLES_FOC_TIME_DEFAULT
+ int "FOC run time default (sec)"
+ default 10
+
+config EXAMPLES_FOC_STATE_INIT
+ int "FOC motor controller state init"
+ default 1
+ range 1 4
+ ---help---
+ 1 - motor FREE (no current)
+ 2 - motor STOP (active break)
+ 3 - motor moves in CW direction
+ 4 - motor moves in CCW direction
+
+config EXAMPLES_FOC_HAVE_BUTTON
+ bool "FOC button support"
+ default n
+ ---help---
+ The button is used to change the motor controller state
+
+if EXAMPLES_FOC_HAVE_BUTTON
+
+config EXAMPLES_FOC_BUTTON_DEVPATH
+ string "FOC button device path"
+ default "/dev/buttons"
+ depends on INPUT_BUTTONS
+
+endif
+
+endmenu # FOC user input
+
+menu "FOC controller parameters"
+
+config EXAMPLES_FOC_OPMODE
+ int "FOC operation mode"
+ default 2
+ range 1 3
+ ---help---
+ 1 - IDLE mode
+ 2 - voltage open-loop velocity controller (default)
+ 3 - current open-loop velocity controller
+
+config EXAMPLES_FOC_OPENLOOP_Q
+ int "FOC open-loop Vq/Iq setting [x1000]"
+ default 200
+
+config EXAMPLES_FOC_IDQ_KP
+ int "FOC PI controller Kp gain [x1000]"
+ default 0
+ ---help---
+ It is set to 0 by default and must be properly configured by the user!
+ The vaule of Kp and Ki depends on the controlled motor parameters.
+ For more instructions see README.md for this example.
+
+config EXAMPLES_FOC_IDQ_KI
+ int "FOC PI controller Ki gain [x1000]"
+ default 0
+ ---help---
+ It is set to 0 by default and must be properly configured by the user!
+ The vaule of Kp and Ki depends on the controlled motor parameters.
+ For more instructions see README.md for this example.
+
+config EXAMPLES_FOC_RAMP_THR
+ int "FOC velocity ramp threshold [x1000]"
+ default 0
+
+config EXAMPLES_FOC_RAMP_ACC
+ int "FOC velocity ramp acc [x1000]"
+ default 0
+
+config EXAMPLES_FOC_RAMP_DEC
+ int "FOC velocity ramp acc [x1000]"
+ default 0
+
+endmenu # FOC controller parameters
+
+endif # EXAMPLES_FOC
+
diff --git a/examples/foc/Make.defs b/examples/foc/Make.defs
new file mode 100644
index 0000000..70926df
--- /dev/null
+++ b/examples/foc/Make.defs
@@ -0,0 +1,24 @@
+############################################################################
+# apps/examples/foc/Make.defs
+# Adds selected applications to apps/ build
+#
+# 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.
+#
+############################################################################
+
+ifeq ($(CONFIG_EXAMPLES_FOC),y)
+CONFIGURED_APPS += examples/foc
+endif
diff --git a/examples/foc/Makefile b/examples/foc/Makefile
new file mode 100644
index 0000000..de91e8e
--- /dev/null
+++ b/examples/foc/Makefile
@@ -0,0 +1,49 @@
+############################################################################
+# apps/examples/foc/Makefile
+#
+# 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 $(APPDIR)/Make.defs
+
+# FOC built-in application info
+
+PROGNAME = foc
+PRIORITY = SCHED_PRIORITY_DEFAULT
+STACKSIZE = $(CONFIG_DEFAULT_TASK_STACKSIZE)
+MODULE = $(CONFIG_EXAMPLES_FOC)
+
+# FOC motor controller example
+
+MAINSRC = foc_main.c
+
+ASRCS =
+CSRCS = foc_device.c foc_mq.c
+
+# fixed16 support
+
+ifeq ($(CONFIG_INDUSTRY_FOC_FIXED16),y)
+ CSRCS += foc_fixed16_thr.c
+endif
+
+# float32 support
+
+ifeq ($(CONFIG_INDUSTRY_FOC_FLOAT),y)
+ CSRCS += foc_float_thr.c
+endif
+
+include $(APPDIR)/Application.mk
diff --git a/examples/foc/README.md b/examples/foc/README.md
new file mode 100644
index 0000000..e124a7a
--- /dev/null
+++ b/examples/foc/README.md
@@ -0,0 +1,83 @@
+# FOC example
+
+The main purpose of this example is to provide a universal template to
+implement the motor controller based on the kernel-side FOC device and
+the application-side FOC library.
+
+At the moment, this example implements a simple open-loop velocity controller.
+
+# Hardware setup
+
+This example has not yet implemented any mechanism to protect the
+powered device. This means that there is no overtemeprature
+protection, no overcurrent protection and no overvoltage protection.
+
+Make sure that you power the device properly and provide current
+limits on your own so as not to break your hardware.
+
+# Configuration
+
+The FOC PI current controller parameters can be obtained from the given
+equations:
+
+```
+Kp = ccb * Ls;
+pp = Rs / Ls;
+Ki = pp * Kp * T;
+```
+
+where:
+ Kp - PI proportional coefficient
+ Ki - PI integral coefficient
+ Rs - average phase serial resistance
+ Ls - average phase serial inductance
+ pp - pole plant
+ ccb - current control bandwidth
+ T - sampling period
+
+## Sample parameters for some commercially available motors
+
+* Odrive D6374 150KV
+ p = 7
+ Rs = 0.0254 Ohm
+ Ls = 8.73 uH
+ i\_max = ?
+ v\_max = ?
+
+ Example configuration for f\_PWM = 20kHz, f\_notifier = 10kHz, ccb=1000:
+ Kp = 0.0087
+ Ki = 0.0025
+
+* Linix 45ZWN24-40 (PMSM motor dedicated for NXP FRDM-MC-LVMTR kit)
+ p = 2
+ Rs = 0.5 Ohm
+ Ls = 0.400 mH
+ i\_max = 2.34 A
+ v\_max = 24 V
+
+ Example configuration for f\_PWM = 10kHz, f\_notifier = 5kHz, ccb=1000:
+ Kp = 0.4
+ Ki = 0.1
+
+* Bull-Running BR2804-1700 kV (motor provided with the ST P-NUCLEO-IHM07 kit)
+ p = 7
+ Rs = 0.11 Ohm
+ Ls = 0.018 mH
+ i\_max = 1.2A
+ v\_max = 12V
+
+ Example configuration for f\_PWM = 20kHz, f\_notifier = 10kHz, ccb=200:
+ Kp = 0.036
+ Ki = 0.022
+
+* iPower GBM2804H-100T (gimbal motor provided with the ST P-NUCLEO-IHM03 kit)
+ p = 7
+ Rs = 5.29 Ohm
+ Ls = 1.05 mH
+ i\_max = 0.15A
+ v\_max = 12V
+
+ Example configuration for f\_PWM = 10kHz, f\_notifier = 5kHz, ccb=TODO:
+ Kp = TODO
+ Ki = TODO
+
diff --git a/examples/foc/foc_adc.h b/examples/foc/foc_adc.h
new file mode 100644
index 0000000..38cc822
--- /dev/null
+++ b/examples/foc/foc_adc.h
@@ -0,0 +1,119 @@
+/****************************************************************************
+ * apps/examples/foc/foc_adc.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 __EXAMPLES_FOC_FOC_ADC_H
+#define __EXAMPLES_FOC_FOC_ADC_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* VBUS source must be specified */
+
+#if defined(CONFIG_EXAMPLES_FOC_VBUS_CONST) && \
+ defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
+# error
+#endif
+
+/* Velocity source must be specified */
+
+#if defined(CONFIG_EXAMPLES_FOC_VEL_CONST) && \
+ defined(CONFIG_EXAMPLES_FOC_VEL_ADC)
+# error
+#endif
+
+/* VBUS ADC scale factor */
+
+#ifdef CONFIG_EXAMPLES_FOC_VBUS_ADC
+# define VBUS_ADC_SCALE (CONFIG_EXAMPLES_FOC_ADC_VREF * \
+ CONFIG_EXAMPLES_FOC_VBUS_SCALE / \
+ CONFIG_EXAMPLES_FOC_ADC_MAX / \
+ 1000.0f / \
+ 1000.0f)
+#endif
+
+/* Velocity ADC scale factor */
+
+#ifdef CONFIG_EXAMPLES_FOC_VEL_ADC
+# define VEL_ADC_SCALE (1.0f / CONFIG_EXAMPLES_FOC_ADC_MAX)
+#endif
+
+/* If constant velocity is selected, velocity value must be provided */
+
+#ifdef CONFIG_EXAMPLES_FOC_VEL_CONST
+# define VEL_ADC_SCALE (1)
+# if CONFIG_EXAMPLES_FOC_VEL_CONST_VALUE == 0
+# error
+# endif
+#endif
+
+/* If constant VBUS is selected, VBUS value must be provided */
+
+#ifdef CONFIG_EXAMPLES_FOC_VBUS_CONST
+# define VBUS_ADC_SCALE (1)
+# define VBUS_CONST_VALUE (CONFIG_EXAMPLES_FOC_VBUS_CONST_VALUE / 1000.0f)
+# if CONFIG_EXAMPLES_FOC_VBUS_CONST_VALUE == 0
+# error
+# endif
+#endif
+
+/* Additional configuration if ADC support is required */
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
+
+/* AUX ADC samples support */
+
+# if defined(CONFIG_EXAMPLES_FOC_VEL_ADC) && \
+ defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
+# define ADC_SAMPLES (2)
+# elif defined(CONFIG_EXAMPLES_FOC_VEL_ADC) || \
+ defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
+# define ADC_SAMPLES (1)
+# else
+# define ADC_SAMPLES (0)
+#endif
+
+/* Verify ADC FIFO size */
+
+# if CONFIG_ADC_FIFOSIZE != (ADC_SAMPLES + 1)
+# error Invalid ADC FIFO size
+# endif
+
+/* Numerate ADC samples */
+
+# if defined(CONFIG_EXAMPLES_FOC_VEL_ADC) && \
+ defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
+# define VBUS_ADC_SAMPLE (0)
+# define VEL_ADC_SAMPLE (1)
+# elif defined(CONFIG_EXAMPLES_FOC_VBUS_ADC)
+# define VBUS_ADC_SAMPLE (0)
+# elif defined(CONFIG_EXAMPLES_FOC_VEL_ADC)
+# define VEL_ADC_SAMPLE (0)
+# endif
+
+#endif /* CONFIG_EXAMPLES_FOC_HAVE_ADC */
+
+#endif /* __EXAMPLES_FOC_FOC_ADC_H */
diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h
new file mode 100644
index 0000000..f8ec1f8
--- /dev/null
+++ b/examples/foc/foc_cfg.h
@@ -0,0 +1,82 @@
+/****************************************************************************
+ * apps/examples/foc/foc_cfg.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 __EXAMPLES_FOC_FOC_CFG_H
+#define __EXAMPLES_FOC_FOC_CFG_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Velocity ramp must be configured */
+
+#if (CONFIG_EXAMPLES_FOC_RAMP_THR == 0)
+# error
+#endif
+#if (CONFIG_EXAMPLES_FOC_RAMP_ACC == 0)
+# error
+#endif
+#if (CONFIG_EXAMPLES_FOC_RAMP_DEC == 0)
+# error
+#endif
+
+/* ADC Iphase ratio must be provided */
+
+#if (CONFIG_EXAMPLES_FOC_IPHASE_ADC == 0)
+# error
+#endif
+
+/* Printer prescaler */
+
+#if defined(CONFIG_INDUSTRY_FOC_HANDLER_PRINT) && \
+ (CONFIG_EXAMPLES_FOC_STATE_PRINT_FREQ > 0)
+# define FOC_STATE_PRINT_PRE (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ / \
+ CONFIG_EXAMPLES_FOC_STATE_PRINT_FREQ)
+#else
+# undef FOC_STATE_PRINT_PRE
+#endif
+
+/* Velocity ramp configuration */
+
+#define RAMP_CFG_THR (CONFIG_EXAMPLES_FOC_RAMP_THR / 1000.0f)
+#define RAMP_CFG_ACC (CONFIG_EXAMPLES_FOC_RAMP_ACC / 1000.0f)
+#define RAMP_CFG_DEC (CONFIG_EXAMPLES_FOC_RAMP_DEC / 1000.0f)
+
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+
+/* PMSM model parameters */
+
+# define FOC_MODEL_POLES 7
+# define FOC_MODEL_LOAD (1.0f)
+# define FOC_MODEL_RES (0.11f)
+# define FOC_MODEL_IND (0.0002f)
+# define FOC_MODEL_INER (0.1f)
+# define FOC_MODEL_FLUX (0.001f)
+# define FOC_MODEL_INDD (0.0002f)
+# define FOC_MODEL_INDQ (0.0002f)
+#endif
+
+#endif /* __EXAMPLES_FOC_FOC_CFG_H */
diff --git a/examples/foc/foc_debug.h b/examples/foc/foc_debug.h
new file mode 100644
index 0000000..ac765cf
--- /dev/null
+++ b/examples/foc/foc_debug.h
@@ -0,0 +1,60 @@
+/****************************************************************************
+ * apps/examples/foc/foc_debug.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 __EXAMPLES_FOC_FOC_DEBUG_H
+#define __EXAMPLES_FOC_FOC_DEBUG_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <inttypes.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Print support */
+
+#if CONFIG_EXAMPLES_FOC_VERBOSE == 0
+# define PRINTF(...)
+# define PRINTFV(...)
+#elif CONFIG_EXAMPLES_FOC_VERBOSE == 1
+# define PRINTF(format, ...) printf(format, ##__VA_ARGS__)
+# define PRINTFV(...)
+#else
+# define PRINTF(format, ...) printf(format, ##__VA_ARGS__)
+# define PRINTFV(format, ...) printf(format, ##__VA_ARGS__)
+#endif
+
+/* If print is enabled we need float print support */
+
+#if CONFIG_EXAMPLES_FOC_VERBOSE > 1
+# ifdef CONFIG_INDUSTRY_FOC_FLOAT
+# ifndef CONFIG_LIBC_FLOATINGPOINT
+# error "CONFIG_LIBC_FLOATINGPOINT must be set!"
+# endif
+# endif
+#endif
+
+#endif /* __EXAMPLES_FOC_FOC_DEBUG_H */
diff --git a/examples/foc/foc_device.c b/examples/foc/foc_device.c
new file mode 100644
index 0000000..a69460c
--- /dev/null
+++ b/examples/foc/foc_device.c
@@ -0,0 +1,82 @@
+/****************************************************************************
+ * apps/examples/foc/foc_device.c
+ *
+ * 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 "foc_debug.h"
+#include "foc_device.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_device_open
+ ****************************************************************************/
+
+int foc_device_open(FAR struct foc_device_s *dev, int id)
+{
+ char devpath[32];
+ int ret = OK;
+
+ DEBUGASSERT(dev);
+
+ /* Get FOC devpath */
+
+ sprintf(devpath, "%s%d", CONFIG_EXAMPLES_FOC_DEVPATH, id);
+
+ /* Open FOC device */
+
+ dev->fd = open(devpath, 0);
+ if (dev->fd <= 0)
+ {
+ PRINTF("ERROR: open %s failed %d\n", devpath, errno);
+ ret = -errno;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_device_close
+ ****************************************************************************/
+
+int foc_device_close(FAR struct foc_device_s *dev)
+{
+ int ret = OK;
+
+ DEBUGASSERT(dev);
+
+ if (dev->fd > 0)
+ {
+ close(dev->fd);
+ }
+
+ return ret;
+}
diff --git a/examples/foc/foc_device.h b/examples/foc/foc_device.h
new file mode 100644
index 0000000..be5be92
--- /dev/null
+++ b/examples/foc/foc_device.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ * apps/examples/foc/foc_device.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 __EXAMPLES_FOC_FOC_DEVICE_H
+#define __EXAMPLES_FOC_FOC_DEVICE_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* FOC device data */
+
+struct foc_device_s
+{
+ int fd; /* FOC device */
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+int foc_device_open(FAR struct foc_device_s *dev, int id);
+int foc_device_close(FAR struct foc_device_s *dev);
+
+#endif /* __EXAMPLES_FOC_FOC_DEVICE_H */
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
new file mode 100644
index 0000000..7a7cde4
--- /dev/null
+++ b/examples/foc/foc_fixed16_thr.c
@@ -0,0 +1,1018 @@
+/****************************************************************************
+ * apps/examples/foc/foc_fixed16_thr.c
+ *
+ * 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 <dspb16.h>
+
+#include "foc_mq.h"
+#include "foc_thr.h"
+#include "foc_cfg.h"
+#include "foc_adc.h"
+
+#include "foc_debug.h"
+
+#include "industry/foc/foc_utils.h"
+#include "industry/foc/foc_common.h"
+#include "industry/foc/fixed16/foc_handler.h"
+#include "industry/foc/fixed16/foc_ramp.h"
+#include "industry/foc/fixed16/foc_angle.h"
+#include "industry/foc/fixed16/foc_velocity.h"
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+# include "industry/foc/fixed16/foc_model.h"
+#endif
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#ifndef CONFIG_INDUSTRY_FOC_FIXED16
+# error
+#endif
+
+#ifndef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+# error For now only open-loop supported
+#endif
+
+/****************************************************************************
+ * Private Type Definition
+ ****************************************************************************/
+
+/* FOC motor data */
+
+struct foc_motor_b16_s
+{
+ FAR struct foc_ctrl_env_s *envp; /* Thread env */
+ bool fault; /* Fault flag */
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+ bool openloop_now; /* Open-loop now */
+#endif
+ int foc_mode; /* FOC mode */
+ b16_t vbus; /* Power bus voltage */
+ b16_t angle_now; /* Phase angle now */
+ b16_t angle_ol; /* Phase angle open-loop */
+ b16_t vel_set; /* Velocity setting now */
+ b16_t vel_now; /* Velocity now */
+ b16_t vel_des; /* Velocity destination */
+ b16_t dir; /* Motor's direction */
+ b16_t per; /* Controller period in seconds */
+ b16_t iphase_adc; /* Iphase ADC scaling factor */
+ dq_frame_b16_t dq_ref; /* DQ reference */
+ dq_frame_b16_t vdq_comp; /* DQ voltage compensation */
+ foc_handler_b16_t handler; /* FOC controller */
+ struct foc_mq_s mq; /* MQ data */
+ struct foc_info_s info; /* Device info */
+ struct foc_state_b16_s foc_state; /* FOC controller sate */
+ struct foc_state_s dev_state; /* FOC dev state */
+ struct foc_params_s dev_params; /* FOC dev params */
+ struct foc_ramp_b16_s ramp; /* Velocity ramp data */
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+ foc_angle_b16_t openloop; /* Open-loop angle handler */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ struct foc_model_b16_s model; /* Model handler */
+ struct foc_model_state_b16_s model_state; /* PMSM model state */
+#endif
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_motor_init
+ ****************************************************************************/
+
+static int foc_motor_init(FAR struct foc_motor_b16_s *motor,
+ FAR struct foc_ctrl_env_s *envp)
+{
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+ struct foc_openloop_cfg_b16_s ol_cfg;
+#endif
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+ DEBUGASSERT(envp);
+
+ /* Reset data */
+
+ memset(motor, 0, sizeof(struct foc_motor_b16_s));
+
+ /* Connect envp with motor handler */
+
+ motor->envp = envp;
+
+ /* Get device info */
+
+ ret = foc_dev_getinfo(envp->dev.fd, &motor->info);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_getinfo failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Initialize motor data */
+
+ motor->per = b16divi(b16ONE, CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
+ motor->iphase_adc = ftob16((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f);
+
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+ /* Initialize open-loop angle handler */
+
+ foc_angle_init_b16(&motor->openloop,
+ &g_foc_angle_ol_b16);
+
+ /* Configure open-loop angle handler */
+
+ ol_cfg.per = motor->per;
+ foc_angle_cfg_b16(&motor->openloop, &ol_cfg);
+#endif
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_mode_init
+ ****************************************************************************/
+
+static int foc_mode_init(FAR struct foc_motor_b16_s *motor)
+{
+ int ret = OK;
+
+ switch (motor->envp->mode)
+ {
+ case FOC_OPMODE_IDLE:
+ {
+ motor->foc_mode = FOC_HANDLER_MODE_IDLE;
+ break;
+ }
+
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+ case FOC_OPMODE_OL_V_VEL:
+ {
+ motor->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+ motor->openloop_now = true;
+ break;
+ }
+
+ case FOC_OPMODE_OL_C_VEL:
+ {
+ motor->foc_mode = FOC_HANDLER_MODE_CURRENT;
+ motor->openloop_now = true;
+ break;
+ }
+#endif
+
+ default:
+ {
+ PRINTF("ERROR: unsupported op mode %d\n", motor->envp->mode);
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_configure
+ ****************************************************************************/
+
+static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
+{
+#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI
+ struct foc_initdata_b16_s ctrl_cfg;
+#endif
+#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
+ struct foc_mod_cfg_b16_s mod_cfg;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ struct foc_model_pmsm_cfg_b16_s pmsm_cfg;
+#endif
+ struct foc_cfg_s cfg;
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* Initialize velocity ramp */
+
+ ret = foc_ramp_init_b16(&motor->ramp,
+ motor->per,
+ ftob16(RAMP_CFG_THR),
+ ftob16(RAMP_CFG_ACC),
+ ftob16(RAMP_CFG_ACC));
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_ramp_init failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Initialize FOC handler */
+
+ ret = foc_handler_init_b16(&motor->handler,
+ &g_foc_control_pi_b16,
+ &g_foc_mod_svm3_b16);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_handler_init failed %d\n", ret);
+ goto errout;
+ }
+
+#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI
+ /* Get PI controller configuration */
+
+ ctrl_cfg.id_kp = ftob16(motor->envp->pi_kp / 1000.0f);
+ ctrl_cfg.id_ki = ftob16(motor->envp->pi_ki / 1000.0f);
+ ctrl_cfg.iq_kp = ftob16(motor->envp->pi_kp / 1000.0f);
+ ctrl_cfg.iq_ki = ftob16(motor->envp->pi_ki / 1000.0f);
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
+ /* Get SVM3 modulation configuration */
+
+ mod_cfg.pwm_duty_max = FOCDUTY_TO_FIXED16(motor->info.hw_cfg.pwm_max);
+#endif
+
+ /* Configure FOC handler */
+
+ foc_handler_cfg_b16(&motor->handler, &ctrl_cfg, &mod_cfg);
+
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ /* Initialize PMSM model */
+
+ ret = foc_model_init_b16(&motor->model,
+ &g_foc_model_pmsm_ops_b16);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_model_init failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Get PMSM model configuration */
+
+ pmsm_cfg.poles = FOC_MODEL_POLES;
+ pmsm_cfg.res = ftob16(FOC_MODEL_RES);
+ pmsm_cfg.ind = ftob16(FOC_MODEL_IND);
+ pmsm_cfg.iner = ftob16(FOC_MODEL_INER);
+ pmsm_cfg.flux_link = ftob16(FOC_MODEL_FLUX);
+ pmsm_cfg.ind_d = ftob16(FOC_MODEL_INDD);
+ pmsm_cfg.ind_q = ftob16(FOC_MODEL_INDQ);
+ pmsm_cfg.per = motor->per;
+ pmsm_cfg.iphase_adc = motor->iphase_adc;
+
+ /* Configure PMSM model */
+
+ foc_model_cfg_b16(&motor->model, &pmsm_cfg);
+#endif
+
+ /* Get FOC device configuration */
+
+ cfg.pwm_freq = (CONFIG_EXAMPLES_FOC_PWM_FREQ);
+ cfg.notifier_freq = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
+
+ /* Print FOC device configuration */
+
+ foc_cfg_print(&cfg);
+
+ /* Configure FOC device */
+
+ ret = foc_dev_setcfg(motor->envp->dev.fd, &cfg);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_setcfg %d!\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_start
+ ****************************************************************************/
+
+static int foc_motor_start(FAR struct foc_motor_b16_s *motor, bool start)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ if (start == true)
+ {
+ /* Start device if VBUS data present */
+
+ if (motor->mq.vbus > 0)
+ {
+ /* Configure FOC device */
+
+ PRINTF("Configure FOC device %d!\n", motor->envp->id);
+
+ ret = foc_motor_configure(motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_configure failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Start device */
+
+ PRINTF("Start FOC device %d!\n", motor->envp->id);
+
+ ret = foc_dev_start(motor->envp->dev.fd);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_start failed %d!\n", ret);
+ goto errout;
+ }
+ }
+ }
+ else
+ {
+ /* Stop FOC device */
+
+ PRINTF("Stop FOC device %d!\n", motor->envp->id);
+
+ ret = foc_dev_stop(motor->envp->dev.fd);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_stop failed %d!\n", ret);
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_deinit
+ ****************************************************************************/
+
+static int foc_motor_deinit(FAR struct foc_motor_b16_s *motor)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ /* Deinitialize PMSM model */
+
+ ret = foc_model_deinit_b16(&motor->model);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_model_deinit failed %d\n", ret);
+ goto errout;
+ }
+#endif
+
+ /* Deinitialize FOC handler */
+
+ ret = foc_handler_deinit_b16(&motor->handler);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_handler_deinit failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Reset data */
+
+ memset(motor, 0, sizeof(struct foc_motor_b16_s));
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_vbus
+ ****************************************************************************/
+
+static int foc_motor_vbus(FAR struct foc_motor_b16_s *motor, uint32_t vbus)
+{
+ DEBUGASSERT(motor);
+
+ /* Update motor VBUS */
+
+ motor->vbus = b16muli(vbus, ftob16(VBUS_ADC_SCALE));
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_motor_vel
+ ****************************************************************************/
+
+static int foc_motor_vel(FAR struct foc_motor_b16_s *motor, uint32_t vel)
+{
+ b16_t tmp1 = 0;
+ b16_t tmp2 = 0;
+
+ DEBUGASSERT(motor);
+
+ /* Update motor velocity destination
+ * NOTE: velmax may not fit in b16_t so we can't use b16idiv()
+ */
+
+ tmp1 = itob16(motor->envp->velmax / 1000);
+ tmp2 = b16mulb16(ftob16(VEL_ADC_SCALE), tmp1);
+
+ motor->vel_des = b16muli(tmp2, vel);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_motor_state
+ ****************************************************************************/
+
+static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* Get open-loop currents
+ * NOTE: Id always set to 0
+ */
+
+ motor->dq_ref.q = b16idiv(motor->envp->qparam, 1000);
+ motor->dq_ref.d = 0;
+
+ /* Update motor state */
+
+ switch (state)
+ {
+ case FOC_EXAMPLE_STATE_FREE:
+ {
+ motor->vel_set = 0;
+ motor->dir = DIR_NONE_B16;
+
+ /* Force DQ vector to zero */
+
+ motor->dq_ref.q = 0;
+ motor->dq_ref.d = 0;
+
+ break;
+ }
+
+ case FOC_EXAMPLE_STATE_STOP:
+ {
+ motor->vel_set = 0;
+ motor->dir = DIR_NONE_B16;
+
+ /* DQ vector not zero - active brake */
+
+ break;
+ }
+
+ case FOC_EXAMPLE_STATE_CW:
+ {
+ motor->vel_set = 0;
+ motor->dir = DIR_CW_B16;
+
+ break;
+ }
+
+ case FOC_EXAMPLE_STATE_CCW:
+ {
+ motor->vel_set = 0;
+ motor->dir = DIR_CCW_B16;
+
+ break;
+ }
+
+ default:
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_run
+ ****************************************************************************/
+
+static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
+{
+ struct foc_angle_in_b16_s ain;
+ struct foc_angle_out_b16_s aout;
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* No velocity feedback - assume that velocity now is velocity set
+ * TODO: velocity observer or sensor
+ */
+
+ motor->vel_now = motor->vel_set;
+
+ /* Run velocity ramp controller */
+
+ ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des,
+ motor->vel_now, &motor->vel_set);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Update open-loop angle handler */
+
+ ain.vel = motor->vel_set;
+ ain.angle = motor->angle_now;
+ ain.dir = motor->dir;
+
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+ foc_angle_run_b16(&motor->openloop, &ain, &aout);
+
+ /* Store open-loop angle */
+
+ motor->angle_ol = aout.angle;
+
+ /* Get phase angle now */
+
+ if (motor->openloop_now == true)
+ {
+ motor->angle_now = motor->angle_ol;
+ }
+ else
+#endif
+ {
+ /* TODO: get phase angle from observer or sensor */
+
+ ASSERT(0);
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_handler_run
+ ****************************************************************************/
+
+static int foc_handler_run(FAR struct foc_motor_b16_s *motor)
+{
+ struct foc_handler_input_b16_s input;
+ struct foc_handler_output_b16_s output;
+ b16_t current[CONFIG_MOTOR_FOC_PHASES];
+ int ret = OK;
+ int i = 0;
+
+ DEBUGASSERT(motor);
+
+ /* FOC device fault */
+
+ if (motor->fault == true)
+ {
+ /* Stop motor */
+
+ motor->dq_ref.q = 0;
+ motor->dq_ref.d = 0;
+ motor->angle_now = 0;
+ motor->vbus = 0;
+
+ /* Force velocity to zero */
+
+ motor->vel_des = 0;
+ }
+
+ /* Get real currents */
+
+ for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
+ {
+ current[i] = b16muli(motor->iphase_adc, motor->dev_state.curr[i]);
+ }
+
+ /* Get input for FOC handler */
+
+ input.current = current;
+ input.dq_ref = &motor->dq_ref;
+ input.vdq_comp = &motor->vdq_comp;
+ input.angle = motor->angle_now;
+ input.vbus = motor->vbus;
+ input.mode = motor->foc_mode;
+
+ /* Run FOC controller */
+
+ ret = foc_handler_run_b16(&motor->handler, &input, &output);
+
+ /* Get duty from controller */
+
+ for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
+ {
+ motor->dev_params.duty[i] = FOCDUTY_FROM_FIXED16(output.duty[i]);
+ }
+
+ /* Get FOC handler state */
+
+ foc_handler_state_b16(&motor->handler, &motor->foc_state);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_dev_state_get
+ ****************************************************************************/
+
+static int foc_dev_state_get(FAR struct foc_motor_b16_s *motor)
+{
+ int ret = OK;
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ int i;
+#endif
+
+ DEBUGASSERT(motor);
+
+ /* Get FOC state - blocking */
+
+ ret = foc_dev_getstate(motor->envp->dev.fd, &motor->dev_state);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_getstate failed %d!\n", ret);
+ goto errout;
+ }
+
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ /* Get model state */
+
+ foc_model_state_b16(&motor->model, &motor->model_state);
+
+ /* Get model currents */
+
+ for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
+ {
+ motor->dev_state.curr[i] = motor->model_state.curr_raw[i];
+ }
+#endif
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_dev_params_set
+ ****************************************************************************/
+
+static int foc_dev_params_set(FAR struct foc_motor_b16_s *motor)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* Write FOC parameters */
+
+ ret = foc_dev_setparams(motor->envp->dev.fd, &motor->dev_params);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_setparams failed %d!\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_state_handle
+ ****************************************************************************/
+
+static int foc_state_handle(FAR struct foc_motor_b16_s *motor)
+{
+ DEBUGASSERT(motor);
+
+ if (motor->dev_state.fault > 0)
+ {
+ PRINTF("FAULT = %d\n", motor->dev_state.fault);
+ motor->fault = true;
+ }
+ else
+ {
+ motor->fault = false;
+ }
+
+ return OK;
+}
+
+#ifdef FOC_STATE_PRINT_PRE
+/****************************************************************************
+ * Name: foc_state_print
+ ****************************************************************************/
+
+static int foc_state_print(FAR struct foc_motor_b16_s *motor)
+{
+ DEBUGASSERT(motor);
+
+ PRINTF("b16 inst %d:\n", motor->envp->inst);
+
+ foc_handler_state_print_b16(&motor->foc_state);
+
+ return OK;
+}
+#endif
+
+/****************************************************************************
+ * Name: foc_motor_handle
+ ****************************************************************************/
+
+static int foc_motor_handle(FAR struct foc_motor_b16_s *motor,
+ FAR struct foc_mq_s *handle)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+ DEBUGASSERT(handle);
+
+ /* Terminate */
+
+ if (handle->quit == true)
+ {
+ motor->mq.quit = true;
+ }
+
+ /* Update motor VBUS */
+
+ if (motor->mq.vbus != handle->vbus)
+ {
+ PRINTFV("Set vbus=%" PRIu32 " for FOC driver %d!\n",
+ handle->vbus, motor->envp->id);
+
+ ret = foc_motor_vbus(motor, handle->vbus);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_vbus failed %d!\n", ret);
+ goto errout;
+ }
+
+ motor->mq.vbus = handle->vbus;
+ }
+
+ /* Update motor velocity destination */
+
+ if (motor->mq.vel != handle->vel)
+ {
+ PRINTFV("Set vel=%" PRIu32 " for FOC driver %d!\n",
+ handle->vel, motor->envp->id);
+
+ ret = foc_motor_vel(motor, handle->vel);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_vel failed %d!\n", ret);
+ goto errout;
+ }
+
+ motor->mq.vel = handle->vel;
+ }
+
+ /* Update motor state */
+
+ if (motor->mq.app_state != handle->app_state)
+ {
+ PRINTFV("Set app_state=%d for FOC driver %d!\n",
+ handle->app_state, motor->envp->id);
+
+ ret = foc_motor_state(motor, handle->app_state);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_state failed %d!\n", ret);
+ goto errout;
+ }
+
+ motor->mq.app_state = handle->app_state;
+ }
+
+ /* Start/stop motor */
+
+ if (motor->mq.start != handle->start)
+ {
+ PRINTFV("Set start=%d for FOC driver %d!\n",
+ handle->start, motor->envp->id);
+
+ ret = foc_motor_start(motor, handle->start);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_start failed %d!\n", ret);
+ goto errout;
+ }
+
+ motor->mq.start = handle->start;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_fixed16_thr
+ ****************************************************************************/
+
+int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
+{
+ struct foc_mq_s handle;
+ struct foc_motor_b16_s motor;
+ int time = 0;
+ int ret = OK;
+
+ DEBUGASSERT(envp);
+
+ PRINTFV("foc_fixed_thr, id=%d\n", envp->id);
+
+ /* Reset data */
+
+ memset(&handle, 0, sizeof(struct foc_mq_s));
+
+ /* Initialize motor controller */
+
+ ret = foc_motor_init(&motor, envp);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_init failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Initialize controller mode */
+
+ ret = foc_mode_init(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_mode_init failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Start with motor free */
+
+ handle.app_state = FOC_EXAMPLE_STATE_FREE;
+
+ /* Wait some time */
+
+ usleep(1000);
+
+ /* Control loop */
+
+ while (motor.mq.quit == false)
+ {
+ PRINTFV("foc_fixed16_thr %d %d\n", envp->id, time);
+
+ /* Handle mqueue */
+
+ ret = foc_mq_handle(envp->mqd, &handle);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_mq_handle failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Handle motor data */
+
+ ret = foc_motor_handle(&motor, &handle);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_handle failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Run control logic if controller started */
+
+ if (motor.mq.start == true)
+ {
+ /* Get FOC device state */
+
+ ret = foc_dev_state_get(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Handle controller state */
+
+ ret = foc_state_handle(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_state_handle failed %d!\n", ret);
+ goto errout;
+ }
+
+ if (motor.dev_state.fault > 0)
+ {
+ /* Clear fault state */
+
+ ret = foc_dev_clearfault(envp->dev.fd);
+ if (ret != OK)
+ {
+ PRINTF("ERROR: foc_dev_clearfault failed %d!\n", ret);
+ goto errout;
+ }
+ }
+
+ /* Run motor controller */
+
+ ret = foc_motor_run(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_run failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Run FOC */
+
+ ret = foc_handler_run(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_handler_run failed %d!\n", ret);
+ goto errout;
+ }
+
+#ifdef FOC_STATE_PRINT_PRE
+ /* Print state if configured */
+
+ if (time % FOC_STATE_PRINT_PRE == 0)
+ {
+ foc_state_print(&motor);
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ /* Feed FOC model with data */
+
+ foc_model_run_b16(&motor.model,
+ ftob16(FOC_MODEL_LOAD),
+ &motor.foc_state.vab);
+#endif
+
+ /* Set FOC device parameters */
+
+ ret = foc_dev_params_set(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_params_set failed %d!\n", ret);
+ goto errout;
+ }
+ }
+ else
+ {
+ usleep(1000);
+ }
+
+ /* Increase counter */
+
+ time += 1;
+ }
+
+errout:
+
+ /* Deinit motor controller */
+
+ ret = foc_motor_deinit(&motor);
+ if (ret != OK)
+ {
+ PRINTF("ERROR: foc_motor_deinit failed %d!\n", ret);
+ }
+
+ PRINTF("Stop FOC device %d!\n", envp->id);
+
+ /* Stop FOC device */
+
+ ret = foc_dev_stop(envp->dev.fd);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_stop %d!\n", ret);
+ }
+
+ PRINTF("foc_fixed16_thr %d exit\n", envp->id);
+
+ return ret;
+}
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
new file mode 100644
index 0000000..0034414
--- /dev/null
+++ b/examples/foc/foc_float_thr.c
@@ -0,0 +1,1017 @@
+/****************************************************************************
+ * apps/examples/foc/foc_float_thr.c
+ *
+ * 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 <dsp.h>
+
+#include "foc_mq.h"
+#include "foc_thr.h"
+#include "foc_cfg.h"
+#include "foc_adc.h"
+
+#include "foc_debug.h"
+
+#include "industry/foc/foc_utils.h"
+#include "industry/foc/foc_common.h"
+#include "industry/foc/float/foc_handler.h"
+#include "industry/foc/float/foc_ramp.h"
+#include "industry/foc/float/foc_angle.h"
+#include "industry/foc/float/foc_velocity.h"
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+# include "industry/foc/float/foc_model.h"
+#endif
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#ifndef CONFIG_INDUSTRY_FOC_FLOAT
+# error
+#endif
+
+#ifndef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+# error For now only open-loop supported
+#endif
+
+/****************************************************************************
+ * Private Type Definition
+ ****************************************************************************/
+
+/* FOC motor data */
+
+struct foc_motor_f32_s
+{
+ FAR struct foc_ctrl_env_s *envp; /* Thread env */
+ bool fault; /* Fault flag */
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+ bool openloop_now; /* Open-loop now */
+#endif
+ int foc_mode; /* FOC mode */
+ float vbus; /* Power bus voltage */
+ float angle_now; /* Phase angle now */
+ b16_t angle_ol; /* Phase angle open-loop */
+ float vel_set; /* Velocity setting now */
+ float vel_now; /* Velocity now */
+ float vel_des; /* Velocity destination */
+ float dir; /* Motor's direction */
+ float per; /* Controller period in seconds */
+ float iphase_adc; /* Iphase ADC scaling factor */
+ dq_frame_f32_t dq_ref; /* DQ reference */
+ dq_frame_f32_t vdq_comp; /* DQ voltage compensation */
+ foc_handler_f32_t handler; /* FOC controller */
+ struct foc_mq_s mq; /* MQ data */
+ struct foc_info_s info; /* Device info */
+ struct foc_state_f32_s foc_state; /* FOC controller sate */
+ struct foc_state_s dev_state; /* FOC dev state */
+ struct foc_params_s dev_params; /* FOC dev params */
+ struct foc_ramp_f32_s ramp; /* Velocity ramp data */
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+ foc_angle_f32_t openloop; /* Open-loop angle handler */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ struct foc_model_f32_s model; /* Model handler */
+ struct foc_model_state_f32_s model_state; /* PMSM model state */
+#endif
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_motor_init
+ ****************************************************************************/
+
+static int foc_motor_init(FAR struct foc_motor_f32_s *motor,
+ FAR struct foc_ctrl_env_s *envp)
+{
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+ struct foc_openloop_cfg_f32_s ol_cfg;
+#endif
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+ DEBUGASSERT(envp);
+
+ /* Reset data */
+
+ memset(motor, 0, sizeof(struct foc_motor_f32_s));
+
+ /* Connect envp with motor handler */
+
+ motor->envp = envp;
+
+ /* Get device info */
+
+ ret = foc_dev_getinfo(envp->dev.fd, &motor->info);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_dev_getinfo failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Initialize motor data */
+
+ motor->per = (float)(1.0f / CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
+ motor->iphase_adc = ((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f);
+
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+ /* Initialize open-loop angle handler */
+
+ foc_angle_init_f32(&motor->openloop,
+ &g_foc_angle_ol_f32);
+
+ /* Configure open-loop angle handler */
+
+ ol_cfg.per = motor->per;
+ foc_angle_cfg_f32(&motor->openloop, &ol_cfg);
+#endif
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_mode_init
+ ****************************************************************************/
+
+static int foc_mode_init(FAR struct foc_motor_f32_s *motor)
+{
+ int ret = OK;
+
+ switch (motor->envp->mode)
+ {
+ case FOC_OPMODE_IDLE:
+ {
+ motor->foc_mode = FOC_HANDLER_MODE_IDLE;
+ break;
+ }
+
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+ case FOC_OPMODE_OL_V_VEL:
+ {
+ motor->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+ motor->openloop_now = true;
+ break;
+ }
+
+ case FOC_OPMODE_OL_C_VEL:
+ {
+ motor->foc_mode = FOC_HANDLER_MODE_CURRENT;
+ motor->openloop_now = true;
+ break;
+ }
+#endif
+
+ default:
+ {
+ PRINTF("ERROR: unsupported op mode %d\n", motor->envp->mode);
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_configure
+ ****************************************************************************/
+
+static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
+{
+#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI
+ struct foc_initdata_f32_s ctrl_cfg;
+#endif
+#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
+ struct foc_mod_cfg_f32_s mod_cfg;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ struct foc_model_pmsm_cfg_f32_s pmsm_cfg;
+#endif
+ struct foc_cfg_s cfg;
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* Initialize velocity ramp */
+
+ ret = foc_ramp_init_f32(&motor->ramp,
+ motor->per,
+ RAMP_CFG_THR,
+ RAMP_CFG_ACC,
+ RAMP_CFG_ACC);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_ramp_init failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Initialize FOC handler */
+
+ ret = foc_handler_init_f32(&motor->handler,
+ &g_foc_control_pi_f32,
+ &g_foc_mod_svm3_f32);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_handler_init failed %d\n", ret);
+ goto errout;
+ }
+
+#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI
+ /* Get PI controller configuration */
+
+ ctrl_cfg.id_kp = (motor->envp->pi_kp / 1000.0f);
+ ctrl_cfg.id_ki = (motor->envp->pi_ki / 1000.0f);
+ ctrl_cfg.iq_kp = (motor->envp->pi_kp / 1000.0f);
+ ctrl_cfg.iq_ki = (motor->envp->pi_ki / 1000.0f);
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
+ /* Get SVM3 modulation configuration */
+
+ mod_cfg.pwm_duty_max = FOCDUTY_TO_FLOAT(motor->info.hw_cfg.pwm_max);
+#endif
+
+ /* Configure FOC handler */
+
+ foc_handler_cfg_f32(&motor->handler, &ctrl_cfg, &mod_cfg);
+
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ /* Initialize PMSM model */
+
+ ret = foc_model_init_f32(&motor->model,
+ &g_foc_model_pmsm_ops_f32);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_model_init failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Get PMSM model configuration */
+
+ pmsm_cfg.poles = FOC_MODEL_POLES;
+ pmsm_cfg.res = FOC_MODEL_RES;
+ pmsm_cfg.ind = FOC_MODEL_IND;
+ pmsm_cfg.iner = FOC_MODEL_INER;
+ pmsm_cfg.flux_link = FOC_MODEL_FLUX;
+ pmsm_cfg.ind_d = FOC_MODEL_INDD;
+ pmsm_cfg.ind_q = FOC_MODEL_INDQ;
+ pmsm_cfg.per = motor->per;
+ pmsm_cfg.iphase_adc = motor->iphase_adc;
+
+ /* Configure PMSM model */
+
+ foc_model_cfg_f32(&motor->model, &pmsm_cfg);
+#endif
+
+ /* Get FOC device configuration */
+
+ cfg.pwm_freq = (CONFIG_EXAMPLES_FOC_PWM_FREQ);
+ cfg.notifier_freq = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
+
+ /* Print FOC device configuration */
+
+ foc_cfg_print(&cfg);
+
+ /* Configure FOC device */
+
+ ret = foc_dev_setcfg(motor->envp->dev.fd, &cfg);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_dev_setcfg %d!\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_start
+ ****************************************************************************/
+
+static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ if (start == true)
+ {
+ /* Start device if VBUS data present */
+
+ if (motor->mq.vbus > 0)
+ {
+ /* Configure FOC device */
+
+ PRINTF("Configure FOC device %d!\n", motor->envp->id);
+
+ ret = foc_motor_configure(motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_configure failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Start device */
+
+ PRINTF("Start FOC device %d!\n", motor->envp->id);
+
+ ret = foc_dev_start(motor->envp->dev.fd);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_dev_start failed %d!\n", ret);
+ goto errout;
+ }
+ }
+ else
+ {
+ /* Return error if no VBUS data */
+
+ PRINTF("ERROR: start request without VBUS!\n");
+ goto errout;
+ }
+ }
+ else
+ {
+ /* Stop FOC device */
+
+ PRINTF("Stop FOC device %d!\n", motor->envp->id);
+
+ ret = foc_dev_stop(motor->envp->dev.fd);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_dev_stop failed %d!\n", ret);
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_deinit
+ ****************************************************************************/
+
+static int foc_motor_deinit(FAR struct foc_motor_f32_s *motor)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ /* Deinitialize PMSM model */
+
+ ret = foc_model_deinit_f32(&motor->model);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_model_deinit failed %d\n", ret);
+ goto errout;
+ }
+#endif
+
+ /* Deinitialize FOC handler */
+
+ ret = foc_handler_deinit_f32(&motor->handler);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_handler_deinit failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Reset data */
+
+ memset(motor, 0, sizeof(struct foc_motor_f32_s));
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_vbus
+ ****************************************************************************/
+
+static int foc_motor_vbus(FAR struct foc_motor_f32_s *motor, uint32_t vbus)
+{
+ DEBUGASSERT(motor);
+
+ /* Update motor VBUS */
+
+ motor->vbus = (vbus * VBUS_ADC_SCALE);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_motor_vel
+ ****************************************************************************/
+
+static int foc_motor_vel(FAR struct foc_motor_f32_s *motor, uint32_t vel)
+{
+ DEBUGASSERT(motor);
+
+ /* Update motor velocity destination */
+
+ motor->vel_des = (vel * VEL_ADC_SCALE * motor->envp->velmax / 1000.0f);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_motor_state
+ ****************************************************************************/
+
+static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* Get open-loop currents
+ * NOTE: Id always set to 0
+ */
+
+ motor->dq_ref.q = (motor->envp->qparam / 1000.0f);
+ motor->dq_ref.d = 0.0f;
+
+ /* Update motor state */
+
+ switch (state)
+ {
+ case FOC_EXAMPLE_STATE_FREE:
+ {
+ motor->vel_set = 0.0f;
+ motor->dir = DIR_NONE;
+
+ /* Force DQ vector to zero */
+
+ motor->dq_ref.q = 0.0f;
+ motor->dq_ref.d = 0.0f;
+
+ break;
+ }
+
+ case FOC_EXAMPLE_STATE_STOP:
+ {
+ motor->vel_set = 0.0f;
+ motor->dir = DIR_NONE;
+
+ /* DQ vector not zero - active brake */
+
+ break;
+ }
+
+ case FOC_EXAMPLE_STATE_CW:
+ {
+ motor->vel_set = 0.0f;
+ motor->dir = DIR_CW;
+
+ break;
+ }
+
+ case FOC_EXAMPLE_STATE_CCW:
+ {
+ motor->vel_set = 0.0f;
+ motor->dir = DIR_CCW;
+
+ break;
+ }
+
+ default:
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_motor_run
+ ****************************************************************************/
+
+static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
+{
+ struct foc_angle_in_f32_s ain;
+ struct foc_angle_out_f32_s aout;
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* No velocity feedback - assume that velocity now is velocity set
+ * TODO: velocity observer or sensor
+ */
+
+ motor->vel_now = motor->vel_set;
+
+ /* Run velocity ramp controller */
+
+ ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des,
+ motor->vel_now, &motor->vel_set);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Update open-loop angle handler */
+
+ ain.vel = motor->vel_set;
+ ain.angle = motor->angle_now;
+ ain.dir = motor->dir;
+
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+ foc_angle_run_f32(&motor->openloop, &ain, &aout);
+
+ /* Store open-loop angle */
+
+ motor->angle_ol = aout.angle;
+
+ /* Get phase angle now */
+
+ if (motor->openloop_now == true)
+ {
+ motor->angle_now = motor->angle_ol;
+ }
+ else
+#endif
+ {
+ /* TODO: get phase angle from observer or sensor */
+
+ ASSERT(0);
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_handler_run
+ ****************************************************************************/
+
+static int foc_handler_run(FAR struct foc_motor_f32_s *motor)
+{
+ struct foc_handler_input_f32_s input;
+ struct foc_handler_output_f32_s output;
+ float current[CONFIG_MOTOR_FOC_PHASES];
+ int ret = OK;
+ int i = 0;
+
+ DEBUGASSERT(motor);
+
+ /* FOC device fault */
+
+ if (motor->fault == true)
+ {
+ /* Stop motor */
+
+ motor->dq_ref.q = 0;
+ motor->dq_ref.d = 0;
+ motor->angle_now = 0;
+ motor->vbus = 0;
+
+ /* Force velocity to zero */
+
+ motor->vel_des = 0;
+ }
+
+ /* Get real currents */
+
+ for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
+ {
+ current[i] = (motor->iphase_adc * motor->dev_state.curr[i]);
+ }
+
+ /* Get input for FOC handler */
+
+ input.current = current;
+ input.dq_ref = &motor->dq_ref;
+ input.vdq_comp = &motor->vdq_comp;
+ input.angle = motor->angle_now;
+ input.vbus = motor->vbus;
+ input.mode = motor->foc_mode;
+
+ /* Run FOC controller */
+
+ ret = foc_handler_run_f32(&motor->handler, &input, &output);
+
+ /* Get duty from controller */
+
+ for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
+ {
+ motor->dev_params.duty[i] = FOCDUTY_FROM_FLOAT(output.duty[i]);
+ }
+
+ /* Get FOC handler state */
+
+ foc_handler_state_f32(&motor->handler, &motor->foc_state);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_dev_state_get
+ ****************************************************************************/
+
+static int foc_dev_state_get(FAR struct foc_motor_f32_s *motor)
+{
+ int ret = OK;
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ int i;
+#endif
+
+ DEBUGASSERT(motor);
+
+ /* Get FOC state - blocking */
+
+ ret = foc_dev_getstate(motor->envp->dev.fd, &motor->dev_state);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_dev_getstate failed %d!\n", ret);
+ goto errout;
+ }
+
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ /* Get model state */
+
+ foc_model_state_f32(&motor->model, &motor->model_state);
+
+ /* Get model currents */
+
+ for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
+ {
+ motor->dev_state.curr[i] = motor->model_state.curr_raw[i];
+ }
+#endif
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_dev_params_set
+ ****************************************************************************/
+
+static int foc_dev_params_set(FAR struct foc_motor_f32_s *motor)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+
+ /* Write FOC parameters */
+
+ ret = foc_dev_setparams(motor->envp->dev.fd, &motor->dev_params);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_dev_setparams failed %d!\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_state_handle
+ ****************************************************************************/
+
+static int foc_state_handle(FAR struct foc_motor_f32_s *motor)
+{
+ DEBUGASSERT(motor);
+
+ if (motor->dev_state.fault > 0)
+ {
+ PRINTF("FAULT = %d\n", motor->dev_state.fault);
+ motor->fault = true;
+ }
+ else
+ {
+ motor->fault = false;
+ }
+
+ return OK;
+}
+
+#ifdef FOC_STATE_PRINT_PRE
+
+/****************************************************************************
+ * Name: foc_state_print
+ ****************************************************************************/
+
+static int foc_state_print(FAR struct foc_motor_f32_s *motor)
+{
+ DEBUGASSERT(motor);
+
+ PRINTF("f32 inst %d:\n", motor->envp->inst);
+
+ foc_handler_state_print_f32(&motor->foc_state);
+
+ return OK;
+}
+#endif
+
+/****************************************************************************
+ * Name: foc_motor_handle
+ ****************************************************************************/
+
+static int foc_motor_handle(FAR struct foc_motor_f32_s *motor,
+ FAR struct foc_mq_s *handle)
+{
+ int ret = OK;
+
+ DEBUGASSERT(motor);
+ DEBUGASSERT(handle);
+
+ /* Terminate */
+
+ if (handle->quit == true)
+ {
+ motor->mq.quit = true;
+ }
+
+ /* Update motor VBUS */
+
+ if (motor->mq.vbus != handle->vbus)
+ {
+ PRINTFV("Set vbus=%" PRIu32 " for FOC driver %d!\n",
+ handle->vbus, motor->envp->id);
+
+ ret = foc_motor_vbus(motor, handle->vbus);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_vbus failed %d!\n", ret);
+ goto errout;
+ }
+
+ motor->mq.vbus = handle->vbus;
+ }
+
+ /* Update motor velocity destination */
+
+ if (motor->mq.vel != handle->vel)
+ {
+ PRINTFV("Set vel=%" PRIu32 " for FOC driver %d!\n",
+ handle->vel, motor->envp->id);
+
+ ret = foc_motor_vel(motor, handle->vel);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_vel failed %d!\n", ret);
+ goto errout;
+ }
+
+ motor->mq.vel = handle->vel;
+ }
+
+ /* Update motor state */
+
+ if (motor->mq.app_state != handle->app_state)
+ {
+ PRINTFV("Set app_state=%d for FOC driver %d!\n",
+ handle->app_state, motor->envp->id);
+
+ ret = foc_motor_state(motor, handle->app_state);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_state failed %d!\n", ret);
+ goto errout;
+ }
+
+ motor->mq.app_state = handle->app_state;
+ }
+
+ /* Start/stop motor */
+
+ if (motor->mq.start != handle->start)
+ {
+ PRINTFV("Set start=%d for FOC driver %d!\n",
+ handle->start, motor->envp->id);
+
+ ret = foc_motor_start(motor, handle->start);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_start failed %d!\n", ret);
+ goto errout;
+ }
+
+ motor->mq.start = handle->start;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_float_thr
+ ****************************************************************************/
+
+int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
+{
+ struct foc_mq_s handle;
+ struct foc_motor_f32_s motor;
+ int time = 0;
+ int ret = OK;
+
+ DEBUGASSERT(envp);
+
+ PRINTFV("foc_float_thr, id=%d\n", envp->id);
+
+ /* Reset data */
+
+ memset(&handle, 0, sizeof(struct foc_mq_s));
+
+ /* Initialize motor controller */
+
+ ret = foc_motor_init(&motor, envp);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_init failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Initialize controller mode */
+
+ ret = foc_mode_init(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_mode_init failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Start with motor free */
+
+ handle.app_state = FOC_EXAMPLE_STATE_FREE;
+
+ /* Wait some time */
+
+ usleep(1000);
+
+ /* Control loop */
+
+ while (motor.mq.quit == false)
+ {
+ PRINTFV("foc_float_thr %d %d\n", envp->id, time);
+
+ /* Handle mqueue */
+
+ ret = foc_mq_handle(envp->mqd, &handle);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_mq_handle failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Handle motor data */
+
+ ret = foc_motor_handle(&motor, &handle);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_handle failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Run control logic if controller started */
+
+ if (motor.mq.start == true)
+ {
+ /* Get FOC device state */
+
+ ret = foc_dev_state_get(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Handle controller state */
+
+ ret = foc_state_handle(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_state_handle failed %d!\n", ret);
+ goto errout;
+ }
+
+ if (motor.dev_state.fault > 0)
+ {
+ /* Clear fault state */
+
+ ret = foc_dev_clearfault(envp->dev.fd);
+ if (ret != OK)
+ {
+ goto errout;
+ }
+ }
+
+ /* Run motor controller */
+
+ ret = foc_motor_run(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_motor_run failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Run FOC */
+
+ ret = foc_handler_run(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_handler_run failed %d!\n", ret);
+ goto errout;
+ }
+
+#ifdef FOC_STATE_PRINT_PRE
+ /* Print state if configured */
+
+ if (time % FOC_STATE_PRINT_PRE == 0)
+ {
+ foc_state_print(&motor);
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
+ /* Feed FOC model with data */
+
+ foc_model_run_f32(&motor.model,
+ FOC_MODEL_LOAD,
+ &motor.foc_state.vab);
+#endif
+
+ /* Set FOC device parameters */
+
+ ret = foc_dev_params_set(&motor);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_dev_prams_set failed %d!\n", ret);
+ goto errout;
+ }
+ }
+ else
+ {
+ usleep(1000);
+ }
+
+ /* Increase counter */
+
+ time += 1;
+ }
+
+errout:
+
+ /* Deinit motor controller */
+
+ ret = foc_motor_deinit(&motor);
+ if (ret != OK)
+ {
+ PRINTF("ERROR: foc_motor_deinit failed %d!\n", ret);
+ }
+
+ PRINTF("Stop FOC device %d!\n", envp->id);
+
+ /* Stop FOC device */
+
+ ret = foc_dev_stop(envp->dev.fd);
+ if (ret < 0)
+ {
+ PRINTFV("ERROR: foc_dev_stop failed %d!\n", ret);
+ }
+
+ PRINTF("foc_float_thr %d exit\n", envp->id);
+
+ return ret;
+}
diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c
new file mode 100644
index 0000000..e95c43c
--- /dev/null
+++ b/examples/foc/foc_main.c
@@ -0,0 +1,1118 @@
+/****************************************************************************
+ * apps/examples/foc/foc_main.c
+ *
+ * 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 <unistd.h>
+#include <string.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <mqueue.h>
+#include <pthread.h>
+
+#include <sys/types.h>
+#include <sys/ioctl.h>
+#include <sys/boardctl.h>
+#include <nuttx/fs/fs.h>
+
+#include "foc_mq.h"
+#include "foc_thr.h"
+#include "foc_adc.h"
+#include "foc_debug.h"
+#include "foc_device.h"
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+# include <nuttx/input/buttons.h>
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
+# include <nuttx/analog/adc.h>
+# include <nuttx/analog/ioctl.h>
+#endif
+
+#include "industry/foc/foc_common.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Main loop sleep */
+
+#define MAIN_LOOP_USLEEP (200000)
+
+/* Button init state */
+
+#if CONFIG_EXAMPLES_FOC_STATE_INIT == 1
+# define STATE_BUTTON_I (0)
+#elif CONFIG_EXAMPLES_FOC_STATE_INIT == 2
+# define STATE_BUTTON_I (2)
+#elif CONFIG_EXAMPLES_FOC_STATE_INIT == 3
+# define STATE_BUTTON_I (1)
+#elif CONFIG_EXAMPLES_FOC_STATE_INIT == 4
+# define STATE_BUTTON_I (3)
+#else
+# error
+#endif
+
+/* Enabled instnaces default state */
+
+#define INST_EN_DEAFULT (0xff)
+
+/****************************************************************************
+ * Private Type Definition
+ ****************************************************************************/
+
+/* Application arguments */
+
+struct args_s
+{
+ int time; /* Run time limit in sec, -1 if forever */
+ int mode; /* Operation mode */
+ int qparam; /* Open-loop Q setting (x1000) */
+ uint32_t pi_kp; /* PI Kp (x1000) */
+ uint32_t pi_ki; /* PI Ki (x1000) */
+ uint32_t velmax; /* Velocity max (x1000) */
+ int state; /* Example state (FREE, CW, CCW, STOP) */
+ int8_t en; /* Enabled instances (bit-encoded) */
+};
+
+/****************************************************************************
+ * Private Function Protototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+/* Example state */
+
+static const int g_state_list[5] =
+{
+ FOC_EXAMPLE_STATE_FREE,
+ FOC_EXAMPLE_STATE_CW,
+ FOC_EXAMPLE_STATE_STOP,
+ FOC_EXAMPLE_STATE_CCW,
+ 0
+};
+#endif
+
+pthread_mutex_t g_cntr_lock;
+
+#ifdef CONFIG_INDUSTRY_FOC_FLOAT
+static int g_float_thr_cntr = 0;
+#endif
+#ifdef CONFIG_INDUSTRY_FOC_FIXED16
+static int g_fixed16_thr_cntr = 0;
+#endif
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+#ifdef CONFIG_BUILTIN
+/****************************************************************************
+ * Name: foc_help
+ ****************************************************************************/
+
+static void foc_help(void)
+{
+ PRINTF("Usage: foc [OPTIONS]\n");
+ PRINTF(" [-t] run time\n");
+ PRINTF(" [-h] shows this message and exits\n");
+ PRINTF(" [-m] controller mode\n");
+ PRINTF(" 1 - IDLE mode\n");
+ PRINTF(" 2 - voltage open-loop velocity \n");
+ PRINTF(" 3 - current open-loop velocity \n");
+ PRINTF(" [-o] openloop Vq/Iq setting [x1000]\n");
+ PRINTF(" [-i] PI Ki coefficient [x1000]\n");
+ PRINTF(" [-p] KI Kp coefficient [x1000]\n");
+ PRINTF(" [-v] velocity [x1000]\n");
+ PRINTF(" [-s] motor state\n");
+ PRINTF(" 1 - motor free\n");
+ PRINTF(" 2 - motor stop\n");
+ PRINTF(" 3 - motor CW\n");
+ PRINTF(" 4 - motor CCW\n");
+ PRINTF(" [-j] enable specific instnaces\n");
+}
+
+/****************************************************************************
+ * Name: arg_string
+ ****************************************************************************/
+
+static int arg_string(FAR char **arg, FAR char **value)
+{
+ FAR char *ptr = *arg;
+
+ if (ptr[2] == '\0')
+ {
+ *value = arg[1];
+ return 2;
+ }
+ else
+ {
+ *value = &ptr[2];
+ return 1;
+ }
+}
+
+/****************************************************************************
+ * Name: arg_decimal
+ ****************************************************************************/
+
+static int arg_decimal(FAR char **arg, FAR int *value)
+{
+ FAR char *string;
+ int ret;
+
+ ret = arg_string(arg, &string);
+ *value = atoi(string);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: parse_args
+ ****************************************************************************/
+
+static void parse_args(FAR struct args_s *args, int argc, FAR char **argv)
+{
+ FAR char *ptr;
+ int index;
+ int nargs;
+ int i_value;
+
+ for (index = 1; index < argc; )
+ {
+ ptr = argv[index];
+ if (ptr[0] != '-')
+ {
+ PRINTF("Invalid options format: %s\n", ptr);
+ exit(0);
+ }
+
+ switch (ptr[1])
+ {
+ /* Get time */
+
+ case 't':
+ {
+ nargs = arg_decimal(&argv[index], &i_value);
+ index += nargs;
+
+ if (i_value <= 0 && i_value != -1)
+ {
+ PRINTF("Invalid time value %d s\n", i_value);
+ exit(1);
+ }
+
+ args->time = i_value;
+ break;
+ }
+
+ case 'm':
+ {
+ nargs = arg_decimal(&argv[index], &i_value);
+ index += nargs;
+
+ if (i_value != FOC_OPMODE_IDLE &&
+ i_value != FOC_OPMODE_OL_V_VEL &&
+ i_value != FOC_OPMODE_OL_C_VEL)
+ {
+ PRINTF("Invalid op mode value %d s\n", i_value);
+ exit(1);
+ }
+
+ args->mode = i_value;
+ break;
+ }
+
+ case 'o':
+ {
+ nargs = arg_decimal(&argv[index], &i_value);
+ index += nargs;
+
+ args->qparam = i_value;
+ break;
+ }
+
+ case 'p':
+ {
+ nargs = arg_decimal(&argv[index], &i_value);
+ index += nargs;
+
+ args->pi_kp = i_value;
+ break;
+ }
+
+ case 'i':
+ {
+ nargs = arg_decimal(&argv[index], &i_value);
+ index += nargs;
+
+ args->pi_ki = i_value;
+ break;
+ }
+
+ case 'v':
+ {
+ nargs = arg_decimal(&argv[index], &i_value);
+ index += nargs;
+
+ args->velmax = i_value;
+ break;
+ }
+
+ case 's':
+ {
+ nargs = arg_decimal(&argv[index], &i_value);
+ index += nargs;
+
+ if (i_value != FOC_EXAMPLE_STATE_FREE &&
+ i_value != FOC_EXAMPLE_STATE_STOP &&
+ i_value != FOC_EXAMPLE_STATE_CW &&
+ i_value != FOC_EXAMPLE_STATE_CCW)
+ {
+ PRINTF("Invalid state value %d s\n", i_value);
+ exit(1);
+ }
+
+ args->state = i_value;
+ break;
+ }
+
+ case 'j':
+ {
+ nargs = arg_decimal(&argv[index], &i_value);
+ index += nargs;
+
+ args->en = i_value;
+ break;
+ }
+
+ case 'h':
+ {
+ foc_help();
+ exit(0);
+ }
+
+ default:
+ {
+ PRINTF("Unsupported option: %s\n", ptr);
+ foc_help();
+ exit(1);
+ }
+ }
+ }
+}
+#endif
+
+/****************************************************************************
+ * Name: init_args
+ ****************************************************************************/
+
+static void init_args(FAR struct args_s *args)
+{
+ args->time =
+ (args->time == 0 ? CONFIG_EXAMPLES_FOC_TIME_DEFAULT : args->time);
+ args->mode =
+ (args->mode == 0 ? CONFIG_EXAMPLES_FOC_OPMODE : args->mode);
+ args->qparam =
+ (args->qparam == 0 ? CONFIG_EXAMPLES_FOC_OPENLOOP_Q : args->qparam);
+ args->pi_kp =
+ (args->pi_kp == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KP : args->pi_kp);
+ args->pi_ki =
+ (args->pi_ki == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KI : args->pi_ki);
+#ifdef CONFIG_EXAMPLES_FOC_VEL_ADC
+ args->velmax =
+ (args->velmax == 0 ? CONFIG_EXAMPLES_FOC_VEL_ADC_MAX : args->velmax);
+#else
+ args->velmax =
+ (args->velmax == 0 ? CONFIG_EXAMPLES_FOC_VEL_CONST_VALUE : args->velmax);
+#endif
+ args->state =
+ (args->state == 0 ? CONFIG_EXAMPLES_FOC_STATE_INIT : args->state);
+ args->en = (args->en == -1 ? INST_EN_DEAFULT : args->en);
+}
+
+/****************************************************************************
+ * Name: validate_args
+ ****************************************************************************/
+
+static int validate_args(FAR struct args_s *args)
+{
+ int ret = -EINVAL;
+
+ if (args->pi_kp == 0 && args->pi_ki == 0)
+ {
+ PRINTF("ERROR: missign Kp/Ki configuration\n");
+ goto errout;
+ }
+
+ /* Otherwise OK */
+
+ ret = OK;
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_mq_send
+ ****************************************************************************/
+
+static int foc_mq_send(mqd_t mqd, uint8_t msg, FAR void *data)
+{
+ int ret = OK;
+ uint8_t buffer[5];
+ uint32_t tmp = 0;
+
+ DEBUGASSERT(data);
+
+ /* Data max 4B */
+
+ tmp = *((FAR uint32_t *) data);
+
+ buffer[0] = msg;
+ buffer[1] = ((tmp & 0x000000ff) >> 0);
+ buffer[2] = ((tmp & 0x0000ff00) >> 8);
+ buffer[3] = ((tmp & 0x00ff0000) >> 16);
+ buffer[4] = ((tmp & 0xff000000) >> 24);
+
+ ret = mq_send(mqd, (FAR char *)buffer, 5, 42);
+ if (ret < 0)
+ {
+ PRINTF("foc_main: mq_send failed %d\n", errno);
+ ret = -errno;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_vbus_send
+ ****************************************************************************/
+
+static int foc_vbus_send(mqd_t mqd, uint32_t vbus)
+{
+ return foc_mq_send(mqd, CONTROL_MQ_MSG_VBUS, (FAR void *)&vbus);
+}
+
+/****************************************************************************
+ * Name: foc_vel_send
+ ****************************************************************************/
+
+static int foc_vel_send(mqd_t mqd, uint32_t vel)
+{
+ return foc_mq_send(mqd, CONTROL_MQ_MSG_VEL, (FAR void *)&vel);
+}
+
+/****************************************************************************
+ * Name: foc_state_send
+ ****************************************************************************/
+
+static int foc_state_send(mqd_t mqd, uint32_t state)
+{
+ return foc_mq_send(mqd, CONTROL_MQ_MSG_APPSTATE, (FAR void *)&state);
+}
+
+/****************************************************************************
+ * Name: foc_start_send
+ ****************************************************************************/
+
+static int foc_start_send(mqd_t mqd)
+{
+ int tmp = 0;
+ return foc_mq_send(mqd, CONTROL_MQ_MSG_START, (FAR void *)&tmp);
+}
+
+/****************************************************************************
+ * Name: foc_kill_send
+ ****************************************************************************/
+
+static int foc_kill_send(mqd_t mqd)
+{
+ int tmp = 0;
+ return foc_mq_send(mqd, CONTROL_MQ_MSG_KILL, (FAR void *)&tmp);
+}
+
+/****************************************************************************
+ * Name: foc_control_thr
+ ****************************************************************************/
+
+FAR void *foc_control_thr(FAR void *arg)
+{
+ FAR struct foc_ctrl_env_s *envp = (FAR struct foc_ctrl_env_s *) arg;
+ char mqname[10];
+ int ret = OK;
+
+ DEBUGASSERT(envp);
+
+ /* Open FOC device as blocking */
+
+ ret = foc_device_open(&envp->dev, envp->id);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_device_open failed %d!\n", ret);
+ goto errout;
+ }
+
+ /* Get controller type */
+
+ pthread_mutex_lock(&g_cntr_lock);
+
+#ifdef CONFIG_INDUSTRY_FOC_FLOAT
+ if (g_float_thr_cntr < CONFIG_EXAMPLES_FOC_FLOAT_INST)
+ {
+ envp->type = FOC_NUMBER_TYPE_FLOAT;
+ }
+ else
+#endif
+#ifdef CONFIG_INDUSTRY_FOC_FIXED16
+ if (g_fixed16_thr_cntr < CONFIG_EXAMPLES_FOC_FIXED16_INST)
+ {
+ envp->type = FOC_NUMBER_TYPE_FIXED16;
+ }
+ else
+#endif
+ {
+ /* Invalid configuration */
+
+ ASSERT(0);
+ }
+
+ pthread_mutex_unlock(&g_cntr_lock);
+
+ PRINTF("FOC device %d type = %d!\n", envp->id, envp->type);
+
+ /* Get queue name */
+
+ sprintf(mqname, "%s%d", CONTROL_MQ_MQNAME, envp->id);
+
+ /* Open queue */
+
+ envp->mqd = mq_open(mqname, (O_RDONLY | O_NONBLOCK), 0666, NULL);
+ if (envp->mqd == (mqd_t)-1)
+ {
+ PRINTF("ERROR: mq_open failed errno=%d\n", errno);
+ goto errout;
+ }
+
+ /* Select control logic according to FOC device type */
+
+ switch (envp->type)
+ {
+#ifdef CONFIG_INDUSTRY_FOC_FLOAT
+ case FOC_NUMBER_TYPE_FLOAT:
+ {
+ pthread_mutex_lock(&g_cntr_lock);
+ envp->inst = g_float_thr_cntr;
+ g_float_thr_cntr += 1;
+ pthread_mutex_unlock(&g_cntr_lock);
+
+ /* Start thread */
+
+ ret = foc_float_thr(envp);
+
+ pthread_mutex_lock(&g_cntr_lock);
+ g_float_thr_cntr -= 1;
+ pthread_mutex_unlock(&g_cntr_lock);
+
+ break;
+ }
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_FIXED16
+ case FOC_NUMBER_TYPE_FIXED16:
+ {
+ pthread_mutex_lock(&g_cntr_lock);
+ envp->inst = g_fixed16_thr_cntr;
+ g_fixed16_thr_cntr += 1;
+ pthread_mutex_unlock(&g_cntr_lock);
+
+ /* Start thread */
+
+ ret = foc_fixed16_thr(envp);
+
+ pthread_mutex_lock(&g_cntr_lock);
+ g_fixed16_thr_cntr -= 1;
+ pthread_mutex_unlock(&g_cntr_lock);
+
+ break;
+ }
+#endif
+
+ default:
+ {
+ PRINTF("ERROR: unknown FOC device type %d\n", envp->type);
+ goto errout;
+ }
+ }
+
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc control thread failed %d\n", ret);
+ }
+
+errout:
+
+ /* Close FOC control device */
+
+ ret = foc_device_close(&envp->dev);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_device_close %d failed %d\n", envp->id, ret);
+ }
+
+ /* Close queue */
+
+ if (envp->mqd == (mqd_t)-1)
+ {
+ mq_close(envp->mqd);
+ }
+
+ PRINTFV("foc_control_thr %d exit\n", envp->id);
+
+ return NULL;
+}
+
+/****************************************************************************
+ * Name: foc_threads_init
+ ****************************************************************************/
+
+static int foc_threads_init(FAR struct foc_ctrl_env_s *foc, int i,
+ FAR mqd_t *mqd, FAR pthread_t *thread)
+{
+ char mqname[10];
+ int ret = OK;
+ pthread_attr_t attr;
+ struct mq_attr mqattr;
+ struct sched_param param;
+
+ DEBUGASSERT(foc);
+ DEBUGASSERT(mqd);
+ DEBUGASSERT(thread);
+
+ /* Store device id */
+
+ foc->id = i;
+
+ /* Fill in attributes for message queue */
+
+ mqattr.mq_maxmsg = CONTROL_MQ_MAXMSG;
+ mqattr.mq_msgsize = CONTROL_MQ_MSGSIZE;
+ mqattr.mq_flags = 0;
+
+ /* Get queue name */
+
+ sprintf(mqname, "%s%d", CONTROL_MQ_MQNAME, foc->id);
+
+ /* Initialize thread recv queue */
+
+ *mqd = mq_open(mqname, (O_WRONLY | O_CREAT | O_NONBLOCK),
+ 0666, &mqattr);
+ if (*mqd < 0)
+ {
+ PRINTF("ERROR: mq_open %s failed errno=%d\n", mqname, errno);
+ goto errout;
+ }
+
+ /* Configure thread */
+
+ pthread_attr_init(&attr);
+ param.sched_priority = CONFIG_EXAMPLES_FOC_CONTROL_PRIO;
+ pthread_attr_setschedparam(&attr, ¶m);
+ pthread_attr_setstacksize(&attr, CONFIG_EXAMPLES_FOC_CONTROL_STACKSIZE);
+
+ /* Create FOC threads */
+
+ ret = pthread_create(thread, &attr, foc_control_thr, foc);
+ if (ret != 0)
+ {
+ PRINTF("ERROR: pthread_create ctrl failed %d\n", ret);
+ ret = -ret;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: main
+ ****************************************************************************/
+
+int main(int argc, char *argv[])
+{
+ struct foc_ctrl_env_s foc[CONFIG_MOTOR_FOC_INST];
+ pthread_t threads[CONFIG_MOTOR_FOC_INST];
+ mqd_t mqd[CONFIG_MOTOR_FOC_INST];
+ struct args_s args;
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
+ int adc_fd = 0;
+ bool adc_trigger = false;
+ struct adc_msg_s adc_sample[ADC_SAMPLES];
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+ btn_buttonset_t b_sample = 0;
+ int b_fd = 0;
+ int state_i = 0;
+#endif
+ uint32_t state = 0;
+ uint32_t vbus_raw = 0;
+ int32_t vel_raw = 0;
+ bool vbus_update = false;
+ bool state_update = false;
+ bool vel_update = false;
+ bool terminate = false;
+ bool started = false;
+ int ret = OK;
+ int i = 0;
+ int time = 0;
+
+ /* Reset some data */
+
+ memset(&args, 0, sizeof(struct args_s));
+ memset(mqd, 0, sizeof(mqd_t) * CONFIG_MOTOR_FOC_INST);
+ memset(foc, 0, sizeof(struct foc_ctrl_env_s) * CONFIG_MOTOR_FOC_INST);
+ memset(threads, 0, sizeof(pthread_t) * CONFIG_MOTOR_FOC_INST);
+
+ /* Initialize args before parse */
+
+ args.en = -1;
+
+#ifdef CONFIG_BUILTIN
+ /* Parse the command line */
+
+ parse_args(&args, argc, argv);
+#endif
+
+ /* Initialize args */
+
+ init_args(&args);
+
+ /* Validate arguments */
+
+ ret = validate_args(&args);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: validate args failed\n");
+ goto errout_no_mutex;
+ }
+
+#ifndef CONFIG_NSH_ARCHINIT
+ /* Perform architecture-specific initialization (if configured) */
+
+ boardctl(BOARDIOC_INIT, 0);
+
+# ifdef CONFIG_BOARDCTL_FINALINIT
+ /* Perform architecture-specific final-initialization (if configured) */
+
+ boardctl(BOARDIOC_FINALINIT, 0);
+# endif
+#endif
+
+ PRINTF("\nStart foc_main application!\n\n");
+
+ /* Initialize mutex */
+
+ ret = pthread_mutex_init(&g_cntr_lock, NULL);
+ if (ret != 0)
+ {
+ PRINTF("ERROR: pthread_mutex_init failed %d\n", errno);
+ goto errout_no_mutex;
+ }
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
+ /* Open ADC */
+
+ adc_fd = open(CONFIG_EXAMPLES_FOC_ADC_DEVPATH, (O_RDONLY | O_NONBLOCK));
+ if (adc_fd <= 0)
+ {
+ PRINTF("ERROR: failed to open %s %d\n",
+ CONFIG_EXAMPLES_FOC_ADC_DEVPATH, errno);
+
+ ret = -errno;
+ goto errout;
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+ /* Open button driver */
+
+ b_fd = open(CONFIG_EXAMPLES_FOC_BUTTON_DEVPATH, (O_RDONLY | O_NONBLOCK));
+ if (b_fd < 0)
+ {
+ PRINTF("ERROR: failed to open %s %d\n",
+ CONFIG_EXAMPLES_FOC_BUTTON_DEVPATH, errno);
+ goto errout;
+ }
+#endif
+
+ /* Initialzie FOC controllers */
+
+ for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
+ {
+ /* Get configuration */
+
+ foc[i].qparam = args.qparam;
+ foc[i].mode = args.mode;
+ foc[i].pi_kp = args.pi_kp;
+ foc[i].pi_ki = args.pi_ki;
+ foc[i].velmax = args.velmax;
+
+ if (args.en & (1 << i))
+ {
+ /* Initialize controller thread if enabled */
+
+ ret = foc_threads_init(&foc[i], i, &mqd[i], &threads[i]);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_threads_init failed %d!\n", ret);
+ goto errout;
+ }
+ }
+ }
+
+ /* Wait some time to finish all controllers initialziation */
+
+ usleep(10000);
+
+ /* Initial update for VBUS and VEL */
+
+#ifndef CONFIG_EXAMPLES_FOC_VBUS_ADC
+ vbus_update = true;
+ vbus_raw = VBUS_CONST_VALUE;
+#endif
+#ifndef CONFIG_EXAMPLES_FOC_VEL_ADC
+ vel_update = true;
+ vel_raw = 1;
+#endif
+ state_update = true;
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
+ /* Initial ADC trigger */
+
+ ret = ioctl(adc_fd, ANIOC_TRIGGER, 0);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: ANIOC_TRIGGER ioctl failed: %d\n", errno);
+ goto errout;
+ }
+
+ /* Make sure that conversion is done before first read form ADC device */
+
+ usleep(10000);
+
+ /* Read ADC data if the first loop cylce */
+
+ adc_trigger = false;
+#endif
+
+ /* Controller state */
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+ state_i = STATE_BUTTON_I;
+#endif
+ state = args.state;
+
+ /* Auxliary control loop */
+
+ while (terminate != true)
+ {
+ PRINTFV("foc_main loop %d\n", time);
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
+ /* Get button state */
+
+ ret = read(b_fd, &b_sample, sizeof(btn_buttonset_t));
+ if (ret < 0)
+ {
+ if (errno != EAGAIN)
+ {
+ PRINTF("ERROR: read button failed %d\n", errno);
+ }
+ }
+
+ /* Next state */
+
+ if (b_sample & (1 << 0))
+ {
+ state_i += 1;
+
+ if (g_state_list[state_i] == 0)
+ {
+ state_i = 0;
+ }
+
+ state = g_state_list[state_i];
+ state_update = true;
+
+ PRINTF("BUTTON STATE %" PRIu32 "\n", state);
+ }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_ADC
+ if (adc_trigger == true)
+ {
+ /* Issue the software trigger to start ADC conversion */
+
+ ret = ioctl(adc_fd, ANIOC_TRIGGER, 0);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: ANIOC_TRIGGER ioctl failed: %d\n", errno);
+ goto errout;
+ }
+
+ /* No ADC trigger next cycle */
+
+ adc_trigger = false;
+ }
+ else
+ {
+ /* Get ADC samples */
+
+ ret = read(adc_fd, adc_sample,
+ (ADC_SAMPLES * sizeof(struct adc_msg_s)));
+ if (ret < 0)
+ {
+ if (errno != EAGAIN)
+ {
+ PRINTF("ERROR: adc read failed %d\n", errno);
+ }
+ }
+ else
+ {
+ /* Verify we have received the configured number of samples */
+
+ if (ret != ADC_SAMPLES * sizeof(struct adc_msg_s))
+ {
+ PRINTF("ERROR: adc read invalid read %d != %d\n",
+ ret, ADC_SAMPLES * sizeof(struct adc_msg_s));
+ ret = -EINVAL;
+ goto errout;
+ }
+
+# ifdef CONFIG_EXAMPLES_FOC_VBUS_ADC
+ /* Get raw VBUS */
+
+ vbus_raw = adc_sample[VBUS_ADC_SAMPLE].am_data;
+
+ vbus_update = true;
+# endif
+
+# ifdef CONFIG_EXAMPLES_FOC_VEL_ADC
+ /* Get raw VEL */
+
+ vel_raw = adc_sample[VEL_ADC_SAMPLE].am_data;
+
+ vel_update = true;
+# endif
+
+ /* ADC trigger next cycle */
+
+ adc_trigger = true;
+ }
+ }
+#endif
+
+ /* 1. Update VBUS */
+
+ if (vbus_update == true)
+ {
+ for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
+ {
+ if (args.en & (1 << i))
+ {
+ PRINTFV("Send vbus to %d\n", i);
+
+ /* Send VBUS to thread */
+
+ ret = foc_vbus_send(mqd[i], vbus_raw);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_vbus_send failed %d\n", ret);
+ goto errout;
+ }
+ }
+ }
+
+ /* Reset flag */
+
+ vbus_update = false;
+ }
+
+ /* 2. Update motor state */
+
+ if (state_update == true)
+ {
+ for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
+ {
+ if (args.en & (1 << i))
+ {
+ PRINTFV("Send state %" PRIu32 " to %d\n", state, i);
+
+ /* Send STATE to thread */
+
+ ret = foc_state_send(mqd[i], state);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_state_send failed %d\n", ret);
+ goto errout;
+ }
+ }
+ }
+
+ /* Reset flag */
+
+ state_update = false;
+ }
+
+ /* 3. Update motor velocity */
+
+ if (vel_update == true)
+ {
+ for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
+ {
+ if (args.en & (1 << i))
+ {
+ PRINTFV("Send velocity to %d\n", i);
+
+ /* Send VELOCITY to threads */
+
+ ret = foc_vel_send(mqd[i], vel_raw);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_vel_send failed %d\n", ret);
+ goto errout;
+ }
+ }
+ }
+
+ /* Reset flag */
+
+ vel_update = false;
+ }
+
+ /* 4. One time start */
+
+ if (started == false)
+ {
+ for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
+ {
+ if (args.en & (1 << i))
+ {
+ PRINTFV("Send start to %d\n", i);
+
+ /* Send START to threads */
+
+ ret = foc_start_send(mqd[i]);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_start_send failed %d\n", ret);
+ goto errout;
+ }
+ }
+ }
+
+ /* Set flag */
+
+ started = true;
+ }
+
+ /* Handle run time */
+
+ time += 1;
+
+ if (args.time != -1)
+ {
+ if (time >= (args.time * (1000000 / MAIN_LOOP_USLEEP)))
+ {
+ /* Exit loop */
+
+ terminate = true;
+ }
+ }
+
+ usleep(MAIN_LOOP_USLEEP);
+ }
+
+errout:
+
+ /* Stop FOC control threads */
+
+ for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
+ {
+ if (args.en & (1 << i))
+ {
+ if (mqd[i] != (mqd_t)-1)
+ {
+ /* Stop thread message */
+
+ ret = foc_kill_send(mqd[i]);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_kill_send failed %d\n", ret);
+ }
+ }
+ }
+ }
+
+ /* Wait some time */
+
+ usleep(100000);
+
+ /* De-initialize all FOC control threads */
+
+ for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
+ {
+ if (args.en & (1 << i))
+ {
+ /* Close FOC control thread queue */
+
+ if (mqd[i] != (mqd_t)-1)
+ {
+ mq_close(mqd[i]);
+ }
+ }
+ }
+
+errout_no_mutex:
+
+ /* Free/uninitialize data structures */
+
+ pthread_mutex_destroy(&g_cntr_lock);
+
+ PRINTF("foc_main exit\n");
+
+ return 0;
+}
diff --git a/examples/foc/foc_mq.c b/examples/foc/foc_mq.c
new file mode 100644
index 0000000..a28116f
--- /dev/null
+++ b/examples/foc/foc_mq.c
@@ -0,0 +1,118 @@
+/****************************************************************************
+ * apps/examples/foc/foc_mq.c
+ *
+ * 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 <errno.h>
+#include <strings.h>
+
+#include "foc_debug.h"
+#include "foc_mq.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_mq_handle
+ ****************************************************************************/
+
+int foc_mq_handle(mqd_t mq, FAR struct foc_mq_s *h)
+{
+ int ret = OK;
+ uint8_t buffer[5];
+
+ /* Get data from AUX */
+
+ ret = mq_receive(mq, (char *)buffer, 5, 0);
+ if (ret < 0)
+ {
+ if (errno != EAGAIN)
+ {
+ PRINTF("ERROR: mq_receive failed %d\n", errno);
+ ret = -errno;
+ goto errout;
+ }
+ else
+ {
+ /* Timeout */
+
+ ret = OK;
+ goto errout;
+ }
+ }
+
+ /* Verify message length */
+
+ if (ret != 5)
+ {
+ PRINTF("ERROR: invalid message length = %d\n", ret);
+ goto errout;
+ }
+
+ /* Handle message */
+
+ switch (buffer[0])
+ {
+ case CONTROL_MQ_MSG_VBUS:
+ {
+ memcpy(&h->vbus, &buffer[1], 4);
+ break;
+ }
+
+ case CONTROL_MQ_MSG_APPSTATE:
+ {
+ memcpy(&h->app_state, &buffer[1], 4);
+ break;
+ }
+
+ case CONTROL_MQ_MSG_VEL:
+ {
+ memcpy(&h->vel, &buffer[1], 4);
+ break;
+ }
+
+ case CONTROL_MQ_MSG_START:
+ {
+ h->start = true;
+ break;
+ }
+
+ case CONTROL_MQ_MSG_KILL:
+ {
+ h->quit = true;
+ break;
+ }
+
+ default:
+ {
+ PRINTF("ERROR: invalid message type %d\n", buffer[0]);
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
diff --git a/examples/foc/foc_mq.h b/examples/foc/foc_mq.h
new file mode 100644
index 0000000..319cc94
--- /dev/null
+++ b/examples/foc/foc_mq.h
@@ -0,0 +1,83 @@
+/****************************************************************************
+ * apps/examples/foc/foc_mq.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 __EXAMPLES_FOC_FOC_MQ_H
+#define __EXAMPLES_FOC_FOC_MQ_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <mqueue.h>
+
+#include "foc_device.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define CONTROL_MQ_MAXMSG (10)
+#define CONTROL_MQ_MSGSIZE (5)
+#define CONTROL_MQ_MQNAME "mqf"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Thread message type */
+
+enum foc_thr_msg_e
+{
+ CONTROL_MQ_MSG_INVALID = 0,
+ CONTROL_MQ_MSG_VBUS = 1,
+ CONTROL_MQ_MSG_APPSTATE = 2,
+ CONTROL_MQ_MSG_VEL = 3,
+ CONTROL_MQ_MSG_START = 4,
+ CONTROL_MQ_MSG_KILL = 5
+};
+
+/* FOC thread handler */
+
+struct foc_mq_s
+{
+ bool quit;
+ bool start;
+ int app_state;
+ uint32_t vbus;
+ uint32_t vel;
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_mq_handle
+ ****************************************************************************/
+
+int foc_mq_handle(mqd_t mq, FAR struct foc_mq_s *h);
+
+#endif /* __EXAMPLES_FOC_FOC_THR_H */
diff --git a/examples/foc/foc_thr.h b/examples/foc/foc_thr.h
new file mode 100644
index 0000000..5bb9e54
--- /dev/null
+++ b/examples/foc/foc_thr.h
@@ -0,0 +1,101 @@
+/****************************************************************************
+ * apps/examples/foc/foc_thr.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 __EXAMPLES_FOC_FOC_THR_H
+#define __EXAMPLES_FOC_FOC_THR_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <nuttx/motor/foc/foc.h>
+
+#include <mqueue.h>
+
+#include "foc_device.h"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* FOC example state */
+
+enum foc_example_state_e
+{
+ FOC_EXAMPLE_STATE_INVALID = 0, /* Reserved */
+ FOC_EXAMPLE_STATE_FREE = 1, /* No current */
+ FOC_EXAMPLE_STATE_STOP = 2, /* Active break */
+ FOC_EXAMPLE_STATE_CW = 3, /* CW direction */
+ FOC_EXAMPLE_STATE_CCW = 4, /* CCW direction */
+};
+
+/* Operation modes */
+
+enum foc_operation_mode_e
+{
+ FOC_OPMODE_INVALID = 0, /* Reserved */
+ FOC_OPMODE_IDLE = 1, /* IDLE */
+ FOC_OPMODE_OL_V_VEL = 2, /* Voltage open-loop velocity controller */
+ FOC_OPMODE_OL_C_VEL = 3, /* Current open-loop velocity controller */
+
+ /* Not supported yet */
+
+#if 0
+ FOC_OPMODE_CL_C_TRQ = 3, /* Current closed-loop torque controller */
+ FOC_OPMODE_CL_C_VEL = 4, /* Current closed-loop velocity controller */
+ FOC_OPMODE_CL_C_POS = 5 /* Current closed-loop position controller */
+#endif
+};
+
+/* FOC thread data */
+
+struct foc_ctrl_env_s
+{
+ struct foc_device_s dev; /* FOC device */
+ mqd_t mqd; /* Control msg queue */
+ int id; /* FOC device id */
+ int inst; /* Type specific instance counter */
+ int type; /* Controller type */
+ int qparam; /* Open-loop Q setting (x1000) */
+ int mode; /* Operation mode */
+ uint32_t pi_kp; /* FOC PI Kp (x1000) */
+ uint32_t pi_ki; /* FOC PI Ki (x1000) */
+ uint32_t velmax; /* Velocity max (x1000) */
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+#ifdef CONFIG_INDUSTRY_FOC_FLOAT
+int foc_float_thr(FAR struct foc_ctrl_env_s *envp);
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_FIXED16
+int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp);
+#endif
+
+#endif /* __EXAMPLES_FOC_FOC_THR_H */