You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by xi...@apache.org on 2023/05/19 12:18:43 UTC
[nuttx-apps] branch master updated (e15db0c21 -> 7e5a436ff)
This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a change to branch master
in repository https://gitbox.apache.org/repos/asf/nuttx-apps.git
from e15db0c21 crypto/tinycrypt/Kconfig: Fix indentation
new 54bcf3afd examples/foc/foc_thr: fix mutex logic
new 55b296a5c examples/foc/foc_thr: get controller type from controller ID
new 7e5a436ff examples/foc: add real time data capture with the NxScope library
The 3 revisions listed above as "new" are entirely new to this
repository and will be described in separate emails. The revisions
listed as "add" were already present in the repository and have only
been added to this reference.
Summary of changes:
examples/foc/Kconfig | 87 +++++++
examples/foc/Makefile | 6 +
examples/foc/foc_fixed16_thr.c | 98 ++++++++
examples/foc/foc_float_thr.c | 98 ++++++++
examples/foc/foc_main.c | 104 +++++++-
examples/foc/foc_nxscope.c | 313 ++++++++++++++++++++++++
examples/foc/{foc_parseargs.h => foc_nxscope.h} | 59 +++--
examples/foc/foc_thr.c | 64 +++--
examples/foc/foc_thr.h | 18 +-
9 files changed, 788 insertions(+), 59 deletions(-)
create mode 100644 examples/foc/foc_nxscope.c
copy examples/foc/{foc_parseargs.h => foc_nxscope.h} (54%)
[nuttx-apps] 02/03: examples/foc/foc_thr: get controller type from controller ID
Posted by xi...@apache.org.
This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/nuttx-apps.git
commit 55b296a5c6135b09a63ca1ac6e8bc00f7836f171
Author: raiden00pl <ra...@railab.me>
AuthorDate: Wed May 17 12:31:07 2023 +0200
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);
[nuttx-apps] 03/03: examples/foc: add real time data capture with the NxScope library
Posted by xi...@apache.org.
This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/nuttx-apps.git
commit 7e5a436ff9a04ca77b8c4140e866f013a4da6776
Author: raiden00pl <ra...@railab.me>
AuthorDate: Wed May 17 12:31:25 2023 +0200
examples/foc: add real time data capture with the NxScope library
---
examples/foc/Kconfig | 87 ++++++++++++
examples/foc/Makefile | 6 +
examples/foc/foc_fixed16_thr.c | 98 +++++++++++++
examples/foc/foc_float_thr.c | 98 +++++++++++++
examples/foc/foc_main.c | 104 ++++++++++++--
examples/foc/foc_nxscope.c | 313 +++++++++++++++++++++++++++++++++++++++++
examples/foc/foc_nxscope.h | 92 ++++++++++++
examples/foc/foc_thr.h | 17 ++-
8 files changed, 797 insertions(+), 18 deletions(-)
diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
index 70aedd380..bb9dc9422 100644
--- a/examples/foc/Kconfig
+++ b/examples/foc/Kconfig
@@ -421,5 +421,92 @@ config EXAMPLES_FOC_RUN_DISABLE
bool "FOC Disable FOC motor controller"
default n
+config EXAMPLES_FOC_NXSCOPE
+ bool "FOC nxscope support"
+ depends on LOGGING_NXSCOPE
+ select LOGGING_NXSCOPE_DISABLE_PUTLOCK
+ default n
+ ---help---
+ This option enables a controller real-time data capture with
+ the NxScope library.
+
+if EXAMPLES_FOC_NXSCOPE
+
+config EXAMPLES_FOC_NXSCOPE_SERIAL
+ bool "FOC nxscope on serial port"
+ select LOGGING_NXSCOPE_INTF_SERIAL
+ default y
+
+if EXAMPLES_FOC_NXSCOPE_SERIAL
+
+config EXAMPLES_FOC_NXSCOPE_SERIAL_PATH
+ string "FOC nxscope serial path"
+ default "/dev/ttyS0"
+
+config EXAMPLES_FOC_NXSCOPE_SERIAL_BAUD
+ int "FOC nxscope serial baud"
+ default 115200
+
+endif # EXAMPLES_FOC_NXSCOPE_SERIAL
+
+config EXAMPLES_FOC_NXSCOPE_STREAMBUF_LEN
+ int "FOC nxscope stream buffer length"
+ default 512
+
+config EXAMPLES_FOC_NXSCOPE_RXBUF_LEN
+ int "FOC nxscope RX buffer length"
+ default 64
+
+config EXAMPLES_FOC_NXSCOPE_RXPADDING
+ int "FOC nxscope RX padding"
+ default 0
+
+config EXAMPLES_FOC_NXSCOPE_CHANNELS
+ int "FOC nxscope channels"
+ default 0
+
+config EXAMPLES_FOC_NXSCOPE_PRESCALER
+ int "FOC nxscope prescaler"
+ default 1
+ ---help---
+ This option allows you to reduce the frequency of adding samples to
+ the NxScope buffer.
+
+config EXAMPLES_FOC_NXSCOPE_CFG
+ hex "FOC nxscope configuration"
+ default 0x00000000
+ ---help---
+ Each bit defines the controller state variable that can be captured.
+ Look at foc_nxscope.h for bits definitions.
+
+config EXAMPLES_FOC_NXSCOPE_START
+ bool "FOC nxscope start frame sync"
+ default n
+ ---help---
+ If this option is set, the controller will be waiting for the start
+ frame from a NxScope master device. This allows us to capture
+ controller data from the very beginning of its operation.
+
+config EXAMPLES_FOC_NXSCOPE_THREAD
+ bool "FOC nxscope uses separate thread"
+ default n
+ ---help---
+ Use a separate thread for NxScope communication.
+
+if EXAMPLES_FOC_NXSCOPE_THREAD
+
+config EXAMPLES_FOC_NXSCOPE_PRIO
+ int "FOC nxscope thread priority"
+ default 100
+
+config EXAMPLES_FOC_NXSCOPE_STACKSIZE
+ int "FOC nxscope thread stack size"
+ default 2048
+
+endif # EXAMPLES_FOC_NXSCOPE_THREAD
+
+endif # EXAMPLES_FOC_NXSCOPE
+
endif # EXAMPLES_FOC
+
diff --git a/examples/foc/Makefile b/examples/foc/Makefile
index 68b0d61b2..51f603577 100644
--- a/examples/foc/Makefile
+++ b/examples/foc/Makefile
@@ -50,4 +50,10 @@ ifeq ($(CONFIG_INDUSTRY_FOC_FLOAT),y)
CSRCS += foc_float_thr.c foc_motor_f32.c
endif
+# NxScope support
+
+ifeq ($(CONFIG_EXAMPLES_FOC_NXSCOPE),y)
+ CSRCS += foc_nxscope.c
+endif
+
include $(APPDIR)/Application.mk
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
index 5735adf29..87dc36ea1 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_fixed16_thr.c
@@ -38,6 +38,10 @@
#include "industry/foc/foc_utils.h"
#include "industry/foc/foc_common.h"
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
+# include "logging/nxscope/nxscope.h"
+#endif
+
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
@@ -173,6 +177,91 @@ static int foc_state_print(FAR struct foc_motor_b16_s *motor)
}
#endif
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
+/****************************************************************************
+ * Name: foc_fixed16_nxscope
+ ****************************************************************************/
+
+static void foc_fixed16_nxscope(FAR struct foc_nxscope_s *nxs,
+ FAR struct foc_motor_b16_s *motor,
+ FAR struct foc_device_s *dev)
+{
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG != 0)
+ FAR b16_t *ptr = NULL;
+ int i = nxs->ch_per_inst * motor->envp->id;
+#endif
+
+ nxscope_lock(&nxs->nxs);
+
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IABC)
+ ptr = (FAR b16_t *)&motor->foc_state.curr;
+ nxscope_put_vb16(&nxs->nxs, i++, ptr, CONFIG_MOTOR_FOC_PHASES);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IDQ)
+ ptr = (FAR b16_t *)&motor->foc_state.idq;
+ nxscope_put_vb16(&nxs->nxs, i++, ptr, 2);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IAB)
+ ptr = (FAR b16_t *)&motor->foc_state.iab;
+ nxscope_put_vb16(&nxs->nxs, i++, ptr, 2);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VABC)
+ ptr = (FAR b16_t *)&motor->foc_state.volt;
+ nxscope_put_vb16(&nxs->nxs, i++, ptr, CONFIG_MOTOR_FOC_PHASES);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQ)
+ ptr = (FAR b16_t *)&motor->foc_state.vdq;
+ nxscope_put_vb16(&nxs->nxs, i++, ptr, 2);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VAB)
+ ptr = (FAR b16_t *)&motor->foc_state.vab;
+ nxscope_put_vb16(&nxs->nxs, i++, ptr, 2);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AEL)
+ ptr = (FAR b16_t *)&motor->angle_el;
+ nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AM)
+ ptr = (FAR b16_t *)&motor->angle_m;
+ nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VEL)
+# warning not supported yet
+ i++;
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM)
+# warning not supported yet
+ i++;
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VBUS)
+ ptr = (FAR b16_t *)&motor->vbus;
+ nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPTORQ)
+ ptr = (FAR b16_t *)&motor->torq;
+ nxscope_put_vb16(&nxs->nxs, i++, ptr, 3);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPVEL)
+ ptr = (FAR b16_t *)&motor->vel;
+ nxscope_put_vb16(&nxs->nxs, i++, ptr, 3);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPPOS)
+ ptr = (FAR b16_t *)&motor->pos;
+ nxscope_put_vb16(&nxs->nxs, i++, ptr, 3);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_DQREF)
+ ptr = (FAR b16_t *)&motor->dq_ref;
+ nxscope_put_vb16_t(&nxs->nxs, i++, ptr, 2);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQCOMP)
+ ptr = (FAR b16_t *)&motor->vdq_comp;
+ nxscope_put_vb16_t(&nxs->nxs, i++, ptr, 2);
+#endif
+
+ nxscope_unlock(&nxs->nxs);
+}
+#endif
+
/****************************************************************************
* Public Functions
****************************************************************************/
@@ -355,6 +444,15 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
goto errout;
}
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
+ /* Capture nxscope samples */
+
+ if (time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0)
+ {
+ foc_fixed16_nxscope(envp->nxs, &motor, &dev);
+ }
+#endif
+
/* Terminate control thread */
if (motor.ctrl_state == FOC_CTRL_STATE_TERMINATE)
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index 131d3c8e4..913162d84 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -38,6 +38,10 @@
#include "industry/foc/foc_utils.h"
#include "industry/foc/foc_common.h"
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
+# include "logging/nxscope/nxscope.h"
+#endif
+
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
@@ -174,6 +178,91 @@ static int foc_state_print(FAR struct foc_motor_f32_s *motor)
}
#endif
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
+/****************************************************************************
+ * Name: foc_float_nxscope
+ ****************************************************************************/
+
+static void foc_float_nxscope(FAR struct foc_nxscope_s *nxs,
+ FAR struct foc_motor_f32_s *motor,
+ FAR struct foc_device_s *dev)
+{
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG != 0)
+ FAR float *ptr = NULL;
+ int i = nxs->ch_per_inst * motor->envp->id;
+#endif
+
+ nxscope_lock(&nxs->nxs);
+
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IABC)
+ ptr = (FAR float *)&motor->foc_state.curr;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, CONFIG_MOTOR_FOC_PHASES);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IDQ)
+ ptr = (FAR float *)&motor->foc_state.idq;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IAB)
+ ptr = (FAR float *)&motor->foc_state.iab;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VABC)
+ ptr = (FAR float *)&motor->foc_state.volt;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, CONFIG_MOTOR_FOC_PHASES);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQ)
+ ptr = (FAR float *)&motor->foc_state.vdq;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VAB)
+ ptr = (FAR float *)&motor->foc_state.vab;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AEL)
+ ptr = (FAR float *)&motor->angle_el;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AM)
+ ptr = (FAR float *)&motor->angle_m;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VEL)
+# warning not supported yet
+ i++;
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM)
+# warning not supported yet
+ i++;
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VBUS)
+ ptr = (FAR float *)&motor->vbus;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPTORQ)
+ ptr = (FAR float *)&motor->torq;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, 3);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPVEL)
+ ptr = (FAR float *)&motor->vel;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, 3);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPPOS)
+ ptr = (FAR float *)&motor->pos;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, 3);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_DQREF)
+ ptr = (FAR float *)&motor->dq_ref;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQCOMP)
+ ptr = (FAR float *)&motor->vdq_comp;
+ nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2);
+#endif
+
+ nxscope_unlock(&nxs->nxs);
+}
+#endif
+
/****************************************************************************
* Public Functions
****************************************************************************/
@@ -339,6 +428,15 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
}
#endif
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
+ /* Capture nxscope samples */
+
+ if (time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0)
+ {
+ foc_float_nxscope(envp->nxs, &motor, &dev);
+ }
+#endif
+
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Feed FOC model with data */
diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c
index 3e8775616..f4d8f95d2 100644
--- a/examples/foc/foc_main.c
+++ b/examples/foc/foc_main.c
@@ -43,6 +43,10 @@
#include "industry/foc/foc_common.h"
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
+# include "foc_nxscope.h"
+#endif
+
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
@@ -59,6 +63,8 @@
* Private Data
****************************************************************************/
+/* Default configuration */
+
struct args_s g_args =
{
.time = CONFIG_EXAMPLES_FOC_TIME_DEFAULT,
@@ -106,6 +112,11 @@ struct args_s g_args =
}
};
+/* Start allowed at defaule */
+
+static bool g_start_allowed = true;
+static pthread_mutex_t g_start_allowed_lock = PTHREAD_MUTEX_INITIALIZER;
+
/****************************************************************************
* Private Functions
****************************************************************************/
@@ -191,6 +202,20 @@ static int foc_kill_send(mqd_t mqd)
return foc_mq_send(mqd, CONTROL_MQ_MSG_KILL, (FAR void *)&tmp);
}
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_START
+/****************************************************************************
+ * Name: foc_nxscope_cb_start
+ ****************************************************************************/
+
+static int foc_nxscope_cb_start(FAR void *priv, bool start)
+{
+ pthread_mutex_lock(&g_start_allowed_lock);
+ g_start_allowed = start;
+ pthread_mutex_unlock(&g_start_allowed_lock);
+ return OK;
+}
+#endif
+
/****************************************************************************
* Public Functions
****************************************************************************/
@@ -209,6 +234,10 @@ int main(int argc, char *argv[])
int ret = OK;
int i = 0;
int time = 0;
+ bool startallowed = false;
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
+ struct foc_nxscope_s nxs;
+#endif
/* Reset some data */
@@ -216,6 +245,9 @@ int main(int argc, char *argv[])
memset(foc, 0, sizeof(struct foc_ctrl_env_s) * CONFIG_MOTOR_FOC_INST);
memset(threads, 0, sizeof(pthread_t) * CONFIG_MOTOR_FOC_INST);
memset(&data, 0, sizeof(struct foc_intf_data_s));
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
+ memset(&nxs, 0, sizeof(struct foc_nxscope_s));
+#endif
#ifdef CONFIG_BUILTIN
/* Parse the command line */
@@ -229,7 +261,7 @@ int main(int argc, char *argv[])
if (ret < 0)
{
PRINTF("ERROR: validate args failed\n");
- goto errout_no_threads;
+ goto errout_no_nxscope;
}
#ifndef CONFIG_NSH_ARCHINIT
@@ -246,6 +278,28 @@ int main(int argc, char *argv[])
PRINTF("\nStart foc_main application!\n\n");
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
+
+# ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_START
+ /* Wait for nxscope */
+
+ g_start_allowed = false;
+
+ /* Connect start callback */
+
+ nxs.cb.start = foc_nxscope_cb_start;
+# endif
+
+ /* Initialize nxscope */
+
+ ret = foc_nxscope_init(&nxs);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: failed to initialize nxscope %d\n", ret);
+ goto errout_no_threads;
+ }
+#endif
+
/* Initialize threads */
ret = foc_threads_init();
@@ -271,6 +325,9 @@ int main(int argc, char *argv[])
/* Get configuration */
foc[i].cfg = &g_args.cfg;
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
+ foc[i].nxs = &nxs;
+#endif
if (g_args.en & (1 << i))
{
@@ -311,6 +368,11 @@ int main(int argc, char *argv[])
{
PRINTFV("foc_main loop %d\n", time);
+#if defined(CONFIG_EXAMPLES_FOC_NXSCOPE) && \
+ !defined(CONFIG_EXAMPLES_FOC_NXSCOPE_THREAD)
+ foc_nxscope_work(&nxs);
+#endif
+
/* Get active control threads */
thrs_active = foc_threads_get();
@@ -407,26 +469,35 @@ int main(int argc, char *argv[])
if (data.started == false)
{
- for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
+ /* Is start allowed now ? */
+
+ pthread_mutex_lock(&g_start_allowed_lock);
+ startallowed = g_start_allowed;
+ pthread_mutex_unlock(&g_start_allowed_lock);
+
+ if (startallowed)
{
- if ((g_args.en & (1 << i)) && (thrs_active & (1 << i)))
+ for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{
- PRINTFV("Send start to %d\n", i);
+ if ((g_args.en & (1 << i)) && (thrs_active & (1 << i)))
+ {
+ PRINTFV("Send start to %d\n", i);
- /* Send START to threads */
+ /* Send START to threads */
- ret = foc_start_send(mqd[i]);
- if (ret < 0)
- {
- PRINTF("ERROR: foc_start_send failed %d\n", ret);
- goto errout;
+ ret = foc_start_send(mqd[i]);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: foc_start_send failed %d\n", ret);
+ goto errout;
+ }
}
}
- }
- /* Set flag */
+ /* Set flag */
- data.started = true;
+ data.started = true;
+ }
}
/* Handle run time */
@@ -514,8 +585,15 @@ errout_no_intf:
foc_threads_deinit();
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
errout_no_threads:
+ /* De-initialize NxScope */
+
+ foc_nxscope_deinit(&nxs);
+#endif
+
+errout_no_nxscope:
PRINTF("foc_main exit\n");
return 0;
}
diff --git a/examples/foc/foc_nxscope.c b/examples/foc/foc_nxscope.c
new file mode 100644
index 000000000..fef6ae239
--- /dev/null
+++ b/examples/foc/foc_nxscope.c
@@ -0,0 +1,313 @@
+/****************************************************************************
+ * apps/examples/foc/foc_nxscope.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <errno.h>
+#include <unistd.h>
+#include <string.h>
+
+#include "foc_debug.h"
+#include "foc_nxscope.h"
+#include "foc_thr.h"
+
+#include "industry/foc/foc_common.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#ifndef CONFIG_LOGGING_NXSCOPE_DISABLE_PUTLOCK
+# error CONFIG_LOGGING_NXSCOPE_DISABLE_PUTLOCK must be set to proper operation.
+#endif
+
+#ifdef CONFIG_LOGGING_NXSCOPE_INTF_SERIAL
+# ifndef CONFIG_SERIAL_TERMIOS
+# error CONFIG_SERIAL_TERMIOS must be set to proper operation.
+# endif
+#endif
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_THREAD
+/****************************************************************************
+ * Name: foc_nxscope_thr
+ ****************************************************************************/
+
+static FAR void *foc_nxscope_thr(FAR void *arg)
+{
+ FAR struct foc_nxscope_s *nxs = (FAR struct foc_nxscope_s *)arg;
+
+ DEBUGASSERT(nxs);
+
+ while (1)
+ {
+ /* NxScope work */
+
+ foc_nxscope_work(nxs);
+
+ usleep(10000);
+ }
+
+ return NULL;
+}
+#endif
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_nxscope_init
+ ****************************************************************************/
+
+int foc_nxscope_init(FAR struct foc_nxscope_s *nxs)
+{
+ union nxscope_chinfo_type_u u;
+ struct nxscope_cfg_s nxs_cfg;
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_THREAD
+ struct sched_param param;
+ pthread_attr_t attr;
+ pthread_t nxsthr = 0;
+#endif
+ int ret = OK;
+ int i = 0;
+ int j = 0;
+
+ DEBUGASSERT(nxs);
+
+ /* Initialize serial interface */
+
+ nxs->ser_cfg.path = CONFIG_EXAMPLES_FOC_NXSCOPE_SERIAL_PATH;
+ nxs->ser_cfg.nonblock = true;
+ nxs->ser_cfg.baud = CONFIG_EXAMPLES_FOC_NXSCOPE_SERIAL_BAUD;
+
+ ret = nxscope_ser_init(&nxs->intf, &nxs->ser_cfg);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: nxscope_ser_init failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Default serial protocol */
+
+ ret = nxscope_proto_ser_init(&nxs->proto, NULL);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: nxscope_proto_ser_init failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Initialize nxscope */
+
+ nxs_cfg.intf_cmd = &nxs->intf;
+ nxs_cfg.proto_cmd = &nxs->proto;
+ nxs_cfg.intf_stream = &nxs->intf;
+ nxs_cfg.proto_stream = &nxs->proto;
+ nxs_cfg.channels = CONFIG_EXAMPLES_FOC_NXSCOPE_CHANNELS;
+ nxs_cfg.streambuf_len = CONFIG_EXAMPLES_FOC_NXSCOPE_STREAMBUF_LEN;
+ nxs_cfg.rxbuf_len = CONFIG_EXAMPLES_FOC_NXSCOPE_RXBUF_LEN;
+ nxs_cfg.rx_padding = CONFIG_EXAMPLES_FOC_NXSCOPE_RXPADDING;
+ nxs_cfg.callbacks = &nxs->cb;
+
+ ret = nxscope_init(&nxs->nxs, &nxs_cfg);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: nxscope_init failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Get channels per instance */
+
+ for (j = 0; j < CONFIG_MOTOR_FOC_INST; j += 1)
+ {
+ if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & (1 << j))
+ {
+ nxs->ch_per_inst += 1;
+ }
+ }
+
+ /* For all FOC controllers */
+
+ for (j = 0; j < CONFIG_MOTOR_FOC_INST; j += 1)
+ {
+ /* Get controller data type */
+
+ switch (foc_thread_type(j))
+ {
+#ifdef CONFIG_INDUSTRY_FOC_FLOAT
+ case FOC_NUMBER_TYPE_FLOAT:
+ {
+ u.s.dtype = NXSCOPE_TYPE_FLOAT;
+ break;
+ }
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_FIXED16
+ case FOC_NUMBER_TYPE_FIXED16:
+ {
+ u.s.dtype = NXSCOPE_TYPE_B16;
+ break;
+ }
+#endif
+
+ default:
+ {
+ ASSERT(0);
+ }
+ }
+
+ /* Create channels */
+
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG == 0)
+ UNUSED(u);
+#endif
+
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IABC)
+ nxscope_chan_init(&nxs->nxs, i++, "iabc", u.u8,
+ CONFIG_MOTOR_FOC_PHASES, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IDQ)
+ nxscope_chan_init(&nxs->nxs, i++, "idq", u.u8, 2, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IAB)
+ nxscope_chan_init(&nxs->nxs, i++, "iab", u.u8, 2, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VABC)
+ nxscope_chan_init(&nxs->nxs, i++, "vabc", u.u8,
+ CONFIG_MOTOR_FOC_PHASES, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQ)
+ nxscope_chan_init(&nxs->nxs, i++, "vdq", u.u8, 2, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VAB)
+ nxscope_chan_init(&nxs->nxs, i++, "vab", u.u8, 2, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AEL)
+ nxscope_chan_init(&nxs->nxs, i++, "a_el", u.u8, 1, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AM)
+ nxscope_chan_init(&nxs->nxs, i++, "a_m", u.u8, 1, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VEL)
+ nxscope_chan_init(&nxs->nxs, i++, "v_el", u.u8, 1, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM)
+ nxscope_chan_init(&nxs->nxs, i++, "v_m", u.u8, 1, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VBUS)
+ nxscope_chan_init(&nxs->nxs, i++, "vbus", u.u8, 1, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPTORQ)
+ nxscope_chan_init(&nxs->nxs, i++, "sp_torq", u.u8, 3, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPVEL)
+ nxscope_chan_init(&nxs->nxs, i++, "sp_vel", u.u8, 3, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPPOS)
+ nxscope_chan_init(&nxs->nxs, i++, "sp_pos", u.u8, 3, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_DQREF)
+ nxscope_chan_init(&nxs->nxs, i++, "dqref", u.u8, 2, 0);
+#endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQCOMP)
+ nxscope_chan_init(&nxs->nxs, i++, "vdqcomp", u.u8, 2, 0);
+#endif
+
+ if (i > CONFIG_EXAMPLES_FOC_NXSCOPE_CHANNELS)
+ {
+ PRINTF("ERROR: invalid nxscope channels value %d\n", i);
+ ret = -ENOBUFS;
+ goto errout;
+ }
+ }
+
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_THREAD
+ /* Configure thread */
+
+ pthread_attr_init(&attr);
+ param.sched_priority = CONFIG_EXAMPLES_FOC_NXSCOPE_PRIO;
+ pthread_attr_setschedparam(&attr, ¶m);
+ pthread_attr_setstacksize(&attr, CONFIG_EXAMPLES_FOC_NXSCOPE_STACKSIZE);
+
+ /* Create nxscope thread */
+
+ ret = pthread_create(&nxsthr, &attr, foc_nxscope_thr, nxs);
+ if (ret != OK)
+ {
+ PRINTF("ERROR: pthread_create failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Set thread name */
+
+ ret = pthread_setname_np(nxsthr, "nxsthr");
+ if (ret != OK)
+ {
+ PRINTF("ERROR: pthread_setname_np failed %d\n", ret);
+ goto errout;
+ }
+#endif
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_nxscope_deinit
+ ****************************************************************************/
+
+void foc_nxscope_deinit(FAR struct foc_nxscope_s *nxs)
+{
+ DEBUGASSERT(nxs);
+
+ nxscope_ser_deinit(nxs->nxs.intf_cmd);
+ nxscope_proto_ser_deinit(nxs->nxs.proto_cmd);
+ nxscope_deinit(&nxs->nxs);
+}
+
+/****************************************************************************
+ * Name: foc_nxscope_work
+ ****************************************************************************/
+
+void foc_nxscope_work(FAR struct foc_nxscope_s *nxs)
+{
+ int ret = OK;
+
+ /* Flush stream data */
+
+ ret = nxscope_stream(&nxs->nxs);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: nxscope_stream failed %d\n", ret);
+ }
+
+ ret = nxscope_recv(&nxs->nxs);
+ if (ret < 0)
+ {
+ PRINTF("ERROR: nxscope_recv failed %d\n", ret);
+ }
+}
diff --git a/examples/foc/foc_nxscope.h b/examples/foc/foc_nxscope.h
new file mode 100644
index 000000000..bfbdda833
--- /dev/null
+++ b/examples/foc/foc_nxscope.h
@@ -0,0 +1,92 @@
+/****************************************************************************
+ * apps/examples/foc/foc_nxscope.h
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+#ifndef __APPS_EXAMPLES_FOC_FOC_NXSCOPE_H
+#define __APPS_EXAMPLES_FOC_FOC_NXSCOPE_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include "logging/nxscope/nxscope.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Definitions for CONFIG_EXAMPLES_FOC_NXSCOPE_CFG bit-mask */
+
+#define FOC_NXSCOPE_IABC (1 << 0) /* Phases current */
+#define FOC_NXSCOPE_IDQ (1 << 1) /* Current dq */
+#define FOC_NXSCOPE_IAB (1 << 2) /* Current alpha-beta */
+#define FOC_NXSCOPE_VABC (1 << 3) /* Phases voltage */
+#define FOC_NXSCOPE_VDQ (1 << 4) /* Voltage dq */
+#define FOC_NXSCOPE_VAB (1 << 5) /* Voltage alpha-beta */
+#define FOC_NXSCOPE_AEL (1 << 6) /* Electrical angle */
+#define FOC_NXSCOPE_AM (1 << 7) /* Mechanical angle */
+#define FOC_NXSCOPE_VEL (1 << 8) /* Electrical velocity */
+#define FOC_NXSCOPE_VM (1 << 9) /* Mechanical velocity */
+#define FOC_NXSCOPE_VBUS (1 << 10) /* VBUS */
+#define FOC_NXSCOPE_SPTORQ (1 << 11) /* Torque setpoint */
+#define FOC_NXSCOPE_SPVEL (1 << 12) /* Velolcity setpoint */
+#define FOC_NXSCOPE_SPPOS (1 << 13) /* Position setpoint */
+#define FOC_NXSCOPE_DQREF (1 << 14) /* DQ reference */
+#define FOC_NXSCOPE_VDQCOMP (1 << 15) /* VDQ compensation */
+ /* Max 32-bit */
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+struct foc_nxscope_s
+{
+ struct nxscope_s nxs;
+ struct nxscope_intf_s intf;
+ struct nxscope_proto_s proto;
+ struct nxscope_ser_cfg_s ser_cfg;
+ struct nxscope_callbacks_s cb;
+ uint8_t ch_per_inst;
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_nxscope_init
+ ****************************************************************************/
+
+int foc_nxscope_init(FAR struct foc_nxscope_s *nxs);
+
+/****************************************************************************
+ * Name: foc_nxscope_deinit
+ ****************************************************************************/
+
+void foc_nxscope_deinit(FAR struct foc_nxscope_s *nxs);
+
+/****************************************************************************
+ * Name: foc_nxscope_work
+ ****************************************************************************/
+
+void foc_nxscope_work(FAR struct foc_nxscope_s *nxs);
+
+#endif /* __APPS_EXAMPLES_FOC_FOC_NXSCOPE_H */
diff --git a/examples/foc/foc_thr.h b/examples/foc/foc_thr.h
index e3d8f1b41..53fe854a4 100644
--- a/examples/foc/foc_thr.h
+++ b/examples/foc/foc_thr.h
@@ -35,6 +35,10 @@
#include "foc_device.h"
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
+# include "foc_nxscope.h"
+#endif
+
/****************************************************************************
* Public Type Definition
****************************************************************************/
@@ -107,11 +111,14 @@ enum foc_controller_state_e
struct foc_ctrl_env_s
{
- mqd_t mqd; /* Control msg queue */
- int id; /* FOC device id */
- int inst; /* Type specific instance counter */
- int type; /* Controller type */
- struct foc_thr_cfg_s *cfg; /* Control thread configuration */
+ mqd_t mqd; /* Control msg queue */
+ int id; /* FOC device id */
+ int inst; /* Type specific instance counter */
+ int type; /* Controller type */
+ FAR struct foc_thr_cfg_s *cfg; /* Control thread configuration */
+#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
+ FAR struct foc_nxscope_s *nxs; /* nxscope handler */
+#endif
};
/****************************************************************************
[nuttx-apps] 01/03: examples/foc/foc_thr: fix mutex logic
Posted by xi...@apache.org.
This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/nuttx-apps.git
commit 54bcf3afdd5f1c7a10dbb425f0e31702937658ff
Author: raiden00pl <ra...@railab.me>
AuthorDate: Wed May 17 12:11:22 2023 +0200
examples/foc/foc_thr: fix mutex logic
---
examples/foc/foc_thr.c | 8 ++++----
1 file changed, 4 insertions(+), 4 deletions(-)
diff --git a/examples/foc/foc_thr.c b/examples/foc/foc_thr.c
index de5d580e5..83b39ce5a 100644
--- a/examples/foc/foc_thr.c
+++ b/examples/foc/foc_thr.c
@@ -256,14 +256,14 @@ bool foc_threads_terminated(void)
{
bool ret = false;
- pthread_mutex_unlock(&g_cntr_lock);
+ pthread_mutex_lock(&g_cntr_lock);
if (g_foc_thr == 0)
{
ret = true;
}
- pthread_mutex_lock(&g_cntr_lock);
+ pthread_mutex_unlock(&g_cntr_lock);
return ret;
}
@@ -276,9 +276,9 @@ 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);
+ ret = g_foc_thr;
+ pthread_mutex_unlock(&g_cntr_lock);
return ret;
}