From 55b296a5c6135b09a63ca1ac6e8bc00f7836f171 Mon Sep 17 00:00:00 2001
From: raiden00pl <raiden00@railab.me>
Date: Wed, 17 May 2023 12:31:07 +0200
Subject: [PATCH] examples/foc/foc_thr: get controller type from controller ID

---
 examples/foc/foc_thr.c | 56 ++++++++++++++++++++++++++++--------------
 examples/foc/foc_thr.h |  1 +
 2 files changed, 39 insertions(+), 18 deletions(-)

diff --git a/examples/foc/foc_thr.c b/examples/foc/foc_thr.c
index 83b39ce5a..a84efc2a7 100644
--- a/examples/foc/foc_thr.c
+++ b/examples/foc/foc_thr.c
@@ -35,6 +35,17 @@
 
 #include "industry/foc/foc_common.h"
 
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#ifndef CONFIG_EXAMPLES_FOC_FLOAT_INST
+#  define CONFIG_EXAMPLES_FOC_FLOAT_INST   (0)
+#endif
+#ifndef CONFIG_EXAMPLES_FOC_FIXED16_INST
+#  define CONFIG_EXAMPLES_FOC_FIXED16_INST (0)
+#endif
+
 /****************************************************************************
  * Extern Functions Prototypes
  ****************************************************************************/
@@ -80,30 +91,14 @@ static FAR void *foc_control_thr(FAR void *arg)
 
   /* Get controller type */
 
-  pthread_mutex_lock(&g_cntr_lock);
-
-#ifdef CONFIG_INDUSTRY_FOC_FLOAT
-  if (g_float_thr_cntr < CONFIG_EXAMPLES_FOC_FLOAT_INST)
-    {
-      envp->type = FOC_NUMBER_TYPE_FLOAT;
-    }
-  else
-#endif
-#ifdef CONFIG_INDUSTRY_FOC_FIXED16
-  if (g_fixed16_thr_cntr < CONFIG_EXAMPLES_FOC_FIXED16_INST)
-    {
-      envp->type = FOC_NUMBER_TYPE_FIXED16;
-    }
-  else
-#endif
+  envp->type = foc_thread_type(envp->id);
+  if (envp->type == -1)
     {
       /* Invalid configuration */
 
       ASSERT(0);
     }
 
-  pthread_mutex_unlock(&g_cntr_lock);
-
   PRINTF("FOC device %d type = %d!\n", envp->id, envp->type);
 
   /* Get queue name */
@@ -283,6 +278,31 @@ uint32_t foc_threads_get(void)
   return ret;
 }
 
+/****************************************************************************
+ * Name: foc_thread_type
+ ****************************************************************************/
+
+int foc_thread_type(int id)
+{
+  int ret = -1;
+
+#ifdef CONFIG_INDUSTRY_FOC_FLOAT
+  if (id < CONFIG_EXAMPLES_FOC_FLOAT_INST)
+    {
+      ret = FOC_NUMBER_TYPE_FLOAT;
+    }
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_FIXED16
+  if (id < CONFIG_EXAMPLES_FOC_FLOAT_INST + CONFIG_EXAMPLES_FOC_FIXED16_INST)
+    {
+      ret = FOC_NUMBER_TYPE_FIXED16;
+    }
+#endif
+
+  return ret;
+}
+
 /****************************************************************************
  * Name: foc_ctrlthr_init
  ****************************************************************************/
diff --git a/examples/foc/foc_thr.h b/examples/foc/foc_thr.h
index 3c6d1fb9a..e3d8f1b41 100644
--- a/examples/foc/foc_thr.h
+++ b/examples/foc/foc_thr.h
@@ -126,6 +126,7 @@ int foc_threads_init(void);
 void foc_threads_deinit(void);
 bool foc_threads_terminated(void);
 uint32_t foc_threads_get(void);
+int foc_thread_type(int id);
 int foc_ctrlthr_init(FAR struct foc_ctrl_env_s *foc, int i, FAR mqd_t *mqd,
                      FAR pthread_t *thread);