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 foc_mq_handle(mqd_t mq, FAR struct foc_mq_s *h)
{ {
int ret = OK; int ret = OK;
uint8_t buffer[5]; uint8_t buffer[CONTROL_MQ_MSGSIZE];
/* Get data from AUX */ /* 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 (ret < 0)
{ {
if (errno != EAGAIN) if (errno != EAGAIN)
@ -65,7 +65,7 @@ int foc_mq_handle(mqd_t mq, FAR struct foc_mq_s *h)
/* Verify message length */ /* Verify message length */
if (ret != 5) if (ret != CONTROL_MQ_MSGSIZE)
{ {
PRINTF("ERROR: invalid message length = %d\n", ret); PRINTF("ERROR: invalid message length = %d\n", ret);
goto errout; 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) static FAR void *foc_control_thr(FAR void *arg)
{ {
FAR struct foc_ctrl_env_s *envp = (FAR struct foc_ctrl_env_s *) arg; FAR struct foc_ctrl_env_s *envp = (FAR struct foc_ctrl_env_s *) arg;
char buffer[CONTROL_MQ_MSGSIZE];
char mqname[10]; char mqname[10];
int ret = OK; int ret = OK;
@ -116,6 +117,26 @@ static FAR void *foc_control_thr(FAR void *arg)
goto errout; 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 */ /* Select control logic according to FOC device type */
switch (envp->type) switch (envp->type)