sergiotarxz/nuttx-apps
sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

examples/foc: send messages only to active control threads

Browse Source
This commit is contained in:
raiden00pl 2023-05-05 13:15:36 +02:00 committed by Alan Carvalho de Assis
parent beb5369a0d
commit f358bdfcb3
3 changed files with 37 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
examples/foc/foc_main.c
View File

@ -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)
{

29
examples/foc/foc_thr.c
View File

@ -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
****************************************************************************/

2
examples/foc/foc_thr.h
View File

@ -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);