From 8e0819df04a3f99eea3692c37446b2ed4b9608a8 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sat, 5 Nov 2022 11:00:19 +0100 Subject: [PATCH] examples/foc: add velocity observers --- examples/foc/Kconfig | 54 ++++++++++ examples/foc/foc_cfg.h | 13 +++ examples/foc/foc_fixed16_thr.c | 8 +- examples/foc/foc_float_thr.c | 12 ++- examples/foc/foc_main.c | 11 ++ examples/foc/foc_motor_b16.c | 185 +++++++++++++++++++++++++++++-- examples/foc/foc_motor_b16.h | 17 +++ examples/foc/foc_motor_f32.c | 191 +++++++++++++++++++++++++++++++-- examples/foc/foc_motor_f32.h | 17 +++ examples/foc/foc_nxscope.c | 3 + examples/foc/foc_nxscope.h | 1 + examples/foc/foc_parseargs.c | 53 +++++++++ 12 files changed, 537 insertions(+), 28 deletions(-) diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig index 057dfbc3c..4b829ed84 100644 --- a/examples/foc/Kconfig +++ b/examples/foc/Kconfig @@ -189,6 +189,60 @@ config EXAMPLES_FOC_HAVE_POS bool "FOC example position controller support" default n +config EXAMPLES_FOC_VELOBS + bool "FOC example velocity observer support" + default n + +if EXAMPLES_FOC_HAVE_VEL + +config EXAMPLES_FOC_VELNOW_FILTER + int "FOC example velocity controller (x1000)" + default 990 + +endif # EXAMPLES_FOC_HAVE_VEL + +if EXAMPLES_FOC_VELOBS + +choice + prompt "FOC example velocity observer selection" + default EXAMPLES_FOC_VELOBS_DIV + +config EXAMPLES_FOC_VELOBS_DIV + bool "FOC velocity DIV observer" + select INDUSTRY_FOC_VELOCITY_ODIV + +config EXAMPLES_FOC_VELOBS_PLL + bool "FOC velocity PLL observer" + select INDUSTRY_FOC_VELOCITY_OPLL + +endchoice # FOC example velocity observer selection + +if EXAMPLES_FOC_VELOBS_DIV + +config EXAMPLES_FOC_VELOBS_DIV_SAMPLES + int "FOC velocity DIV observer samples" + default 10 + +config EXAMPLES_FOC_VELOBS_DIV_FILTER + int "FOC velocity DIV observer filter (x1000)" + default 990 + +endif # INDUSTRY_FOC_VELOCITY_ODIV + +if EXAMPLES_FOC_VELOBS_PLL + +config EXAMPLES_FOC_VELOBS_PLL_KP + int "FOC velocity PLL observer Kp (x1)" + default 0 + +config EXAMPLES_FOC_VELOBS_PLL_KI + int "FOC velocity PLL observer Ki (x1)" + default 0 + +endif # EXAMPLES_FOC_VELOBS_PLL + +endif # EXAMPLES_FOC_VELOBS + menu "FOC user input" config EXAMPLES_FOC_HAVE_ADC diff --git a/examples/foc/foc_cfg.h b/examples/foc/foc_cfg.h index 14a1d1a47..ac2e6b3bc 100644 --- a/examples/foc/foc_cfg.h +++ b/examples/foc/foc_cfg.h @@ -244,6 +244,19 @@ struct foc_thr_cfg_s uint32_t ident_ind_volt; /* Ident res voltage (x1000) */ uint32_t ident_ind_sec; /* Ident ind sec */ #endif + +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + uint32_t vel_filter; /* Velocity filter (x1000) */ +#endif + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + uint32_t vel_pll_kp; /* Vel PLL observer Kp (x1000) */ + uint32_t vel_pll_ki; /* Vel PLL observer Ki (x1000) */ +#endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + uint32_t vel_div_samples; /* Vel DIV observer samples */ + uint32_t vel_div_filter; /* Vel DIV observer filter (x1000) */ +#endif }; #endif /* __APPS_EXAMPLES_FOC_FOC_CFG_H */ diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c index 66ce7feab..d3d7bbc5c 100644 --- a/examples/foc/foc_fixed16_thr.c +++ b/examples/foc/foc_fixed16_thr.c @@ -228,12 +228,12 @@ static void foc_fixed16_nxscope(FAR struct foc_nxscope_s *nxs, nxscope_put_vb16(&nxs->nxs, i++, ptr, 1); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VEL) -# warning not supported yet - i++; + ptr = (FAR b16_t *)&motor->vel_el; + nxscope_put_vb16(&nxs->nxs, i++, ptr, 1); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM) -# warning not supported yet - i++; + ptr = (FAR b16_t *)&motor->vel_mech; + nxscope_put_vb16(&nxs->nxs, i++, ptr, 1); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VBUS) ptr = (FAR b16_t *)&motor->vbus; diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c index cc362a1da..94d8596be 100644 --- a/examples/foc/foc_float_thr.c +++ b/examples/foc/foc_float_thr.c @@ -229,12 +229,12 @@ static void foc_float_nxscope(FAR struct foc_nxscope_s *nxs, nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VEL) -# warning not supported yet - i++; + ptr = (FAR float *)&motor->vel_el; + nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM) -# warning not supported yet - i++; + ptr = (FAR float *)&motor->vel_mech; + nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1); #endif #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VBUS) ptr = (FAR float *)&motor->vbus; @@ -278,6 +278,10 @@ static void foc_float_nxscope(FAR struct foc_nxscope_s *nxs, ptr = svm3_tmp; nxscope_put_vfloat(&nxs->nxs, i++, ptr, 4); #endif +#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VOBS) + ptr = (FAR float *)&motor->vel_obs; + nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1); +#endif nxscope_unlock(&nxs->nxs); } diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c index f4d8f95d2..9258a46f6 100644 --- a/examples/foc/foc_main.c +++ b/examples/foc/foc_main.c @@ -108,6 +108,17 @@ struct args_s g_args = .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 +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + .vel_filter = CONFIG_EXAMPLES_FOC_VELNOW_FILTER, +#endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + .vel_pll_kp = CONFIG_EXAMPLES_FOC_VELOBS_PLL_KP, + .vel_pll_ki = CONFIG_EXAMPLES_FOC_VELOBS_PLL_KI, +#endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + .vel_div_samples = CONFIG_EXAMPLES_FOC_VELOBS_DIV_SAMPLES, + .vel_div_filter = CONFIG_EXAMPLES_FOC_VELOBS_DIV_FILTER, #endif } }; diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c index f1b6be344..06a7201d2 100644 --- a/examples/foc/foc_motor_b16.c +++ b/examples/foc/foc_motor_b16.c @@ -704,9 +704,32 @@ errout: static int foc_motor_run_init(FAR struct foc_motor_b16_s *motor) { - /* Empty for now */ + int ret = OK; - return OK; + /* Reset velocity observer */ + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + ret = foc_velocity_zero_b16(&motor->vel_div); + if (ret < 0) + { + PRINTF("ERROR: foc_velocity_zero failed %d\n", ret); + goto errout; + } +#endif + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + ret = foc_velocity_zero_b16(&motor->vel_pll); + if (ret < 0) + { + PRINTF("ERROR: foc_velocity_zero failed %d\n", ret); + goto errout; + } +#endif + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS +errout: +#endif + return ret; } /**************************************************************************** @@ -831,6 +854,12 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL struct foc_hall_cfg_b16_s hl_cfg; #endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + struct foc_vel_div_b16_cfg_s odiv_cfg; +#endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + struct foc_vel_pll_b16_cfg_s opll_cfg; +#endif #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN struct foc_routine_align_cfg_b16_s align_cfg; #endif @@ -944,6 +973,56 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, } #endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + /* Initialize velocity observer */ + + ret = foc_velocity_init_b16(&motor->vel_div, + &g_foc_velocity_odiv_b16); + if (ret < 0) + { + PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret); + goto errout; + } + + /* Configure velocity observer */ + + odiv_cfg.samples = (motor->envp->cfg->vel_div_samples); + odiv_cfg.filter = ftob16(motor->envp->cfg->vel_div_samples / 1000.0f); + odiv_cfg.per = motor->per; + + ret = foc_velocity_cfg_b16(&motor->vel_div, &odiv_cfg); + if (ret < 0) + { + PRINTFV("ERROR: foc_velocity_cfg_b16 failed %d!\n", ret); + goto errout; + } +#endif + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + /* Initialize velocity observer */ + + ret = foc_velocity_init_b16(&motor->vel_pll, + &g_foc_velocity_opll_b16); + if (ret < 0) + { + PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret); + goto errout; + } + + /* Configure velocity observer */ + + opll_cfg.kp = ftob16(motor->envp->cfg->vel_pll_kp / 1.0f); + opll_cfg.ki = ftob16(motor->envp->cfg->vel_pll_ki / 1.0f); + opll_cfg.per = motor->per; + + ret = foc_velocity_cfg_b16(&motor->vel_pll, &opll_cfg); + if (ret < 0) + { + PRINTFV("ERROR: foc_velocity_cfg_b16 failed %d!\n", ret); + goto errout; + } +#endif + #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN /* Initialize motor alignment routine */ @@ -1016,6 +1095,12 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor, } #endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + /* Store velocity filter value */ + + motor->vel_filter = ftob16(motor->envp->cfg->vel_filter / 1000.0f); +#endif + /* Initialize controller state */ #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN @@ -1087,6 +1172,28 @@ int foc_motor_deinit(FAR struct foc_motor_b16_s *motor) } #endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + /* Deinitialize DIV observer handler */ + + ret = foc_velocity_deinit_b16(&motor->vel_div); + if (ret < 0) + { + PRINTFV("ERROR: foc_velocity_deinit_b16 failed %d!\n", ret); + goto errout; + } +#endif + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + /* Deinitialize PLL observer handler */ + + ret = foc_velocity_deinit_b16(&motor->vel_pll); + if (ret < 0) + { + PRINTFV("ERROR: foc_velocity_deinit_b16 failed %d!\n", ret); + goto errout; + } +#endif + #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN /* Deinitialize motor alignment routine */ @@ -1143,9 +1250,13 @@ errout: int foc_motor_get(FAR struct foc_motor_b16_s *motor) { - struct foc_angle_in_b16_s ain; - struct foc_angle_out_b16_s aout; - int ret = OK; + struct foc_angle_in_b16_s ain; + struct foc_angle_out_b16_s aout; +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + struct foc_velocity_in_b16_s vin; + struct foc_velocity_out_b16_s vout; +#endif + int ret = OK; DEBUGASSERT(motor); @@ -1202,7 +1313,7 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor) /* Convert mechanical angle to electrical angle */ motor->angle_el = (b16muli(motor->angle_m, - motor->phy.poles) % + motor->phy.p) % MOTOR_ANGLE_E_MAX_B16); } @@ -1227,20 +1338,74 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor) motor->angle_now = motor->angle_el; } +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + /* Update velocity handler input data */ + + vin.state = &motor->foc_state; + vin.angle = motor->angle_now; + vin.vel = motor->vel.now; + vin.dir = motor->dir; + + /* Get velocity */ + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + ret = foc_velocity_run_b16(&motor->vel_div, &vin, &vout); + if (ret < 0) + { + PRINTF("ERROR: foc_velocity_run failed %d\n", ret); + goto errout; + } +#endif + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + ret = foc_velocity_run_b16(&motor->vel_pll, &vin, &vout); + if (ret < 0) + { + PRINTF("ERROR: foc_velocity_run failed %d\n", ret); + goto errout; + } +#endif + + /* Get motor electrical velocity now */ + #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP if (motor->openloop_now == true) { - /* No velocity feedback - assume that velocity now is velocity set */ +#ifdef CONFIG_EXAMPLES_FOC_VELOBS + /* Electrical velocity from observer */ - motor->vel.now = motor->vel.set; + motor->vel_el = motor->vel_obs; +#else + UNUSED(vin); + UNUSED(vout); + + /* No velocity feedback - assume that electical velocity is + * velocity set + */ + + motor->vel_el = motor->vel.set; +#endif } else #endif { - /* TODO: velocity observer or sensor */ +#ifdef CONFIG_EXAMPLES_FOC_VELOBS + /* Store electrical velocity */ + + motor->vel_el = motor->vel_obs; +#else + ASSERT(0); +#endif } -#ifdef CONFIG_EXAMPLES_FOC_SENSORED + LP_FILTER_B16(motor->vel.now, motor->vel_el, motor->vel_filter); + + /* Get mechanical velocity */ + + motor->vel_mech = b16mulb16(motor->vel_el, motor->phy.one_by_p); +#endif /* CONFIG_EXAMPLES_FOC_HAVE_VEL */ + +#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || defined(CONFIG_EXAMPLES_FOC_VELOBS) errout: #endif return ret; diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h index 8159cbb2d..843302da8 100644 --- a/examples/foc/foc_motor_b16.h +++ b/examples/foc/foc_motor_b16.h @@ -102,6 +102,17 @@ struct foc_motor_b16_s b16_t angle_m; /* Motor mechanical angle */ b16_t angle_el; /* Motor electrical angle */ + /* Velocity state *********************************************************/ + +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + b16_t vel_el; /* Velocity - electrical */ + b16_t vel_mech; /* Velocity - mechanical */ + b16_t vel_filter; /* Velocity low-pass filter */ +#endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS + b16_t vel_obs; /* Velocity observer output */ +#endif + /* Motor setpoints ********************************************************/ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ @@ -150,6 +161,12 @@ struct foc_motor_b16_s foc_angle_b16_t qenco; /* Qenco angle handler */ char qedpath[32]; /* Qenco devpath */ #endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + foc_velocity_b16_t vel_div; /* DIV velocity observer */ +#endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + foc_velocity_b16_t vel_pll; /* PLL velocity observer */ +#endif }; /**************************************************************************** diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c index fc4e7f9d2..da6bdcab4 100644 --- a/examples/foc/foc_motor_f32.c +++ b/examples/foc/foc_motor_f32.c @@ -688,9 +688,32 @@ errout: static int foc_motor_run_init(FAR struct foc_motor_f32_s *motor) { - /* Empty for now */ + int ret = OK; - return OK; + /* Reset velocity observer */ + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + ret = foc_velocity_zero_f32(&motor->vel_div); + if (ret < 0) + { + PRINTF("ERROR: foc_velocity_zero failed %d\n", ret); + goto errout; + } +#endif + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + ret = foc_velocity_zero_f32(&motor->vel_pll); + if (ret < 0) + { + PRINTF("ERROR: foc_velocity_zero failed %d\n", ret); + goto errout; + } +#endif + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS +errout: +#endif + return ret; } /**************************************************************************** @@ -815,6 +838,12 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL struct foc_hall_cfg_f32_s hl_cfg; #endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + struct foc_vel_div_f32_cfg_s odiv_cfg; +#endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + struct foc_vel_pll_f32_cfg_s opll_cfg; +#endif #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN struct foc_routine_align_cfg_f32_s align_cfg; #endif @@ -928,6 +957,56 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, } #endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + /* Initialize velocity observer */ + + ret = foc_velocity_init_f32(&motor->vel_div, + &g_foc_velocity_odiv_f32); + if (ret < 0) + { + PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret); + goto errout; + } + + /* Configure velocity observer */ + + odiv_cfg.samples = (motor->envp->cfg->vel_div_samples); + odiv_cfg.filter = (motor->envp->cfg->vel_div_samples / 1000.0f); + odiv_cfg.per = motor->per; + + ret = foc_velocity_cfg_f32(&motor->vel_div, &odiv_cfg); + if (ret < 0) + { + PRINTFV("ERROR: foc_velocity_cfg_f32 failed %d!\n", ret); + goto errout; + } +#endif + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + /* Initialize velocity observer */ + + ret = foc_velocity_init_f32(&motor->vel_pll, + &g_foc_velocity_opll_f32); + if (ret < 0) + { + PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret); + goto errout; + } + + /* Configure velocity observer */ + + opll_cfg.kp = (motor->envp->cfg->vel_pll_kp / 1.0f); + opll_cfg.ki = (motor->envp->cfg->vel_pll_ki / 1.0f); + opll_cfg.per = motor->per; + + ret = foc_velocity_cfg_f32(&motor->vel_pll, &opll_cfg); + if (ret < 0) + { + PRINTFV("ERROR: foc_velocity_cfg_f32 failed %d!\n", ret); + goto errout; + } +#endif + #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN /* Initialize motor alignment routine */ @@ -997,6 +1076,12 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor, } #endif +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + /* Store velocity filter value */ + + motor->vel_filter = motor->envp->cfg->vel_filter / 1000.0f; +#endif + /* Initialize controller state */ #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN @@ -1073,6 +1158,28 @@ int foc_motor_deinit(FAR struct foc_motor_f32_s *motor) } #endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + /* Deinitialize DIV observer handler */ + + ret = foc_velocity_deinit_f32(&motor->vel_div); + if (ret < 0) + { + PRINTFV("ERROR: foc_velocity_deinit_f32 failed %d!\n", ret); + goto errout; + } +#endif + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + /* Deinitialize PLL observer handler */ + + ret = foc_velocity_deinit_f32(&motor->vel_pll); + if (ret < 0) + { + PRINTFV("ERROR: foc_velocity_deinit_f32 failed %d!\n", ret); + goto errout; + } +#endif + #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN /* Deinitialize motor alignment routine */ @@ -1129,9 +1236,13 @@ errout: int foc_motor_get(FAR struct foc_motor_f32_s *motor) { - struct foc_angle_in_f32_s ain; - struct foc_angle_out_f32_s aout; - int ret = OK; + struct foc_angle_in_f32_s ain; + struct foc_angle_out_f32_s aout; +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + struct foc_velocity_in_f32_s vin; + struct foc_velocity_out_f32_s vout; +#endif + int ret = OK; DEBUGASSERT(motor); @@ -1187,7 +1298,7 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor) /* Convert mechanical angle to electrical angle */ - motor->angle_el = fmodf(motor->angle_m * motor->phy.poles, + motor->angle_el = fmodf(motor->angle_m * motor->phy.p, MOTOR_ANGLE_E_MAX); } @@ -1212,20 +1323,80 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor) motor->angle_now = motor->angle_el; } +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + /* Update velocity handler input data */ + + vin.state = &motor->foc_state; + vin.angle = motor->angle_now; + vin.vel = motor->vel.now; + vin.dir = motor->dir; + + /* Get velocity */ + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + ret = foc_velocity_run_f32(&motor->vel_div, &vin, &vout); + if (ret < 0) + { + PRINTF("ERROR: foc_velocity_run failed %d\n", ret); + goto errout; + } + + motor->vel_obs = vout.velocity; +#endif + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + ret = foc_velocity_run_f32(&motor->vel_pll, &vin, &vout); + if (ret < 0) + { + PRINTF("ERROR: foc_velocity_run failed %d\n", ret); + goto errout; + } + + motor->vel_obs = vout.velocity; +#endif + + /* Get motor electrical velocity now */ + #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP if (motor->openloop_now == true) { - /* No velocity feedback - assume that velocity now is velocity set */ +#ifdef CONFIG_EXAMPLES_FOC_VELOBS + /* Electrical velocity from observer */ - motor->vel.now = motor->vel.set; + motor->vel_el = motor->vel_obs; +#else + UNUSED(vin); + UNUSED(vout); + + /* No velocity feedback - assume that electical velocity is + * velocity set + */ + + motor->vel_el = motor->vel.set; +#endif } else #endif { - /* TODO: velocity observer or sensor */ +#ifdef CONFIG_EXAMPLES_FOC_VELOBS + /* Store electrical velocity */ + + motor->vel_el = motor->vel_obs; +#else + ASSERT(0); +#endif } -#ifdef CONFIG_EXAMPLES_FOC_SENSORED + /* Store filtered velocity */ + + LP_FILTER(motor->vel.now, motor->vel_el, motor->vel_filter); + + /* Get mechanical velocity */ + + motor->vel_mech = motor->vel_el * motor->phy.one_by_p; +#endif /* CONFIG_EXAMPLES_FOC_HAVE_VEL */ + +#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || defined(CONFIG_EXAMPLES_FOC_VELOBS) errout: #endif return ret; diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h index 7dac93ca8..84d8a3c7b 100644 --- a/examples/foc/foc_motor_f32.h +++ b/examples/foc/foc_motor_f32.h @@ -102,6 +102,17 @@ struct foc_motor_f32_s float angle_m; /* Motor mechanical angle */ float angle_el; /* Motor electrical angle */ + /* Velocity state *********************************************************/ + +#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL + float vel_el; /* Velocity - electrical */ + float vel_mech; /* Velocity - mechanical */ + float vel_filter; /* Velocity low-pass filter */ +#endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS + float vel_obs; /* Velocity observer output */ +#endif + /* Motor setpoints ********************************************************/ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ @@ -150,6 +161,12 @@ struct foc_motor_f32_s foc_angle_f32_t qenco; /* Qenco angle handler */ char qedpath[32]; /* Qenco devpath */ #endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + foc_velocity_f32_t vel_div; /* DIV velocity observer */ +#endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + foc_velocity_f32_t vel_pll; /* PLL velocity observer */ +#endif }; /**************************************************************************** diff --git a/examples/foc/foc_nxscope.c b/examples/foc/foc_nxscope.c index a6fbde880..101a07ac7 100644 --- a/examples/foc/foc_nxscope.c +++ b/examples/foc/foc_nxscope.c @@ -239,6 +239,9 @@ int foc_nxscope_init(FAR struct foc_nxscope_s *nxs) #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SVM3) nxscope_chan_init(&nxs->nxs, i++, "svm3", u.u8, 4, 0); #endif +#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VOBS) + nxscope_chan_init(&nxs->nxs, i++, "vobs", u.u8, 1, 0); +#endif if (i > CONFIG_EXAMPLES_FOC_NXSCOPE_CHANNELS) { diff --git a/examples/foc/foc_nxscope.h b/examples/foc/foc_nxscope.h index ef7a79a6d..38a998e2a 100644 --- a/examples/foc/foc_nxscope.h +++ b/examples/foc/foc_nxscope.h @@ -52,6 +52,7 @@ #define FOC_NXSCOPE_DQREF (1 << 14) /* DQ reference */ #define FOC_NXSCOPE_VDQCOMP (1 << 15) /* VDQ compensation */ #define FOC_NXSCOPE_SVM3 (1 << 16) /* Space-vector modulation sector */ +#define FOC_NXSCOPE_VOBS (1 << 17) /* Output from velocity observer */ /* Max 32-bit */ /**************************************************************************** diff --git a/examples/foc/foc_parseargs.c b/examples/foc/foc_parseargs.c index fa4de580e..609500eac 100644 --- a/examples/foc/foc_parseargs.c +++ b/examples/foc/foc_parseargs.c @@ -45,6 +45,11 @@ #define OPT_IIV (SCHAR_MAX + 6) #define OPT_IIS (SCHAR_MAX + 7) +#define OPT_VOPLLKP (SCHAR_MAX + 7) +#define OPT_VOPLLKI (SCHAR_MAX + 8) +#define OPT_VODIVS (SCHAR_MAX + 9) +#define OPT_VODIVF (SCHAR_MAX + 10) + /**************************************************************************** * Private Data ****************************************************************************/ @@ -79,6 +84,14 @@ static struct option g_long_options[] = { "irs", required_argument, 0, OPT_IRS }, { "iiv", required_argument, 0, OPT_IIV }, { "iis", required_argument, 0, OPT_IIS }, +#endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + { "vopllkp", required_argument, 0, OPT_VOPLLKP }, + { "vopllki", required_argument, 0, OPT_VOPLLKI }, +#endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + { "vodivs", required_argument, 0, OPT_VODIVS }, + { "vodivf", required_argument, 0, OPT_VODIVF }, #endif { 0, 0, 0, 0 } }; @@ -153,6 +166,18 @@ static void foc_help(void) PRINTF(" [--iis] ind sec (default: %d)\n", CONFIG_EXAMPLES_FOC_IDENT_IND_SEC); #endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + PRINTF(" [--vopllkp] velobs PLL Kp (default: %d)\n", + CONFIG_EXAMPLES_FOC_VELOBS_PLL_KP); + PRINTF(" [--vopllki] velobs PLL Ki (default: %d)\n", + CONFIG_EXAMPLES_FOC_VELOBS_PLL_KI); +#endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + PRINTF(" [--vodivs] velobs DIV samples (default: %d)\n", + CONFIG_EXAMPLES_FOC_VELOBS_DIV_SAMPLES); + PRINTF(" [--vodivf] velobs DIV filter (default: %d)\n", + CONFIG_EXAMPLES_FOC_VELOBS_DIV_FILTER); +#endif } /**************************************************************************** @@ -226,6 +251,34 @@ void parse_args(FAR struct args_s *args, int argc, FAR char **argv) } #endif +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL + case OPT_VOPLLKP: + { + args->cfg.vel_pll_kp = atoi(optarg); + break; + } + + case OPT_VOPLLKI: + { + args->cfg.vel_pll_ki = atoi(optarg); + break; + } +#endif + +#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV + case OPT_VODIVS: + { + args->cfg.vel_div_samples = atoi(optarg); + break; + } + + case OPT_VODIVF: + { + args->cfg.vel_div_filter = atoi(optarg); + break; + } +#endif + case 't': { args->time = atoi(optarg);