From f987c6c02ee12a0b98787d9a69c8fc14be638c2a Mon Sep 17 00:00:00 2001
From: raiden00pl <raiden00@railab.me>
Date: Sat, 27 Aug 2022 12:05:37 +0200
Subject: [PATCH] examples/foc: make sure that the queue is empty before start
 the control thread

---
 examples/foc/foc_mq.c  |  6 +++---
 examples/foc/foc_thr.c | 21 +++++++++++++++++++++
 2 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/examples/foc/foc_mq.c b/examples/foc/foc_mq.c
index e33617034..aa32c4ed4 100644
--- a/examples/foc/foc_mq.c
+++ b/examples/foc/foc_mq.c
@@ -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;
diff --git a/examples/foc/foc_thr.c b/examples/foc/foc_thr.c
index aee4b8bf8..c10b2606a 100644
--- a/examples/foc/foc_thr.c
+++ b/examples/foc/foc_thr.c
@@ -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)