/**************************************************************************** * 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 buffer[CONTROL_MQ_MSGSIZE]; 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; } /* Make sure that the queue is empty */ while (1) { ret = mq_receive(envp->mqd, buffer, CONTROL_MQ_MSGSIZE, 0); if (ret < 0) { if (errno == EAGAIN) { break; } else { PRINTF("ERROR: mq_receive failed errno=%d\n", errno); ret = -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; }