From 8d61a10a74574952d3fced8594423724313c80e7 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Thu, 18 Aug 2022 11:38:32 +0200 Subject: [PATCH] industry/foc/foc_ident: make the Ki resistance measurement parameter configurable The hardcoded parameter may not be suitable for various types of motors --- examples/foc/Kconfig | 4 ++++ examples/foc/foc_cfg.h | 3 +++ examples/foc/foc_motor_b16.c | 2 ++ examples/foc/foc_motor_f32.c | 1 + include/industry/foc/fixed16/foc_ident.h | 1 + include/industry/foc/float/foc_ident.h | 1 + industry/foc/fixed16/foc_ident.c | 10 ++++++++-- industry/foc/float/foc_ident.c | 10 ++++++++-- 8 files changed, 28 insertions(+), 4 deletions(-) diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig index c3a1066c1..ade011557 100644 --- a/examples/foc/Kconfig +++ b/examples/foc/Kconfig @@ -386,6 +386,10 @@ config EXAMPLES_FOC_IDENT_RES_CURRENT int "FOC motor ident resistance current (x1000)" default 0 +config EXAMPLES_FOC_IDENT_RES_KI + int "FOC motor ident resistance Ki (x1000)" + default 50 + config EXAMPLES_FOC_IDENT_IND_VOLTAGE int "FOC motor ident inductance voltage (x1000)" default 0 diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h index 155b44379..4874aed58 100644 --- a/examples/foc/foc_cfg.h +++ b/examples/foc/foc_cfg.h @@ -88,6 +88,9 @@ # if (CONFIG_EXAMPLES_FOC_IDENT_RES_CURRENT == 0) # error # endif +# if (CONFIG_EXAMPLES_FOC_IDENT_RES_KI == 0) +# error +# endif # if (CONFIG_EXAMPLES_FOC_IDENT_IND_VOLTAGE == 0) # error # endif diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c index a3996f6d1..8a4083a0f 100644 --- a/examples/foc/foc_motor_b16.c +++ b/examples/foc/foc_motor_b16.c @@ -989,6 +989,8 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, ident_cfg.per = motor->per; ident_cfg.res_current = ftob16(CONFIG_EXAMPLES_FOC_IDENT_RES_CURRENT / 1000.0f); + ident_cfg.res_ki = ftob16(CONFIG_EXAMPLES_FOC_IDENT_RES_KI / + 1000.0f); ident_cfg.ind_volt = ftob16(CONFIG_EXAMPLES_FOC_IDENT_IND_VOLTAGE / 1000.0f); ident_cfg.res_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ * diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c index 6c9b54374..c123881cf 100644 --- a/examples/foc/foc_motor_f32.c +++ b/examples/foc/foc_motor_f32.c @@ -972,6 +972,7 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, 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_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ * \ CONFIG_EXAMPLES_FOC_IDENT_RES_SEC / 1000); diff --git a/include/industry/foc/fixed16/foc_ident.h b/include/industry/foc/fixed16/foc_ident.h index 276fa631e..d7360e19b 100644 --- a/include/industry/foc/fixed16/foc_ident.h +++ b/include/industry/foc/fixed16/foc_ident.h @@ -41,6 +41,7 @@ struct foc_routine_ident_cfg_b16_s { b16_t per; /* Routine period in sec */ b16_t res_current; /* Resistance measurement current */ + b16_t res_ki; /* Resistance measurement Ki */ b16_t ind_volt; /* Inductance measurement current */ int res_steps; /* Resistance measurement steps */ int ind_steps; /* Inductance measurement steps */ diff --git a/include/industry/foc/float/foc_ident.h b/include/industry/foc/float/foc_ident.h index a2d392ba9..66da973a4 100644 --- a/include/industry/foc/float/foc_ident.h +++ b/include/industry/foc/float/foc_ident.h @@ -83,6 +83,7 @@ struct foc_routine_ident_cfg_f32_s #endif float per; /* Routine period in sec */ float res_current; /* Resistance measurement current */ + float res_ki; /* Resistance measurement Ki */ float ind_volt; /* Inductance measurement voltage */ int res_steps; /* Resistance measurement steps */ int ind_steps; /* Inductance measurement steps */ diff --git a/industry/foc/fixed16/foc_ident.c b/industry/foc/fixed16/foc_ident.c index 65dab3535..be9d4eaef 100644 --- a/industry/foc/fixed16/foc_ident.c +++ b/industry/foc/fixed16/foc_ident.c @@ -39,7 +39,6 @@ ****************************************************************************/ #define IDENT_PI_KP (ftob16(0.0f)) -#define IDENT_PI_KI (ftob16(0.05f)) /**************************************************************************** * Private Data Types @@ -169,7 +168,8 @@ int foc_ident_res_run_b16(FAR struct foc_ident_b16_s *ident, if (ident->cntr == 0) { - pi_controller_init_b16(&ident->pi, IDENT_PI_KP, IDENT_PI_KI); + DEBUGASSERT(ident->cfg.res_ki > 0); + pi_controller_init_b16(&ident->pi, IDENT_PI_KP, ident->cfg.res_ki); } /* PI saturation */ @@ -427,6 +427,12 @@ int foc_routine_ident_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg) goto errout; } + if (i->cfg.res_ki <= 0) + { + ret = -EINVAL; + goto errout; + } + if (i->cfg.res_current <= 0 || i->cfg.ind_volt <= 0) { ret = -EINVAL; diff --git a/industry/foc/float/foc_ident.c b/industry/foc/float/foc_ident.c index 8ffba33d0..4a04c74d3 100644 --- a/industry/foc/float/foc_ident.c +++ b/industry/foc/float/foc_ident.c @@ -39,7 +39,6 @@ ****************************************************************************/ #define IDENT_PI_KP (0.0f) -#define IDENT_PI_KI (0.05f) /**************************************************************************** * Private Data Types @@ -194,7 +193,8 @@ int foc_ident_res_run_f32(FAR struct foc_ident_f32_s *ident, if (ident->cntr == 0) { - pi_controller_init(&ident->pi, IDENT_PI_KP, IDENT_PI_KI); + DEBUGASSERT(ident->cfg.res_ki > 0.0f); + pi_controller_init(&ident->pi, IDENT_PI_KP, ident->cfg.res_ki); } /* PI saturation */ @@ -576,6 +576,12 @@ int foc_routine_ident_cfg_f32(FAR foc_routine_f32_t *r, FAR void *cfg) goto errout; } + if (i->cfg.res_ki <= 0.0f) + { + ret = -EINVAL; + goto errout; + } + if (i->cfg.res_current <= 0.0f || i->cfg.ind_volt <= 0.0f #ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX || i->cfg.flux_curr <= 0.0f