diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig index e5a8292fb86..428021e3cb2 100644 --- a/examples/foc/Kconfig +++ b/examples/foc/Kconfig @@ -98,6 +98,37 @@ config EXAMPLES_FOC_SENSORED endchoice # +menu "Motor phy" + +config EXAMPLES_FOC_MOTOR_POLES + int "FOC example motor poles pairs" + default 0 + +config EXAMPLES_FOC_MOTOR_RES + int "FOC example motor phase resistance (x1000000)" + default 0 + ---help--- + The unit is micro-ohm (1/1000000 ohm). + +config EXAMPLES_FOC_MOTOR_IND + int "FOC example motor phase inductance (x1000000)" + default 0 + ---help--- + The unit is micro micro-henry (1/1000000 henry). + +config EXAMPLES_FOC_MOTOR_FLUXLINK + int "FOC example motor flux linkage (x1000000)" + default 0 + ---help--- + The unit is micro-Vs (1/1000000 Vs). + Flux linkage can be obtained from the formula: + lambda_pm = (1 / sqrt(3)) * (60 / 2*PI) * (Ke / P) [Vs] + where: + Ke - motor voltage constant Ke [V/rpm] + P - motor pole pairs + +endmenu # Motor phy + if EXAMPLES_FOC_SENSORED choice @@ -115,10 +146,6 @@ endchoice # FOC sensored sensor selection if EXAMPLES_FOC_HAVE_QENCO -config EXAMPLES_FOC_MOTOR_POLES - int "FOC example motor poles pairs" - default 0 - config EXAMPLES_FOC_QENCO_POSMAX int "FOC example qencoder maximum position" default 0 @@ -148,6 +175,7 @@ endif # EXAMPLES_FOC_SENSORED config EXAMPLES_FOC_HAVE_OPENLOOP bool "FOC example have open-loop controller" select INDUSTRY_FOC_ANGLE_OPENLOOP + select EXAMPLES_FOC_HAVE_ALIGN default EXAMPLES_FOC_SENSORLESS config EXAMPLES_FOC_HAVE_TORQ @@ -162,6 +190,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 44f46c4ab1d..17d64d28905 100644 --- a/examples/foc/foc_cfg.h +++ b/examples/foc/foc_cfg.h @@ -132,17 +132,6 @@ # define FOC_MODEL_INDQ (0.0002f) #endif -/* Motor alignment configuration */ - -#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN -# if CONFIG_EXAMPLES_FOC_ALIGN_VOLT == 0 -# error -# endif -# if CONFIG_EXAMPLES_FOC_ALIGN_SEC == 0 -# error -# endif -#endif - /* Qenco configuration */ #ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO @@ -244,6 +233,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 569b9e70a27..7e6eae3bdef 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 cc362a1da1e..94d8596bec1 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 f4d8f95d2ba..9258a46f666 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 2fe9dfd2cc4..53e614c3f31 100644 --- a/examples/foc/foc_motor_b16.c +++ b/examples/foc/foc_motor_b16.c @@ -362,11 +362,13 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor) foc_handler_cfg_b16(&motor->handler, &ctrl_cfg, &mod_cfg); -#ifdef CONFIG_EXAMPLES_FOC_MOTOR_POLES - /* Configure motor poles */ + /* Configure motor phy */ - motor->poles = CONFIG_EXAMPLES_FOC_MOTOR_POLES; -#endif + motor_phy_params_init_b16(&motor->phy, + CONFIG_EXAMPLES_FOC_MOTOR_POLES, + ftob16(CONFIG_EXAMPLES_FOC_MOTOR_RES / 1000000.0f), + ftob16(CONFIG_EXAMPLES_FOC_MOTOR_IND / 1000000.0f), + ftob16(CONFIG_EXAMPLES_FOC_MOTOR_FLUXLINK / 1000000.0f)); #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM /* Initialize PMSM model */ @@ -573,6 +575,40 @@ static int foc_motor_setpoint(FAR struct foc_motor_b16_s *motor, uint32_t sp) return ret; } +/**************************************************************************** + * Name: foc_motor_vel_reset + ****************************************************************************/ + +static int foc_motor_vel_reset(FAR struct foc_motor_b16_s *motor) +{ + int ret = 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; +} + /**************************************************************************** * Name: foc_motor_state ****************************************************************************/ @@ -629,6 +665,27 @@ static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state) } } +#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP + /* Re-align motor if we change mode from FREE/STOP to CW/CCW otherwise, + * the open-loop may fail because the rotor position at the start is + * random. + */ + + if ((motor->mq.app_state == FOC_EXAMPLE_STATE_FREE || + motor->mq.app_state == FOC_EXAMPLE_STATE_STOP) && + (state == FOC_EXAMPLE_STATE_CW || + state == FOC_EXAMPLE_STATE_CCW)) + { + motor->ctrl_state = FOC_CTRL_STATE_ALIGN; + motor->align_done = false; + motor->angle_now = 0; + + /* Reset velocity observer */ + + foc_motor_vel_reset(motor); + } +#endif + /* Reset current setpoint */ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ @@ -696,9 +753,13 @@ static int foc_motor_start(FAR struct foc_motor_b16_s *motor, bool start) static int foc_motor_run_init(FAR struct foc_motor_b16_s *motor) { - /* Empty for now */ + int ret = OK; - return OK; +#ifdef CONFIG_EXAMPLES_FOC_VELOBS + ret = foc_motor_vel_reset(motor); +#endif + + return ret; } /**************************************************************************** @@ -823,6 +884,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 @@ -936,6 +1003,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 */ @@ -1008,6 +1125,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 @@ -1079,6 +1202,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 */ @@ -1135,9 +1280,13 @@ int foc_motor_deinit(FAR struct foc_motor_b16_s *motor) 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); @@ -1194,7 +1343,8 @@ 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->poles) % MOTOR_ANGLE_E_MAX_B16); + motor->phy.p) % + MOTOR_ANGLE_E_MAX_B16); } else @@ -1218,20 +1368,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_el = motor->vel_obs; +#else + UNUSED(vin); + UNUSED(vout); - motor->vel.now = motor->vel.set; + /* 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 0c3605389b9..843302da897 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 @@ -133,7 +144,7 @@ struct foc_motor_b16_s struct foc_model_b16_s model; /* Model handler */ struct foc_model_state_b16_s model_state; /* PMSM model state */ #endif - uint8_t poles; /* Motor poles */ + struct motor_phy_params_b16_s phy; /* Motor phy */ /* Motor velocity and angle handlers **************************************/ @@ -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 ead7e56b31a..8c9291be2ce 100644 --- a/examples/foc/foc_motor_f32.c +++ b/examples/foc/foc_motor_f32.c @@ -360,11 +360,13 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor) foc_handler_cfg_f32(&motor->handler, &ctrl_cfg, &mod_cfg); -#ifdef CONFIG_EXAMPLES_FOC_MOTOR_POLES - /* Configure motor poles */ + /* Configure motor phy */ - motor->poles = CONFIG_EXAMPLES_FOC_MOTOR_POLES; -#endif + motor_phy_params_init(&motor->phy, + CONFIG_EXAMPLES_FOC_MOTOR_POLES, + CONFIG_EXAMPLES_FOC_MOTOR_RES / 1000000.0f, + CONFIG_EXAMPLES_FOC_MOTOR_IND / 1000000.0f, + CONFIG_EXAMPLES_FOC_MOTOR_FLUXLINK / 1000000.0f); #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM /* Initialize PMSM model */ @@ -551,6 +553,40 @@ static int foc_motor_setpoint(FAR struct foc_motor_f32_s *motor, uint32_t sp) return ret; } +/**************************************************************************** + * Name: foc_motor_vel_reset + ****************************************************************************/ + +static int foc_motor_vel_reset(FAR struct foc_motor_f32_s *motor) +{ + int ret = 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; +} + /**************************************************************************** * Name: foc_motor_state ****************************************************************************/ @@ -607,6 +643,27 @@ static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state) } } +#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP + /* Re-align motor if we change mode from FREE/STOP to CW/CCW otherwise, + * the open-loop may fail because the rotor position at the start is + * random. + */ + + if ((motor->mq.app_state == FOC_EXAMPLE_STATE_FREE || + motor->mq.app_state == FOC_EXAMPLE_STATE_STOP) && + (state == FOC_EXAMPLE_STATE_CW || + state == FOC_EXAMPLE_STATE_CCW)) + { + motor->ctrl_state = FOC_CTRL_STATE_ALIGN; + motor->align_done = false; + motor->angle_now = 0.0f; + + /* Reset velocity observer */ + + foc_motor_vel_reset(motor); + } +#endif + /* Reset current setpoint */ #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ @@ -681,9 +738,13 @@ static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start) static int foc_motor_run_init(FAR struct foc_motor_f32_s *motor) { - /* Empty for now */ + int ret = OK; - return OK; +#ifdef CONFIG_EXAMPLES_FOC_VELOBS + ret = foc_motor_vel_reset(motor); +#endif + + return ret; } /**************************************************************************** @@ -808,6 +869,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 @@ -921,6 +988,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 */ @@ -990,6 +1107,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 @@ -1066,6 +1189,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 */ @@ -1122,9 +1267,13 @@ int foc_motor_deinit(FAR struct foc_motor_f32_s *motor) 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); @@ -1180,7 +1329,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->poles, + motor->angle_el = fmodf(motor->angle_m * motor->phy.p, MOTOR_ANGLE_E_MAX); } @@ -1205,20 +1354,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_el = motor->vel_obs; +#else + UNUSED(vin); + UNUSED(vout); + + /* No velocity feedback - assume that electical velocity is + * velocity set + */ - motor->vel.now = motor->vel.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 fb4767885ee..84d8a3c7b88 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 @@ -133,7 +144,7 @@ struct foc_motor_f32_s struct foc_model_f32_s model; /* Model handler */ struct foc_model_state_f32_s model_state; /* PMSM model state */ #endif - uint8_t poles; /* Motor poles */ + struct motor_phy_params_f32_s phy; /* Motor phy */ /* Motor velocity and angle handlers **************************************/ @@ -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 a6fbde8801a..101a07ac78a 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 ef7a79a6dc2..38a998e2a13 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 fa4de580e24..609500eac0d 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);