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

examples/foc: refactor args

Browse Source
This commit is contained in:
raiden00pl 2022-10-22 18:01:06 +02:00 committed by Xiang Xiao
parent 6e3c4aab7d
commit c2efa80969
1 changed files with 55 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

142
examples/foc/foc_main.c
View File

@ -56,73 +56,51 @@
#define INST_EN_DEFAULT (0xff) #define INST_EN_DEFAULT (0xff)
/**************************************************************************** /****************************************************************************
* Private Functions * Private Data
****************************************************************************/ ****************************************************************************/
/**************************************************************************** struct args_s g_args =
* Name: init_args
****************************************************************************/
static void init_args(FAR struct args_s *args)
{ {
args->time = .time = CONFIG_EXAMPLES_FOC_TIME_DEFAULT,
(args->time == 0 ? CONFIG_EXAMPLES_FOC_TIME_DEFAULT : args->time); .fmode = CONFIG_EXAMPLES_FOC_FMODE,
args->fmode = .mmode = CONFIG_EXAMPLES_FOC_MMODE,
(args->fmode == 0 ? CONFIG_EXAMPLES_FOC_FMODE : args->fmode);
args->mmode =
(args->mmode == 0 ? CONFIG_EXAMPLES_FOC_MMODE : args->mmode);
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
args->qparam = .qparam = CONFIG_EXAMPLES_FOC_OPENLOOP_Q,
(args->qparam == 0 ? CONFIG_EXAMPLES_FOC_OPENLOOP_Q : args->qparam);
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI #ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
args->foc_pi_kp = .foc_pi_kp = CONFIG_EXAMPLES_FOC_IDQ_KP,
(args->foc_pi_kp == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KP : args->foc_pi_kp); .foc_pi_ki = CONFIG_EXAMPLES_FOC_IDQ_KI,
args->foc_pi_ki =
(args->foc_pi_ki == 0 ? CONFIG_EXAMPLES_FOC_IDQ_KI : args->foc_pi_ki);
#endif #endif
/* Setpoint configuration */ .state = CONFIG_EXAMPLES_FOC_STATE_INIT,
.en = INST_EN_DEFAULT,
#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST
# ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
.torqmax = CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE,
# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
.velmax = CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE,
# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
.posmax = CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE,
# endif
#else
# ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
.torqmax = CONFIG_EXAMPLES_FOC_SETPOINT_MAX,
# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
.velmax = CONFIG_EXAMPLES_FOC_SETPOINT_MAX,
# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
.posmax = CONFIG_EXAMPLES_FOC_SETPOINT_MAX,
# endif
#endif
};
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ /****************************************************************************
#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST * Private Functions
args->torqmax = ****************************************************************************/
(args->torqmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->torqmax);
#else
args->torqmax =
(args->torqmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_MAX : args->torqmax);
#endif
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST
args->velmax =
(args->velmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->velmax);
#else
args->velmax =
(args->velmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_MAX : args->velmax);
#endif
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
#ifdef CONFIG_EXAMPLES_FOC_SETPOINT_CONST
args->posmax =
(args->posmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_CONST_VALUE : args->posmax);
#else
args->posmax =
(args->posmax == 0 ?
CONFIG_EXAMPLES_FOC_SETPOINT_MAX : args->posmax);
#endif
#endif
args->state =
(args->state == 0 ? CONFIG_EXAMPLES_FOC_STATE_INIT : args->state);
args->en = (args->en == -1 ? INST_EN_DEFAULT : args->en);
}
/**************************************************************************** /****************************************************************************
* Name: validate_args * Name: validate_args
@ -297,7 +275,6 @@ int main(int argc, char *argv[])
struct foc_ctrl_env_s foc[CONFIG_MOTOR_FOC_INST]; struct foc_ctrl_env_s foc[CONFIG_MOTOR_FOC_INST];
pthread_t threads[CONFIG_MOTOR_FOC_INST]; pthread_t threads[CONFIG_MOTOR_FOC_INST];
mqd_t mqd[CONFIG_MOTOR_FOC_INST]; mqd_t mqd[CONFIG_MOTOR_FOC_INST];
struct args_s args;
struct foc_intf_data_s data; struct foc_intf_data_s data;
int ret = OK; int ret = OK;
int i = 0; int i = 0;
@ -305,29 +282,20 @@ int main(int argc, char *argv[])
/* Reset some data */ /* Reset some data */
memset(&args, 0, sizeof(struct args_s));
memset(mqd, 0, sizeof(mqd_t) * CONFIG_MOTOR_FOC_INST); memset(mqd, 0, sizeof(mqd_t) * CONFIG_MOTOR_FOC_INST);
memset(foc, 0, sizeof(struct foc_ctrl_env_s) * CONFIG_MOTOR_FOC_INST); memset(foc, 0, sizeof(struct foc_ctrl_env_s) * CONFIG_MOTOR_FOC_INST);
memset(threads, 0, sizeof(pthread_t) * CONFIG_MOTOR_FOC_INST); memset(threads, 0, sizeof(pthread_t) * CONFIG_MOTOR_FOC_INST);
memset(&data, 0, sizeof(struct foc_intf_data_s)); memset(&data, 0, sizeof(struct foc_intf_data_s));
/* Initialize args before parse */
args.en = -1;
#ifdef CONFIG_BUILTIN #ifdef CONFIG_BUILTIN
/* Parse the command line */ /* Parse the command line */
parse_args(&args, argc, argv); parse_args(&g_args, argc, argv);
#endif #endif
/* Initialize args */
init_args(&args);
/* Validate arguments */ /* Validate arguments */
ret = validate_args(&args); ret = validate_args(&g_args);
if (ret < 0) if (ret < 0)
{ {
PRINTF("ERROR: validate args failed\n"); PRINTF("ERROR: validate args failed\n");
@ -373,25 +341,25 @@ int main(int argc, char *argv[])
/* Get configuration */ /* Get configuration */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
foc[i].qparam = args.qparam; foc[i].qparam = g_args.qparam;
#endif #endif
foc[i].fmode = args.fmode; foc[i].fmode = g_args.fmode;
foc[i].mmode = args.mmode; foc[i].mmode = g_args.mmode;
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI #ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
foc[i].foc_pi_kp = args.foc_pi_kp; foc[i].foc_pi_kp = g_args.foc_pi_kp;
foc[i].foc_pi_ki = args.foc_pi_ki; foc[i].foc_pi_ki = g_args.foc_pi_ki;
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
foc[i].torqmax = args.torqmax; foc[i].torqmax = g_args.torqmax;
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
foc[i].velmax = args.velmax; foc[i].velmax = g_args.velmax;
#endif #endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS #ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
foc[i].posmax = args.posmax; foc[i].posmax = g_args.posmax;
#endif #endif
if (args.en & (1 << i)) if (g_args.en & (1 << i))
{ {
/* Initialize controller thread if enabled */ /* Initialize controller thread if enabled */
@ -422,7 +390,7 @@ int main(int argc, char *argv[])
/* Controller state */ /* Controller state */
data.state = args.state; data.state = g_args.state;
/* Auxliary control loop */ /* Auxliary control loop */
@ -445,7 +413,7 @@ int main(int argc, char *argv[])
{ {
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{ {
if (args.en & (1 << i)) if (g_args.en & (1 << i))
{ {
PRINTFV("Send vbus to %d\n", i); PRINTFV("Send vbus to %d\n", i);
@ -471,7 +439,7 @@ int main(int argc, char *argv[])
{ {
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{ {
if (args.en & (1 << i)) if (g_args.en & (1 << i))
{ {
PRINTFV("Send state %" PRIu32 " to %d\n", data.state, i); PRINTFV("Send state %" PRIu32 " to %d\n", data.state, i);
@ -497,7 +465,7 @@ int main(int argc, char *argv[])
{ {
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{ {
if (args.en & (1 << i)) if (g_args.en & (1 << i))
{ {
PRINTFV("Send setpoint = %" PRIu32 "to %d\n", PRINTFV("Send setpoint = %" PRIu32 "to %d\n",
data.sp_raw, i); data.sp_raw, i);
@ -524,7 +492,7 @@ int main(int argc, char *argv[])
{ {
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{ {
if (args.en & (1 << i)) if (g_args.en & (1 << i))
{ {
PRINTFV("Send start to %d\n", i); PRINTFV("Send start to %d\n", i);
@ -548,9 +516,9 @@ int main(int argc, char *argv[])
time += 1; time += 1;
if (args.time != -1) if (g_args.time != -1)
{ {
if (time >= (args.time * (1000000 / MAIN_LOOP_USLEEP))) if (time >= (g_args.time * (1000000 / MAIN_LOOP_USLEEP)))
{ {
/* Exit loop */ /* Exit loop */
@ -574,7 +542,7 @@ errout:
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{ {
if (args.en & (1 << i)) if (g_args.en & (1 << i))
{ {
if (mqd[i] != (mqd_t)-1) if (mqd[i] != (mqd_t)-1)
{ {
@ -600,7 +568,7 @@ errout:
for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1) for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1)
{ {
if (args.en & (1 << i)) if (g_args.en & (1 << i))
{ {
/* Close FOC control thread queue */ /* Close FOC control thread queue */