examples/foc: send messages only to active control threads

examples/foc/foc_main.c

@@ -205,6 +205,7 @@ int main(int argc, char *argv[])
   pthread_t              threads[CONFIG_MOTOR_FOC_INST];
   mqd_t                  mqd[CONFIG_MOTOR_FOC_INST];
   struct foc_intf_data_s data;
+  uint32_t               thrs_active  = 0;
   int                    ret          = OK;
   int                    i            = 0;
   int                    time         = 0;
@@ -310,6 +311,10 @@ int main(int argc, char *argv[])
     {
       PRINTFV("foc_main loop %d\n", time);
 
+      /* Get active control threads */
+
+      thrs_active = foc_threads_get();
+
       /* Update control interface */
 
       ret = foc_intf_update(&data);
@@ -325,7 +330,7 @@ int main(int argc, char *argv[])
         {
           for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
             {
-              if (g_args.en & (1 << i))
+              if ((g_args.en & (1 << i)) && (thrs_active & (1 << i)))
                 {
                   PRINTFV("Send vbus to %d\n", i);
 
@@ -351,7 +356,7 @@ int main(int argc, char *argv[])
         {
           for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
             {
-              if (g_args.en & (1 << i))
+              if ((g_args.en & (1 << i)) && (thrs_active & (1 << i)))
                 {
                   PRINTFV("Send state %" PRIu32 " to %d\n", data.state, i);
 
@@ -377,7 +382,7 @@ int main(int argc, char *argv[])
         {
           for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
             {
-              if (g_args.en & (1 << i))
+              if ((g_args.en & (1 << i)) && (thrs_active & (1 << i)))
                 {
                   PRINTFV("Send setpoint = %" PRIu32 "to %d\n",
                           data.sp_raw, i);
@@ -404,7 +409,7 @@ int main(int argc, char *argv[])
         {
           for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
             {
-              if (g_args.en & (1 << i))
+              if ((g_args.en & (1 << i)) && (thrs_active & (1 << i)))
                 {
                   PRINTFV("Send start to %d\n", i);
 
@@ -464,7 +469,11 @@ errout_no_intf:
 
   for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
     {
-      if (g_args.en & (1 << i))
+      /* Only for active threads */
+
+      thrs_active = foc_threads_get();
+
+      if ((g_args.en & (1 << i)) && (thrs_active & (1 << i)))
         {
           if (mqd[i] != (mqd_t)-1)
             {

examples/foc/foc_thr.c

@@ -53,6 +53,7 @@ extern int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp);
 
 pthread_mutex_t g_cntr_lock;
 
+static uint32_t g_foc_thr = 0;
 #ifdef CONFIG_INDUSTRY_FOC_FLOAT
 static int g_float_thr_cntr = 0;
 #endif
@@ -148,6 +149,7 @@ static FAR void *foc_control_thr(FAR void *arg)
           pthread_mutex_lock(&g_cntr_lock);
           envp->inst = g_float_thr_cntr;
           g_float_thr_cntr += 1;
+          g_foc_thr |= (1 << envp->id);
           pthread_mutex_unlock(&g_cntr_lock);
 
           /* Start thread */
@@ -156,6 +158,7 @@ static FAR void *foc_control_thr(FAR void *arg)
 
           pthread_mutex_lock(&g_cntr_lock);
           g_float_thr_cntr -= 1;
+          g_foc_thr &= ~(1 << envp->id);
           pthread_mutex_unlock(&g_cntr_lock);
 
           break;
@@ -168,6 +171,7 @@ static FAR void *foc_control_thr(FAR void *arg)
           pthread_mutex_lock(&g_cntr_lock);
           envp->inst = g_fixed16_thr_cntr;
           g_fixed16_thr_cntr += 1;
+          g_foc_thr |= (1 << envp->id);
           pthread_mutex_unlock(&g_cntr_lock);
 
           /* Start thread */
@@ -176,6 +180,7 @@ static FAR void *foc_control_thr(FAR void *arg)
 
           pthread_mutex_lock(&g_cntr_lock);
           g_fixed16_thr_cntr -= 1;
+          g_foc_thr &= ~(1 << envp->id);
           pthread_mutex_unlock(&g_cntr_lock);
 
           break;
@@ -253,14 +258,7 @@ bool foc_threads_terminated(void)
 
   pthread_mutex_unlock(&g_cntr_lock);
 
-  if (1
+  if (g_foc_thr == 0)
-#ifdef CONFIG_INDUSTRY_FOC_FLOAT
-      && g_float_thr_cntr <= 0
-#endif
-#ifdef CONFIG_INDUSTRY_FOC_FIXED16
-      && g_fixed16_thr_cntr <= 0
-#endif
-    )
     {
       ret = true;
     }
@@ -270,6 +268,21 @@ bool foc_threads_terminated(void)
   return ret;
 }
 
+/****************************************************************************
+ * 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;
+}
+
 /****************************************************************************
  * Name: foc_ctrlthr_init
  ****************************************************************************/

examples/foc/foc_thr.h

@@ -27,6 +27,7 @@
 
 #include <nuttx/config.h>
 
+#include <stdint.h>
 #include <pthread.h>
 #include <mqueue.h>
 
@@ -124,6 +125,7 @@ struct foc_ctrl_env_s
 int foc_threads_init(void);
 void foc_threads_deinit(void);
 bool foc_threads_terminated(void);
+uint32_t foc_threads_get(void);
 int foc_ctrlthr_init(FAR struct foc_ctrl_env_s *foc, int i, FAR mqd_t *mqd,
                      FAR pthread_t *thread);