2021-12-05 15:57:42 +01:00
|
|
|
/****************************************************************************
|
|
|
|
* 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 <assert.h>
|
|
|
|
#include <errno.h>
|
2023-03-08 19:57:51 +01:00
|
|
|
#include <fcntl.h>
|
|
|
|
#include <sched.h>
|
2021-12-05 15:57:42 +01:00
|
|
|
|
|
|
|
#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;
|
|
|
|
|
2023-05-05 13:15:36 +02:00
|
|
|
static uint32_t g_foc_thr = 0;
|
2021-12-05 15:57:42 +01:00
|
|
|
#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)
|
|
|
|
{
|
2022-10-08 10:31:21 +02:00
|
|
|
FAR struct foc_ctrl_env_s *envp = (FAR struct foc_ctrl_env_s *)arg;
|
2022-08-27 12:05:37 +02:00
|
|
|
char buffer[CONTROL_MQ_MSGSIZE];
|
2021-12-05 15:57:42 +01:00
|
|
|
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 */
|
|
|
|
|
2023-03-05 16:55:15 +01:00
|
|
|
snprintf(mqname, sizeof(mqname), "%s%d", CONTROL_MQ_MQNAME, envp->id);
|
2021-12-05 15:57:42 +01:00
|
|
|
|
|
|
|
/* 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;
|
|
|
|
}
|
|
|
|
|
2022-08-27 12:05:37 +02:00
|
|
|
/* 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;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2021-12-05 15:57:42 +01:00
|
|
|
/* 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;
|
2023-05-05 13:15:36 +02:00
|
|
|
g_foc_thr |= (1 << envp->id);
|
2021-12-05 15:57:42 +01:00
|
|
|
pthread_mutex_unlock(&g_cntr_lock);
|
|
|
|
|
|
|
|
/* Start thread */
|
|
|
|
|
|
|
|
ret = foc_float_thr(envp);
|
|
|
|
|
|
|
|
pthread_mutex_lock(&g_cntr_lock);
|
|
|
|
g_float_thr_cntr -= 1;
|
2023-05-05 13:15:36 +02:00
|
|
|
g_foc_thr &= ~(1 << envp->id);
|
2021-12-05 15:57:42 +01:00
|
|
|
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;
|
2023-05-05 13:15:36 +02:00
|
|
|
g_foc_thr |= (1 << envp->id);
|
2021-12-05 15:57:42 +01:00
|
|
|
pthread_mutex_unlock(&g_cntr_lock);
|
|
|
|
|
|
|
|
/* Start thread */
|
|
|
|
|
|
|
|
ret = foc_fixed16_thr(envp);
|
|
|
|
|
|
|
|
pthread_mutex_lock(&g_cntr_lock);
|
|
|
|
g_fixed16_thr_cntr -= 1;
|
2023-05-05 13:15:36 +02:00
|
|
|
g_foc_thr &= ~(1 << envp->id);
|
2021-12-05 15:57:42 +01:00
|
|
|
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);
|
|
|
|
|
2023-05-05 13:15:36 +02:00
|
|
|
if (g_foc_thr == 0)
|
2021-12-05 15:57:42 +01:00
|
|
|
{
|
|
|
|
ret = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
pthread_mutex_lock(&g_cntr_lock);
|
|
|
|
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
2023-05-05 13:15:36 +02:00
|
|
|
/****************************************************************************
|
|
|
|
* Name: foc_threads_get
|
|
|
|
****************************************************************************/
|
|
|
|
|
|
|
|
uint32_t foc_threads_get(void)
|
|
|
|
{
|
|
|
|
uint32_t ret = 0;
|
|
|
|
|
|
|
|
pthread_mutex_unlock(&g_cntr_lock);
|
|
|
|
ret = g_foc_thr;
|
|
|
|
pthread_mutex_lock(&g_cntr_lock);
|
|
|
|
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
2021-12-05 15:57:42 +01:00
|
|
|
/****************************************************************************
|
|
|
|
* 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)
|
|
|
|
{
|
2022-10-08 10:31:21 +02:00
|
|
|
char mqname[10];
|
|
|
|
int ret = OK;
|
|
|
|
pthread_attr_t attr;
|
|
|
|
struct mq_attr mqattr;
|
|
|
|
struct sched_param param;
|
2021-12-05 15:57:42 +01:00
|
|
|
|
|
|
|
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 */
|
|
|
|
|
2023-03-05 16:55:15 +01:00
|
|
|
snprintf(mqname, sizeof(mqname), "%s%d", CONTROL_MQ_MQNAME, foc->id);
|
2021-12-05 15:57:42 +01:00
|
|
|
|
|
|
|
/* 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;
|
|
|
|
}
|