From ea27aacbd227b54b5d2201f166f4d7b43f8abd2b Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sun, 5 Dec 2021 15:57:42 +0100 Subject: [PATCH] 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(-) create mode 100644 examples/foc/foc_thr.c diff --git a/examples/foc/Makefile b/examples/foc/Makefile index 1fb157be4..6b2b79198 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 134774cc8..fd1deec39 100644 --- a/examples/foc/foc_main.c +++ b/examples/foc/foc_main.c @@ -28,22 +28,16 @@ #include #include #include -#include #include #include -#include -#include #include -#include #include -#include #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 @@ -83,40 +77,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 ****************************************************************************/ @@ -335,218 +295,6 @@ static int foc_kill_send(mqd_t mqd) 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); - - /* 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 000000000..aee4b8bf8 --- /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 + +#include +#include +#include + +#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 c33dfefc3..aaa8ae5dd 100644 --- a/examples/foc/foc_thr.h +++ b/examples/foc/foc_thr.h @@ -27,10 +27,11 @@ #include -#include - +#include #include +#include + #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 */