From f358bdfcb35f6b403e830d9e7025989938d15625 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Fri, 5 May 2023 13:15:36 +0200 Subject: [PATCH] examples/foc: send messages only to active control threads --- examples/foc/foc_main.c | 19 ++++++++++++++----- examples/foc/foc_thr.c | 29 +++++++++++++++++++++-------- examples/foc/foc_thr.h | 2 ++ 3 files changed, 37 insertions(+), 13 deletions(-) diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c index 331b40005..3e8775616 100644 --- a/examples/foc/foc_main.c +++ b/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) { diff --git a/examples/foc/foc_thr.c b/examples/foc/foc_thr.c index f7f18bf1b..de5d580e5 100644 --- a/examples/foc/foc_thr.c +++ b/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 -#ifdef CONFIG_INDUSTRY_FOC_FLOAT - && g_float_thr_cntr <= 0 -#endif -#ifdef CONFIG_INDUSTRY_FOC_FIXED16 - && g_fixed16_thr_cntr <= 0 -#endif - ) + if (g_foc_thr == 0) { 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 ****************************************************************************/ diff --git a/examples/foc/foc_thr.h b/examples/foc/foc_thr.h index 629801141..3c6d1fb9a 100644 --- a/examples/foc/foc_thr.h +++ b/examples/foc/foc_thr.h @@ -27,6 +27,7 @@ #include +#include #include #include @@ -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);