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/12/07 13:52:57 UTC
[incubator-nuttx-apps] 03/04: examples/foc: move threads related logic to a separate file
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
commit ea27aacbd227b54b5d2201f166f4d7b43f8abd2b
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sun Dec 5 15:57:42 2021 +0100
examples/foc: move threads related logic to a separate file
---
examples/foc/Makefile | 2 +-
examples/foc/foc_main.c | 268 ++---------------------------------------
examples/foc/foc_thr.c | 312 ++++++++++++++++++++++++++++++++++++++++++++++++
examples/foc/foc_thr.h | 17 ++-
4 files changed, 328 insertions(+), 271 deletions(-)
diff --git a/examples/foc/Makefile b/examples/foc/Makefile
index 1fb157b..6b2b791 100644
--- a/examples/foc/Makefile
+++ b/examples/foc/Makefile
@@ -32,7 +32,7 @@ MODULE = $(CONFIG_EXAMPLES_FOC)
MAINSRC = foc_main.c
ASRCS =
-CSRCS = foc_device.c foc_mq.c
+CSRCS = foc_device.c foc_mq.c foc_thr.c
ifeq ($(CONFIG_BUILTIN),y)
CSRCS += foc_parseargs.c
diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c
index 134774c..fd1deec 100644
--- a/examples/foc/foc_main.c
+++ b/examples/foc/foc_main.c
@@ -28,22 +28,16 @@
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
-#include <fcntl.h>
#include <assert.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_cfg.h"
#include "foc_debug.h"
-#include "foc_device.h"
#include "foc_parseargs.h"
#ifdef CONFIG_EXAMPLES_FOC_HAVE_BUTTON
@@ -84,40 +78,6 @@
#define INST_EN_DEAFULT (0xff)
/****************************************************************************
- * Private Type Definition
- ****************************************************************************/
-
-/****************************************************************************
- * 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
****************************************************************************/
@@ -336,218 +296,6 @@ static int foc_kill_send(mqd_t mqd)
}
/****************************************************************************
- * 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);
-
- /* 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 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_terminated
- ****************************************************************************/
-
-static bool foc_threads_terminated(void)
-{
- bool ret = false;
-
- pthread_mutex_unlock(&g_cntr_lock);
-
- if (1
-#ifdef CONFIG_INDUSTRY_FOC_FLOAT
- && g_float_thr_cntr <= 0
-#endif
-#ifdef CONFIG_INDUSTRY_FOC_FIXED16
- && g_fixed16_thr_cntr <= 0
-#endif
- )
- {
- ret = true;
- }
-
- pthread_mutex_lock(&g_cntr_lock);
-
- return ret;
-}
-
-/****************************************************************************
- * 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
****************************************************************************/
@@ -627,12 +375,12 @@ int main(int argc, char *argv[])
PRINTF("\nStart foc_main application!\n\n");
- /* Initialize mutex */
+ /* Initialize threads */
- ret = pthread_mutex_init(&g_cntr_lock, NULL);
- if (ret != 0)
+ ret = foc_threads_init();
+ if (ret < 0)
{
- PRINTF("ERROR: pthread_mutex_init failed %d\n", errno);
+ PRINTF("ERROR: failed to initialize threads %d\n", ret);
goto errout_no_mutex;
}
@@ -689,10 +437,10 @@ int main(int argc, char *argv[])
{
/* Initialize controller thread if enabled */
- ret = foc_threads_init(&foc[i], i, &mqd[i], &threads[i]);
+ ret = foc_ctrlthr_init(&foc[i], i, &mqd[i], &threads[i]);
if (ret < 0)
{
- PRINTF("ERROR: foc_threads_init failed %d!\n", ret);
+ PRINTF("ERROR: foc_ctrlthr_init failed %d!\n", ret);
goto errout;
}
}
@@ -1010,9 +758,7 @@ errout:
errout_no_mutex:
- /* Free/uninitialize data structures */
-
- pthread_mutex_destroy(&g_cntr_lock);
+ foc_threads_deinit();
PRINTF("foc_main exit\n");
diff --git a/examples/foc/foc_thr.c b/examples/foc/foc_thr.c
new file mode 100644
index 0000000..aee4b8b
--- /dev/null
+++ b/examples/foc/foc_thr.c
@@ -0,0 +1,312 @@
+/****************************************************************************
+ * apps/examples/foc/foc_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 <fcntl.h>
+#include <assert.h>
+#include <errno.h>
+
+#include "foc_mq.h"
+#include "foc_thr.h"
+#include "foc_debug.h"
+
+#include "industry/foc/foc_common.h"
+
+/****************************************************************************
+ * Extern Functions Prototypes
+ ****************************************************************************/
+
+#ifdef CONFIG_INDUSTRY_FOC_FLOAT
+extern int foc_float_thr(FAR struct foc_ctrl_env_s *envp);
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_FIXED16
+extern int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp);
+#endif
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+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
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_control_thr
+ ****************************************************************************/
+
+static 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);
+
+ /* 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 queue */
+
+ if (envp->mqd == (mqd_t)-1)
+ {
+ mq_close(envp->mqd);
+ }
+
+ PRINTFV("foc_control_thr %d exit\n", envp->id);
+
+ return NULL;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_threads_init
+ ****************************************************************************/
+
+int foc_threads_init(void)
+{
+ int ret = OK;
+
+ /* Initialize mutex */
+
+ ret = pthread_mutex_init(&g_cntr_lock, NULL);
+ if (ret != 0)
+ {
+ PRINTF("ERROR: pthread_mutex_init failed %d\n", errno);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_threads_deinit
+ ****************************************************************************/
+
+void foc_threads_deinit(void)
+{
+ /* Free/uninitialize data structures */
+
+ pthread_mutex_destroy(&g_cntr_lock);
+}
+
+/****************************************************************************
+ * Name: foc_threads_terminated
+ ****************************************************************************/
+
+bool foc_threads_terminated(void)
+{
+ bool ret = false;
+
+ pthread_mutex_unlock(&g_cntr_lock);
+
+ if (1
+#ifdef CONFIG_INDUSTRY_FOC_FLOAT
+ && g_float_thr_cntr <= 0
+#endif
+#ifdef CONFIG_INDUSTRY_FOC_FIXED16
+ && g_fixed16_thr_cntr <= 0
+#endif
+ )
+ {
+ ret = true;
+ }
+
+ pthread_mutex_lock(&g_cntr_lock);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_ctrlthr_init
+ ****************************************************************************/
+
+int foc_ctrlthr_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;
+}
diff --git a/examples/foc/foc_thr.h b/examples/foc/foc_thr.h
index c33dfef..aaa8ae5 100644
--- a/examples/foc/foc_thr.h
+++ b/examples/foc/foc_thr.h
@@ -27,10 +27,11 @@
#include <nuttx/config.h>
-#include <nuttx/motor/foc/foc.h>
-
+#include <pthread.h>
#include <mqueue.h>
+#include <nuttx/motor/foc/foc.h>
+
#include "foc_device.h"
/****************************************************************************
@@ -122,12 +123,10 @@ struct foc_ctrl_env_s
* 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
+int foc_threads_init(void);
+void foc_threads_deinit(void);
+bool foc_threads_terminated(void);
+int foc_ctrlthr_init(FAR struct foc_ctrl_env_s *foc, int i, FAR mqd_t *mqd,
+ FAR pthread_t *thread);
#endif /* __EXAMPLES_FOC_FOC_THR_H */