From 13bfad1053e205e5b0911c879c2149d15642a454 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sat, 22 Oct 2022 19:41:34 +0200 Subject: [PATCH] examples/foc: control motor identification parameters from cmd line --- examples/foc/foc_cfg.h | 8 ++++ examples/foc/foc_main.c | 7 ++++ examples/foc/foc_motor_b16.c | 10 ++--- examples/foc/foc_motor_f32.c | 10 ++--- examples/foc/foc_parseargs.c | 74 ++++++++++++++++++++++++++++++++++++ 5 files changed, 99 insertions(+), 10 deletions(-) diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h index d80ff25b9..44f46c4ab 100644 --- a/examples/foc/foc_cfg.h +++ b/examples/foc/foc_cfg.h @@ -236,6 +236,14 @@ struct foc_thr_cfg_s #ifdef CONFIG_EXAMPLES_FOC_HAVE_POS uint32_t posmax; /* Position max (x1000) */ #endif + +#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT + uint32_t ident_res_ki; /* Ident res Ki (x1000) */ + uint32_t ident_res_curr; /* Ident res current (x1000) */ + uint32_t ident_res_sec; /* Ident res sec */ + uint32_t ident_ind_volt; /* Ident res voltage (x1000) */ + uint32_t ident_ind_sec; /* Ident ind sec */ +#endif }; #endif /* __APPS_EXAMPLES_FOC_FOC_CFG_H */ diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c index c985b3f9a..dc529862d 100644 --- a/examples/foc/foc_main.c +++ b/examples/foc/foc_main.c @@ -95,6 +95,13 @@ struct args_s g_args = # ifdef CONFIG_EXAMPLES_FOC_HAVE_POS .posmax = CONFIG_EXAMPLES_FOC_SETPOINT_MAX, # endif +#endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT + .ident_res_ki = CONFIG_EXAMPLES_FOC_IDENT_RES_KI, + .ident_res_curr = CONFIG_EXAMPLES_FOC_IDENT_RES_CURRENT, + .ident_res_sec = CONFIG_EXAMPLES_FOC_IDENT_RES_SEC, + .ident_ind_volt = CONFIG_EXAMPLES_FOC_IDENT_IND_VOLTAGE, + .ident_ind_sec = CONFIG_EXAMPLES_FOC_IDENT_IND_SEC, #endif } }; diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c index 29eb49e76..b2a8ef5cf 100644 --- a/examples/foc/foc_motor_b16.c +++ b/examples/foc/foc_motor_b16.c @@ -988,16 +988,16 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, /* Initialize motor identification data */ ident_cfg.per = motor->per; - ident_cfg.res_current = ftob16(CONFIG_EXAMPLES_FOC_IDENT_RES_CURRENT / + ident_cfg.res_current = ftob16(motor->envp->cfg->ident_res_curr / 1000.0f); - ident_cfg.res_ki = ftob16(CONFIG_EXAMPLES_FOC_IDENT_RES_KI / + ident_cfg.res_ki = ftob16(motor->envp->cfg->ident_res_ki / 1000.0f); - ident_cfg.ind_volt = ftob16(CONFIG_EXAMPLES_FOC_IDENT_IND_VOLTAGE / + ident_cfg.ind_volt = ftob16(motor->envp->cfg->ident_ind_volt / 1000.0f); ident_cfg.res_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ * - CONFIG_EXAMPLES_FOC_IDENT_RES_SEC / 1000); + motor->envp->cfg->ident_res_sec / 1000); ident_cfg.ind_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ * - CONFIG_EXAMPLES_FOC_IDENT_IND_SEC / 1000); + motor->envp->cfg->ident_ind_sec / 1000); ident_cfg.idle_steps = CONFIG_EXAMPLES_FOC_IDENT_IDLE; ret = foc_routine_cfg_b16(&motor->ident, &ident_cfg); diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c index 587744757..37103e0d2 100644 --- a/examples/foc/foc_motor_f32.c +++ b/examples/foc/foc_motor_f32.c @@ -972,13 +972,13 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, /* Initialize motor identification data */ ident_cfg.per = motor->per; - ident_cfg.res_current = (CONFIG_EXAMPLES_FOC_IDENT_RES_CURRENT / 1000.0f); - ident_cfg.res_ki = (CONFIG_EXAMPLES_FOC_IDENT_RES_KI / 1000.0f); - ident_cfg.ind_volt = (CONFIG_EXAMPLES_FOC_IDENT_IND_VOLTAGE / 1000.0f); + ident_cfg.res_current = (motor->envp->cfg->ident_res_curr / 1000.0f); + ident_cfg.res_ki = (motor->envp->cfg->ident_res_ki / 1000.0f); + ident_cfg.ind_volt = (motor->envp->cfg->ident_ind_volt / 1000.0f); ident_cfg.res_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ * \ - CONFIG_EXAMPLES_FOC_IDENT_RES_SEC / 1000); + motor->envp->cfg->ident_res_sec / 1000); ident_cfg.ind_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ * \ - CONFIG_EXAMPLES_FOC_IDENT_IND_SEC / 1000); + motor->envp->cfg->ident_ind_sec / 1000); ident_cfg.idle_steps = CONFIG_EXAMPLES_FOC_IDENT_IDLE; ret = foc_routine_cfg_f32(&motor->ident, &ident_cfg); diff --git a/examples/foc/foc_parseargs.c b/examples/foc/foc_parseargs.c index 1b382595d..f4e669a0b 100644 --- a/examples/foc/foc_parseargs.c +++ b/examples/foc/foc_parseargs.c @@ -39,6 +39,12 @@ #define OPT_FKI (SCHAR_MAX + 1) #define OPT_FKP (SCHAR_MAX + 2) +#define OPT_IRKI (SCHAR_MAX + 3) +#define OPT_IRC (SCHAR_MAX + 4) +#define OPT_IRS (SCHAR_MAX + 5) +#define OPT_IIV (SCHAR_MAX + 6) +#define OPT_IIS (SCHAR_MAX + 7) + /**************************************************************************** * Private Data ****************************************************************************/ @@ -66,6 +72,13 @@ static struct option g_long_options[] = #ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI { "fkp", required_argument, 0, OPT_FKP }, { "fki", required_argument, 0, OPT_FKI }, +#endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT + { "irki", required_argument, 0, OPT_IRKI }, + { "irc", required_argument, 0, OPT_IRC }, + { "irs", required_argument, 0, OPT_IRS }, + { "iiv", required_argument, 0, OPT_IIV }, + { "iis", required_argument, 0, OPT_IIS }, #endif { 0, 0, 0, 0 } }; @@ -127,6 +140,18 @@ static void foc_help(void) PRINTF(" [--fkp] PI Ki coefficient [x1000] (default: %d)\n", CONFIG_EXAMPLES_FOC_IDQ_KI); #endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT + PRINTF(" [--irki] res Ki coefficient [x1000] (default: %d)\n", + CONFIG_EXAMPLES_FOC_IDENT_RES_KI); + PRINTF(" [--irc] res current [x1000] (default: %d)\n", + CONFIG_EXAMPLES_FOC_IDENT_RES_CURRENT); + PRINTF(" [--irs] res sec (default: %d)\n", + CONFIG_EXAMPLES_FOC_IDENT_RES_SEC); + PRINTF(" [--iiv] ind voltage [x1000] (default: %d)\n", + CONFIG_EXAMPLES_FOC_IDENT_IND_VOLTAGE); + PRINTF(" [--iis] ind sec (default: %d)\n", + CONFIG_EXAMPLES_FOC_IDENT_IND_SEC); +#endif } /**************************************************************************** @@ -168,6 +193,38 @@ void parse_args(FAR struct args_s *args, int argc, FAR char **argv) } #endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT + case OPT_IRKI: + { + args->cfg.ident_res_ki = atoi(optarg); + break; + } + + case OPT_IRC: + { + args->cfg.ident_res_curr = atoi(optarg); + break; + } + + case OPT_IRS: + { + args->cfg.ident_res_sec = atoi(optarg); + break; + } + + case OPT_IIV: + { + args->cfg.ident_ind_volt = atoi(optarg); + break; + } + + case OPT_IIS: + { + args->cfg.ident_ind_sec = atoi(optarg); + break; + } +#endif + case 't': { args->time = atoi(optarg); @@ -327,6 +384,23 @@ int validate_args(FAR struct args_s *args) goto errout; } +#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT + /* Motor identification parameters */ + + if (args->cfg.ident_res_ki == 0 || args->cfg.ident_res_curr == 0 || + args->cfg.ident_res_sec == 0) + { + PRINTF("ERROR: missing motor res ident configuration\n"); + goto errout; + } + + if (args->cfg.ident_ind_volt == 0 || args->cfg.ident_ind_sec == 0) + { + PRINTF("ERROR: missing motor ind ident configuration\n"); + goto errout; + } +#endif + /* Otherwise OK */ ret = OK;