sergiotarxz/nuttx-apps
sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

examples/foc: make sure that the queue is empty before start the control thread

Browse Source
This commit is contained in:
raiden00pl 2022-08-27 12:05:37 +02:00 committed by Xiang Xiao
parent 429f5a166f
commit f987c6c02e
2 changed files with 24 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
examples/foc/foc_mq.c
View File

@ -41,11 +41,11 @@
int foc_mq_handle(mqd_t mq, FAR struct foc_mq_s *h)
{
int ret = OK;
uint8_t buffer[5];
uint8_t buffer[CONTROL_MQ_MSGSIZE];
/* Get data from AUX */
ret = mq_receive(mq, (char *)buffer, 5, 0);
ret = mq_receive(mq, (char *)buffer, CONTROL_MQ_MSGSIZE, 0);
if (ret < 0)
{
if (errno != EAGAIN)
@ -65,7 +65,7 @@ int foc_mq_handle(mqd_t mq, FAR struct foc_mq_s *h)
/* Verify message length */
if (ret != 5)
if (ret != CONTROL_MQ_MSGSIZE)
{
PRINTF("ERROR: invalid message length = %d\n", ret);
goto errout;

21
examples/foc/foc_thr.c
View File

@ -70,6 +70,7 @@ static int g_fixed16_thr_cntr = 0;
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;
@ -116,6 +117,26 @@ static FAR void *foc_control_thr(FAR void *arg)
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)