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

Author: raiden00pl

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;

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)