diff --git a/examples/foc/Makefile b/examples/foc/Makefile index 96e9e7346..1fb157be4 100644 --- a/examples/foc/Makefile +++ b/examples/foc/Makefile @@ -41,13 +41,13 @@ endif # fixed16 support ifeq ($(CONFIG_INDUSTRY_FOC_FIXED16),y) - CSRCS += foc_fixed16_thr.c + CSRCS += foc_fixed16_thr.c foc_motor_b16.c endif # float32 support ifeq ($(CONFIG_INDUSTRY_FOC_FLOAT),y) - CSRCS += foc_float_thr.c + CSRCS += foc_float_thr.c foc_motor_f32.c endif include $(APPDIR)/Application.mk diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c index c86b928cf..ce816bb3c 100644 --- a/examples/foc/foc_fixed16_thr.c +++ b/examples/foc/foc_fixed16_thr.c @@ -30,22 +30,12 @@ #include -#include "foc_mq.h" -#include "foc_thr.h" #include "foc_cfg.h" -#include "foc_adc.h" - #include "foc_debug.h" +#include "foc_motor_b16.h" #include "industry/foc/foc_utils.h" #include "industry/foc/foc_common.h" -#include "industry/foc/fixed16/foc_handler.h" -#include "industry/foc/fixed16/foc_ramp.h" -#include "industry/foc/fixed16/foc_angle.h" -#include "industry/foc/fixed16/foc_velocity.h" -#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM -# include "industry/foc/fixed16/foc_model.h" -#endif /**************************************************************************** * Pre-processor Definitions @@ -59,87 +49,10 @@ * Private Type Definition ****************************************************************************/ -/* FOC motor data */ - -struct foc_motor_b16_s -{ - FAR struct foc_ctrl_env_s *envp; /* Thread env */ - bool fault; /* Fault flag */ - bool startstop; /* Start/stop request */ -#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP - bool openloop_now; /* Open-loop now */ - b16_t angle_ol; /* Phase angle open-loop */ - foc_angle_b16_t openloop; /* Open-loop angle handler */ -#endif - int foc_mode; /* FOC mode */ - b16_t vbus; /* Power bus voltage */ - b16_t angle_now; /* Phase angle now */ - b16_t vel_set; /* Velocity setting now */ - b16_t vel_now; /* Velocity now */ - b16_t vel_des; /* Velocity destination */ - b16_t dir; /* Motor's direction */ - b16_t per; /* Controller period in seconds */ - b16_t iphase_adc; /* Iphase ADC scaling factor */ - b16_t pwm_duty_max; /* PWM duty max */ - dq_frame_b16_t dq_ref; /* DQ reference */ - dq_frame_b16_t vdq_comp; /* DQ voltage compensation */ - foc_handler_b16_t handler; /* FOC controller */ - struct foc_mq_s mq; /* MQ data */ - struct foc_state_b16_s foc_state; /* FOC controller sate */ - struct foc_ramp_b16_s ramp; /* Velocity ramp data */ -#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM - struct foc_model_b16_s model; /* Model handler */ - struct foc_model_state_b16_s model_state; /* PMSM model state */ -#endif -}; - /**************************************************************************** * Private Functions ****************************************************************************/ -/**************************************************************************** - * Name: foc_motor_init - ****************************************************************************/ - -static int foc_motor_init(FAR struct foc_motor_b16_s *motor, - FAR struct foc_ctrl_env_s *envp) -{ -#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP - struct foc_openloop_cfg_b16_s ol_cfg; -#endif - int ret = OK; - - DEBUGASSERT(motor); - DEBUGASSERT(envp); - - /* Reset data */ - - memset(motor, 0, sizeof(struct foc_motor_b16_s)); - - /* Connect envp with motor handler */ - - motor->envp = envp; - - /* Initialize motor data */ - - motor->per = b16divi(b16ONE, CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ); - motor->iphase_adc = ftob16((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f); - -#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP - /* Initialize open-loop angle handler */ - - foc_angle_init_b16(&motor->openloop, - &g_foc_angle_ol_b16); - - /* Configure open-loop angle handler */ - - ol_cfg.per = motor->per; - foc_angle_cfg_b16(&motor->openloop, &ol_cfg); -#endif - - return ret; -} - /**************************************************************************** * Name: foc_mode_init ****************************************************************************/ @@ -184,362 +97,6 @@ errout: return ret; } -/**************************************************************************** - * Name: foc_motor_configure - ****************************************************************************/ - -static int foc_motor_configure(FAR struct foc_motor_b16_s *motor) -{ -#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI - struct foc_initdata_b16_s ctrl_cfg; -#endif -#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 - struct foc_mod_cfg_b16_s mod_cfg; -#endif -#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM - struct foc_model_pmsm_cfg_b16_s pmsm_cfg; -#endif - int ret = OK; - - DEBUGASSERT(motor); - - /* Initialize velocity ramp */ - - ret = foc_ramp_init_b16(&motor->ramp, - motor->per, - ftob16(RAMP_CFG_THR), - ftob16(RAMP_CFG_ACC), - ftob16(RAMP_CFG_ACC)); - if (ret < 0) - { - PRINTF("ERROR: foc_ramp_init failed %d\n", ret); - goto errout; - } - - /* Initialize FOC handler */ - - ret = foc_handler_init_b16(&motor->handler, - &g_foc_control_pi_b16, - &g_foc_mod_svm3_b16); - if (ret < 0) - { - PRINTF("ERROR: foc_handler_init failed %d\n", ret); - goto errout; - } - -#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI - /* Get PI controller configuration */ - - ctrl_cfg.id_kp = ftob16(motor->envp->pi_kp / 1000.0f); - ctrl_cfg.id_ki = ftob16(motor->envp->pi_ki / 1000.0f); - ctrl_cfg.iq_kp = ftob16(motor->envp->pi_kp / 1000.0f); - ctrl_cfg.iq_ki = ftob16(motor->envp->pi_ki / 1000.0f); -#endif - -#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 - /* Get SVM3 modulation configuration */ - - mod_cfg.pwm_duty_max = motor->pwm_duty_max; -#endif - - /* Configure FOC handler */ - - foc_handler_cfg_b16(&motor->handler, &ctrl_cfg, &mod_cfg); - -#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM - /* Initialize PMSM model */ - - ret = foc_model_init_b16(&motor->model, - &g_foc_model_pmsm_ops_b16); - if (ret < 0) - { - PRINTF("ERROR: foc_model_init failed %d\n", ret); - goto errout; - } - - /* Get PMSM model configuration */ - - pmsm_cfg.poles = FOC_MODEL_POLES; - pmsm_cfg.res = ftob16(FOC_MODEL_RES); - pmsm_cfg.ind = ftob16(FOC_MODEL_IND); - pmsm_cfg.iner = ftob16(FOC_MODEL_INER); - pmsm_cfg.flux_link = ftob16(FOC_MODEL_FLUX); - pmsm_cfg.ind_d = ftob16(FOC_MODEL_INDD); - pmsm_cfg.ind_q = ftob16(FOC_MODEL_INDQ); - pmsm_cfg.per = motor->per; - pmsm_cfg.iphase_adc = motor->iphase_adc; - - /* Configure PMSM model */ - - foc_model_cfg_b16(&motor->model, &pmsm_cfg); -#endif - -errout: - return ret; -} - -/**************************************************************************** - * Name: foc_motor_start - ****************************************************************************/ - -static int foc_motor_start(FAR struct foc_motor_b16_s *motor, bool start) -{ - int ret = OK; - - DEBUGASSERT(motor); - - if (start == true) - { - /* Start motor if VBUS data present */ - - if (motor->mq.vbus > 0) - { - /* Configure motor controller */ - - PRINTF("Configure motor %d!\n", motor->envp->id); - - ret = foc_motor_configure(motor); - if (ret < 0) - { - PRINTF("ERROR: foc_motor_configure failed %d!\n", ret); - goto errout; - } - - /* Start/stop FOC dev request */ - - motor->startstop = true; - } - } - else - { - /* Start/stop FOC dev request */ - - motor->startstop = true; - } - -errout: - return ret; -} - -/**************************************************************************** - * Name: foc_motor_deinit - ****************************************************************************/ - -static int foc_motor_deinit(FAR struct foc_motor_b16_s *motor) -{ - int ret = OK; - - DEBUGASSERT(motor); - -#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM - /* Deinitialize PMSM model */ - - ret = foc_model_deinit_b16(&motor->model); - if (ret < 0) - { - PRINTF("ERROR: foc_model_deinit failed %d\n", ret); - goto errout; - } -#endif - - /* Deinitialize FOC handler */ - - ret = foc_handler_deinit_b16(&motor->handler); - if (ret < 0) - { - PRINTF("ERROR: foc_handler_deinit failed %d\n", ret); - goto errout; - } - - /* Reset data */ - - memset(motor, 0, sizeof(struct foc_motor_b16_s)); - -errout: - return ret; -} - -/**************************************************************************** - * Name: foc_motor_vbus - ****************************************************************************/ - -static int foc_motor_vbus(FAR struct foc_motor_b16_s *motor, uint32_t vbus) -{ - DEBUGASSERT(motor); - - /* Update motor VBUS */ - - motor->vbus = b16muli(vbus, ftob16(VBUS_ADC_SCALE)); - - return OK; -} - -/**************************************************************************** - * Name: foc_motor_vel - ****************************************************************************/ - -static int foc_motor_vel(FAR struct foc_motor_b16_s *motor, uint32_t vel) -{ - b16_t tmp1 = 0; - b16_t tmp2 = 0; - - DEBUGASSERT(motor); - - /* Update motor velocity destination - * NOTE: velmax may not fit in b16_t so we can't use b16idiv() - */ - - tmp1 = itob16(motor->envp->velmax / 1000); - tmp2 = b16mulb16(ftob16(SETPOINT_ADC_SCALE), tmp1); - - motor->vel_des = b16muli(tmp2, vel); - - return OK; -} - -/**************************************************************************** - * Name: foc_motor_state - ****************************************************************************/ - -static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state) -{ - int ret = OK; - - DEBUGASSERT(motor); - - /* Get open-loop currents - * NOTE: Id always set to 0 - */ - - motor->dq_ref.q = b16idiv(motor->envp->qparam, 1000); - motor->dq_ref.d = 0; - - /* Update motor state */ - - switch (state) - { - case FOC_EXAMPLE_STATE_FREE: - { - motor->vel_set = 0; - motor->dir = DIR_NONE_B16; - - /* Force DQ vector to zero */ - - motor->dq_ref.q = 0; - motor->dq_ref.d = 0; - - break; - } - - case FOC_EXAMPLE_STATE_STOP: - { - motor->vel_set = 0; - motor->dir = DIR_NONE_B16; - - /* DQ vector not zero - active brake */ - - break; - } - - case FOC_EXAMPLE_STATE_CW: - { - motor->vel_set = 0; - motor->dir = DIR_CW_B16; - - break; - } - - case FOC_EXAMPLE_STATE_CCW: - { - motor->vel_set = 0; - motor->dir = DIR_CCW_B16; - - break; - } - - default: - { - ret = -EINVAL; - goto errout; - } - } - -errout: - return ret; -} - -/**************************************************************************** - * Name: foc_motor_get - ****************************************************************************/ - -static 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; - - DEBUGASSERT(motor); - - /* Update open-loop angle handler */ - - ain.vel = motor->vel_set; - ain.angle = motor->angle_now; - ain.dir = motor->dir; - -#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP - foc_angle_run_b16(&motor->openloop, &ain, &aout); - - /* Store open-loop angle */ - - motor->angle_ol = aout.angle; - - /* Get phase angle now */ - - if (motor->openloop_now == true) - { - motor->angle_now = motor->angle_ol; - } - else -#endif - { - /* TODO: get phase angle from observer or sensor */ - - ASSERT(0); - } - - return ret; -} - -/**************************************************************************** - * Name: foc_motor_control - ****************************************************************************/ - -static int foc_motor_control(FAR struct foc_motor_b16_s *motor) -{ - int ret = OK; - - DEBUGASSERT(motor); - - /* No velocity feedback - assume that velocity now is velocity set - * TODO: velocity observer or sensor - */ - - motor->vel_now = motor->vel_set; - - /* Run velocity ramp controller */ - - ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des, - motor->vel_now, &motor->vel_set); - if (ret < 0) - { - PRINTF("ERROR: foc_ramp_run failed %d\n", ret); - goto errout; - } - -errout: - return ret; -} - /**************************************************************************** * Name: foc_handler_run ****************************************************************************/ @@ -651,99 +208,6 @@ static int foc_state_print(FAR struct foc_motor_b16_s *motor) } #endif -/**************************************************************************** - * Name: foc_motor_handle - ****************************************************************************/ - -static int foc_motor_handle(FAR struct foc_motor_b16_s *motor, - FAR struct foc_mq_s *handle) -{ - int ret = OK; - - DEBUGASSERT(motor); - DEBUGASSERT(handle); - - /* Terminate */ - - if (handle->quit == true) - { - motor->mq.quit = true; - } - - /* Update motor VBUS */ - - if (motor->mq.vbus != handle->vbus) - { - PRINTFV("Set vbus=%" PRIu32 " for FOC driver %d!\n", - handle->vbus, motor->envp->id); - - ret = foc_motor_vbus(motor, handle->vbus); - if (ret < 0) - { - PRINTF("ERROR: foc_motor_vbus failed %d!\n", ret); - goto errout; - } - - motor->mq.vbus = handle->vbus; - } - - /* Update motor velocity destination */ - - if (motor->mq.setpoint != handle->setpoint) - { - PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n", - handle->setpoint, motor->envp->id); - - ret = foc_motor_vel(motor, handle->setpoint); - if (ret < 0) - { - PRINTF("ERROR: foc_motor_vel failed %d!\n", ret); - goto errout; - } - - motor->mq.setpoint = handle->setpoint; - } - - /* Update motor state */ - - if (motor->mq.app_state != handle->app_state) - { - PRINTFV("Set app_state=%d for FOC driver %d!\n", - handle->app_state, motor->envp->id); - - ret = foc_motor_state(motor, handle->app_state); - if (ret < 0) - { - PRINTF("ERROR: foc_motor_state failed %d!\n", ret); - goto errout; - } - - motor->mq.app_state = handle->app_state; - } - - /* Start/stop controller */ - - if (motor->mq.start != handle->start) - { - PRINTFV("Set start=%d for FOC driver %d!\n", - handle->start, motor->envp->id); - - /* Start/stop motor controller */ - - ret = foc_motor_start(motor, handle->start); - if (ret < 0) - { - PRINTF("ERROR: foc_motor_start failed %d!\n", ret); - goto errout; - } - - motor->mq.start = handle->start; - } - -errout: - return ret; -} - /**************************************************************************** * Public Functions ****************************************************************************/ diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c index fae538286..0db14bec9 100644 --- a/examples/foc/foc_float_thr.c +++ b/examples/foc/foc_float_thr.c @@ -30,22 +30,12 @@ #include -#include "foc_mq.h" -#include "foc_thr.h" #include "foc_cfg.h" -#include "foc_adc.h" - #include "foc_debug.h" +#include "foc_motor_f32.h" #include "industry/foc/foc_utils.h" #include "industry/foc/foc_common.h" -#include "industry/foc/float/foc_handler.h" -#include "industry/foc/float/foc_ramp.h" -#include "industry/foc/float/foc_angle.h" -#include "industry/foc/float/foc_velocity.h" -#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM -# include "industry/foc/float/foc_model.h" -#endif /**************************************************************************** * Pre-processor Definitions @@ -59,87 +49,10 @@ * Private Type Definition ****************************************************************************/ -/* FOC motor data */ - -struct foc_motor_f32_s -{ - FAR struct foc_ctrl_env_s *envp; /* Thread env */ - bool fault; /* Fault flag */ - bool startstop; /* Start/stop request */ -#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP - bool openloop_now; /* Open-loop now */ - float angle_ol; /* Phase angle open-loop */ - foc_angle_f32_t openloop; /* Open-loop angle handler */ -#endif - int foc_mode; /* FOC mode */ - float vbus; /* Power bus voltage */ - float angle_now; /* Phase angle now */ - float vel_set; /* Velocity setting now */ - float vel_now; /* Velocity now */ - float vel_des; /* Velocity destination */ - float dir; /* Motor's direction */ - float per; /* Controller period in seconds */ - float iphase_adc; /* Iphase ADC scaling factor */ - float pwm_duty_max; /* PWM duty max */ - dq_frame_f32_t dq_ref; /* DQ reference */ - dq_frame_f32_t vdq_comp; /* DQ voltage compensation */ - foc_handler_f32_t handler; /* FOC controller */ - struct foc_mq_s mq; /* MQ data */ - struct foc_state_f32_s foc_state; /* FOC controller sate */ - struct foc_ramp_f32_s ramp; /* Velocity ramp data */ -#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM - struct foc_model_f32_s model; /* Model handler */ - struct foc_model_state_f32_s model_state; /* PMSM model state */ -#endif -}; - /**************************************************************************** * Private Functions ****************************************************************************/ -/**************************************************************************** - * Name: foc_motor_init - ****************************************************************************/ - -static int foc_motor_init(FAR struct foc_motor_f32_s *motor, - FAR struct foc_ctrl_env_s *envp) -{ -#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP - struct foc_openloop_cfg_f32_s ol_cfg; -#endif - int ret = OK; - - DEBUGASSERT(motor); - DEBUGASSERT(envp); - - /* Reset data */ - - memset(motor, 0, sizeof(struct foc_motor_f32_s)); - - /* Connect envp with motor handler */ - - motor->envp = envp; - - /* Initialize motor data */ - - motor->per = (float)(1.0f / CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ); - motor->iphase_adc = ((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f); - -#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP - /* Initialize open-loop angle handler */ - - foc_angle_init_f32(&motor->openloop, - &g_foc_angle_ol_f32); - - /* Configure open-loop angle handler */ - - ol_cfg.per = motor->per; - foc_angle_cfg_f32(&motor->openloop, &ol_cfg); -#endif - - return ret; -} - /**************************************************************************** * Name: foc_mode_init ****************************************************************************/ @@ -184,362 +97,6 @@ errout: return ret; } -/**************************************************************************** - * Name: foc_motor_configure - ****************************************************************************/ - -static int foc_motor_configure(FAR struct foc_motor_f32_s *motor) -{ -#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI - struct foc_initdata_f32_s ctrl_cfg; -#endif -#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 - struct foc_mod_cfg_f32_s mod_cfg; -#endif -#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM - struct foc_model_pmsm_cfg_f32_s pmsm_cfg; -#endif - int ret = OK; - - DEBUGASSERT(motor); - - /* Initialize velocity ramp */ - - ret = foc_ramp_init_f32(&motor->ramp, - motor->per, - RAMP_CFG_THR, - RAMP_CFG_ACC, - RAMP_CFG_ACC); - if (ret < 0) - { - PRINTF("ERROR: foc_ramp_init failed %d\n", ret); - goto errout; - } - - /* Initialize FOC handler */ - - ret = foc_handler_init_f32(&motor->handler, - &g_foc_control_pi_f32, - &g_foc_mod_svm3_f32); - if (ret < 0) - { - PRINTF("ERROR: foc_handler_init failed %d\n", ret); - goto errout; - } - -#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI - /* Get PI controller configuration */ - - ctrl_cfg.id_kp = (motor->envp->pi_kp / 1000.0f); - ctrl_cfg.id_ki = (motor->envp->pi_ki / 1000.0f); - ctrl_cfg.iq_kp = (motor->envp->pi_kp / 1000.0f); - ctrl_cfg.iq_ki = (motor->envp->pi_ki / 1000.0f); -#endif - -#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 - /* Get SVM3 modulation configuration */ - - mod_cfg.pwm_duty_max = motor->pwm_duty_max; -#endif - - /* Configure FOC handler */ - - foc_handler_cfg_f32(&motor->handler, &ctrl_cfg, &mod_cfg); - -#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM - /* Initialize PMSM model */ - - ret = foc_model_init_f32(&motor->model, - &g_foc_model_pmsm_ops_f32); - if (ret < 0) - { - PRINTF("ERROR: foc_model_init failed %d\n", ret); - goto errout; - } - - /* Get PMSM model configuration */ - - pmsm_cfg.poles = FOC_MODEL_POLES; - pmsm_cfg.res = FOC_MODEL_RES; - pmsm_cfg.ind = FOC_MODEL_IND; - pmsm_cfg.iner = FOC_MODEL_INER; - pmsm_cfg.flux_link = FOC_MODEL_FLUX; - pmsm_cfg.ind_d = FOC_MODEL_INDD; - pmsm_cfg.ind_q = FOC_MODEL_INDQ; - pmsm_cfg.per = motor->per; - pmsm_cfg.iphase_adc = motor->iphase_adc; - - /* Configure PMSM model */ - - foc_model_cfg_f32(&motor->model, &pmsm_cfg); -#endif - -errout: - return ret; -} - -/**************************************************************************** - * Name: foc_motor_start - ****************************************************************************/ - -static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start) -{ - int ret = OK; - - DEBUGASSERT(motor); - - if (start == true) - { - /* Start motor if VBUS data present */ - - if (motor->mq.vbus > 0) - { - /* Configure motor controller */ - - PRINTF("Configure motor %d!\n", motor->envp->id); - - ret = foc_motor_configure(motor); - if (ret < 0) - { - PRINTF("ERROR: foc_motor_configure failed %d!\n", ret); - goto errout; - } - - /* Start/stop FOC dev request */ - - motor->startstop = true; - } - else - { - /* Return error if no VBUS data */ - - PRINTF("ERROR: start request without VBUS!\n"); - goto errout; - } - } - else - { - /* Start/stop FOC dev request */ - - motor->startstop = true; - } - -errout: - return ret; -} - -/**************************************************************************** - * Name: foc_motor_deinit - ****************************************************************************/ - -static int foc_motor_deinit(FAR struct foc_motor_f32_s *motor) -{ - int ret = OK; - - DEBUGASSERT(motor); - -#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM - /* Deinitialize PMSM model */ - - ret = foc_model_deinit_f32(&motor->model); - if (ret < 0) - { - PRINTF("ERROR: foc_model_deinit failed %d\n", ret); - goto errout; - } -#endif - - /* Deinitialize FOC handler */ - - ret = foc_handler_deinit_f32(&motor->handler); - if (ret < 0) - { - PRINTF("ERROR: foc_handler_deinit failed %d\n", ret); - goto errout; - } - - /* Reset data */ - - memset(motor, 0, sizeof(struct foc_motor_f32_s)); - -errout: - return ret; -} - -/**************************************************************************** - * Name: foc_motor_vbus - ****************************************************************************/ - -static int foc_motor_vbus(FAR struct foc_motor_f32_s *motor, uint32_t vbus) -{ - DEBUGASSERT(motor); - - /* Update motor VBUS */ - - motor->vbus = (vbus * VBUS_ADC_SCALE); - - return OK; -} - -/**************************************************************************** - * Name: foc_motor_vel - ****************************************************************************/ - -static int foc_motor_vel(FAR struct foc_motor_f32_s *motor, uint32_t vel) -{ - DEBUGASSERT(motor); - - /* Update motor velocity destination */ - - motor->vel_des = (vel * SETPOINT_ADC_SCALE * - motor->envp->velmax / 1000.0f); - - return OK; -} - -/**************************************************************************** - * Name: foc_motor_state - ****************************************************************************/ - -static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state) -{ - int ret = OK; - - DEBUGASSERT(motor); - - /* Get open-loop currents - * NOTE: Id always set to 0 - */ - - motor->dq_ref.q = (motor->envp->qparam / 1000.0f); - motor->dq_ref.d = 0.0f; - - /* Update motor state */ - - switch (state) - { - case FOC_EXAMPLE_STATE_FREE: - { - motor->vel_set = 0.0f; - motor->dir = DIR_NONE; - - /* Force DQ vector to zero */ - - motor->dq_ref.q = 0.0f; - motor->dq_ref.d = 0.0f; - - break; - } - - case FOC_EXAMPLE_STATE_STOP: - { - motor->vel_set = 0.0f; - motor->dir = DIR_NONE; - - /* DQ vector not zero - active brake */ - - break; - } - - case FOC_EXAMPLE_STATE_CW: - { - motor->vel_set = 0.0f; - motor->dir = DIR_CW; - - break; - } - - case FOC_EXAMPLE_STATE_CCW: - { - motor->vel_set = 0.0f; - motor->dir = DIR_CCW; - - break; - } - - default: - { - ret = -EINVAL; - goto errout; - } - } - -errout: - return ret; -} - -/**************************************************************************** - * Name: foc_motor_get - ****************************************************************************/ - -static 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; - - DEBUGASSERT(motor); - - /* Update open-loop angle handler */ - - ain.vel = motor->vel_set; - ain.angle = motor->angle_now; - ain.dir = motor->dir; - -#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP - foc_angle_run_f32(&motor->openloop, &ain, &aout); - - /* Store open-loop angle */ - - motor->angle_ol = aout.angle; - - /* Get phase angle now */ - - if (motor->openloop_now == true) - { - motor->angle_now = motor->angle_ol; - } - else -#endif - { - /* TODO: get phase angle from observer or sensor */ - - ASSERT(0); - } - - return ret; -} - -/**************************************************************************** - * Name: foc_motor_control - ****************************************************************************/ - -static int foc_motor_control(FAR struct foc_motor_f32_s *motor) -{ - int ret = OK; - - DEBUGASSERT(motor); - - /* No velocity feedback - assume that velocity now is velocity set - * TODO: velocity observer or sensor - */ - - motor->vel_now = motor->vel_set; - - /* Run velocity ramp controller */ - - ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des, - motor->vel_now, &motor->vel_set); - if (ret < 0) - { - PRINTF("ERROR: foc_ramp_run failed %d\n", ret); - goto errout; - } - -errout: - return ret; -} - /**************************************************************************** * Name: foc_handler_run ****************************************************************************/ @@ -652,101 +209,6 @@ static int foc_state_print(FAR struct foc_motor_f32_s *motor) } #endif -/**************************************************************************** - * Name: foc_motor_handle - ****************************************************************************/ - -static int foc_motor_handle(FAR struct foc_motor_f32_s *motor, - FAR struct foc_mq_s *handle) -{ - int ret = OK; - - DEBUGASSERT(motor); - DEBUGASSERT(handle); - - /* Terminate */ - - if (handle->quit == true) - { - motor->mq.quit = true; - } - - /* Update motor VBUS */ - - if (motor->mq.vbus != handle->vbus) - { - PRINTFV("Set vbus=%" PRIu32 " for FOC driver %d!\n", - handle->vbus, motor->envp->id); - - ret = foc_motor_vbus(motor, handle->vbus); - if (ret < 0) - { - PRINTF("ERROR: foc_motor_vbus failed %d!\n", ret); - goto errout; - } - - motor->mq.vbus = handle->vbus; - } - - /* Update motor velocity destination - * NOTE: only velocity control supported for now - */ - - if (motor->mq.setpoint != handle->setpoint) - { - PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n", - handle->setpoint, motor->envp->id); - - ret = foc_motor_vel(motor, handle->setpoint); - if (ret < 0) - { - PRINTF("ERROR: foc_motor_vel failed %d!\n", ret); - goto errout; - } - - motor->mq.setpoint = handle->setpoint; - } - - /* Update motor state */ - - if (motor->mq.app_state != handle->app_state) - { - PRINTFV("Set app_state=%d for FOC driver %d!\n", - handle->app_state, motor->envp->id); - - ret = foc_motor_state(motor, handle->app_state); - if (ret < 0) - { - PRINTF("ERROR: foc_motor_state failed %d!\n", ret); - goto errout; - } - - motor->mq.app_state = handle->app_state; - } - - /* Start/stop controller */ - - if (motor->mq.start != handle->start) - { - PRINTFV("Set start=%d for FOC driver %d!\n", - handle->start, motor->envp->id); - - /* Start/stop motor controller */ - - ret = foc_motor_start(motor, handle->start); - if (ret < 0) - { - PRINTF("ERROR: foc_motor_start failed %d!\n", ret); - goto errout; - } - - motor->mq.start = handle->start; - } - -errout: - return ret; -} - /**************************************************************************** * Public Functions ****************************************************************************/ diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c new file mode 100644 index 000000000..ab543f243 --- /dev/null +++ b/examples/foc/foc_motor_b16.c @@ -0,0 +1,541 @@ +/**************************************************************************** + * apps/examples/foc/foc_motor_b16.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include "foc_motor_b16.h" + +#include "foc_cfg.h" +#include "foc_adc.h" +#include "foc_debug.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Definition + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_motor_configure + ****************************************************************************/ + +static int foc_motor_configure(FAR struct foc_motor_b16_s *motor) +{ +#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI + struct foc_initdata_b16_s ctrl_cfg; +#endif +#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 + struct foc_mod_cfg_b16_s mod_cfg; +#endif +#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM + struct foc_model_pmsm_cfg_b16_s pmsm_cfg; +#endif + int ret = OK; + + DEBUGASSERT(motor); + + /* Initialize velocity ramp */ + + ret = foc_ramp_init_b16(&motor->ramp, + motor->per, + ftob16(RAMP_CFG_THR), + ftob16(RAMP_CFG_ACC), + ftob16(RAMP_CFG_ACC)); + if (ret < 0) + { + PRINTF("ERROR: foc_ramp_init failed %d\n", ret); + goto errout; + } + + /* Initialize FOC handler */ + + ret = foc_handler_init_b16(&motor->handler, + &g_foc_control_pi_b16, + &g_foc_mod_svm3_b16); + if (ret < 0) + { + PRINTF("ERROR: foc_handler_init failed %d\n", ret); + goto errout; + } + +#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI + /* Get PI controller configuration */ + + ctrl_cfg.id_kp = ftob16(motor->envp->pi_kp / 1000.0f); + ctrl_cfg.id_ki = ftob16(motor->envp->pi_ki / 1000.0f); + ctrl_cfg.iq_kp = ftob16(motor->envp->pi_kp / 1000.0f); + ctrl_cfg.iq_ki = ftob16(motor->envp->pi_ki / 1000.0f); +#endif + +#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 + /* Get SVM3 modulation configuration */ + + mod_cfg.pwm_duty_max = motor->pwm_duty_max; +#endif + + /* Configure FOC handler */ + + foc_handler_cfg_b16(&motor->handler, &ctrl_cfg, &mod_cfg); + +#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM + /* Initialize PMSM model */ + + ret = foc_model_init_b16(&motor->model, + &g_foc_model_pmsm_ops_b16); + if (ret < 0) + { + PRINTF("ERROR: foc_model_init failed %d\n", ret); + goto errout; + } + + /* Get PMSM model configuration */ + + pmsm_cfg.poles = FOC_MODEL_POLES; + pmsm_cfg.res = ftob16(FOC_MODEL_RES); + pmsm_cfg.ind = ftob16(FOC_MODEL_IND); + pmsm_cfg.iner = ftob16(FOC_MODEL_INER); + pmsm_cfg.flux_link = ftob16(FOC_MODEL_FLUX); + pmsm_cfg.ind_d = ftob16(FOC_MODEL_INDD); + pmsm_cfg.ind_q = ftob16(FOC_MODEL_INDQ); + pmsm_cfg.per = motor->per; + pmsm_cfg.iphase_adc = motor->iphase_adc; + + /* Configure PMSM model */ + + foc_model_cfg_b16(&motor->model, &pmsm_cfg); +#endif + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_motor_vbus + ****************************************************************************/ + +static int foc_motor_vbus(FAR struct foc_motor_b16_s *motor, uint32_t vbus) +{ + DEBUGASSERT(motor); + + /* Update motor VBUS */ + + motor->vbus = b16muli(vbus, ftob16(VBUS_ADC_SCALE)); + + return OK; +} + +/**************************************************************************** + * Name: foc_motor_vel + ****************************************************************************/ + +static int foc_motor_vel(FAR struct foc_motor_b16_s *motor, uint32_t vel) +{ + b16_t tmp1 = 0; + b16_t tmp2 = 0; + + DEBUGASSERT(motor); + + /* Update motor velocity destination + * NOTE: velmax may not fit in b16_t so we can't use b16idiv() + */ + + tmp1 = itob16(motor->envp->velmax / 1000); + tmp2 = b16mulb16(ftob16(SETPOINT_ADC_SCALE), tmp1); + + motor->vel_des = b16muli(tmp2, vel); + + return OK; +} + +/**************************************************************************** + * Name: foc_motor_state + ****************************************************************************/ + +static int foc_motor_state(FAR struct foc_motor_b16_s *motor, int state) +{ + int ret = OK; + + DEBUGASSERT(motor); + + /* Get open-loop currents + * NOTE: Id always set to 0 + */ + + motor->dq_ref.q = b16idiv(motor->envp->qparam, 1000); + motor->dq_ref.d = 0; + + /* Update motor state */ + + switch (state) + { + case FOC_EXAMPLE_STATE_FREE: + { + motor->vel_set = 0; + motor->dir = DIR_NONE_B16; + + /* Force DQ vector to zero */ + + motor->dq_ref.q = 0; + motor->dq_ref.d = 0; + + break; + } + + case FOC_EXAMPLE_STATE_STOP: + { + motor->vel_set = 0; + motor->dir = DIR_NONE_B16; + + /* DQ vector not zero - active brake */ + + break; + } + + case FOC_EXAMPLE_STATE_CW: + { + motor->vel_set = 0; + motor->dir = DIR_CW_B16; + + break; + } + + case FOC_EXAMPLE_STATE_CCW: + { + motor->vel_set = 0; + motor->dir = DIR_CCW_B16; + + break; + } + + default: + { + ret = -EINVAL; + goto errout; + } + } + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_motor_start + ****************************************************************************/ + +static int foc_motor_start(FAR struct foc_motor_b16_s *motor, bool start) +{ + int ret = OK; + + DEBUGASSERT(motor); + + if (start == true) + { + /* Start motor if VBUS data present */ + + if (motor->mq.vbus > 0) + { + /* Configure motor controller */ + + PRINTF("Configure motor %d!\n", motor->envp->id); + + ret = foc_motor_configure(motor); + if (ret < 0) + { + PRINTF("ERROR: foc_motor_configure failed %d!\n", ret); + goto errout; + } + + /* Start/stop FOC dev request */ + + motor->startstop = true; + } + } + else + { + /* Start/stop FOC dev request */ + + motor->startstop = true; + } + +errout: + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_motor_init + ****************************************************************************/ + +int foc_motor_init(FAR struct foc_motor_b16_s *motor, + FAR struct foc_ctrl_env_s *envp) +{ +#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP + struct foc_openloop_cfg_b16_s ol_cfg; +#endif + int ret = OK; + + DEBUGASSERT(motor); + DEBUGASSERT(envp); + + /* Reset data */ + + memset(motor, 0, sizeof(struct foc_motor_b16_s)); + + /* Connect envp with motor handler */ + + motor->envp = envp; + + /* Initialize motor data */ + + motor->per = b16divi(b16ONE, CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ); + motor->iphase_adc = ftob16((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f); + +#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP + /* Initialize open-loop angle handler */ + + foc_angle_init_b16(&motor->openloop, + &g_foc_angle_ol_b16); + + /* Configure open-loop angle handler */ + + ol_cfg.per = motor->per; + foc_angle_cfg_b16(&motor->openloop, &ol_cfg); +#endif + + return ret; +} + +/**************************************************************************** + * Name: foc_motor_deinit + ****************************************************************************/ + +int foc_motor_deinit(FAR struct foc_motor_b16_s *motor) +{ + int ret = OK; + + DEBUGASSERT(motor); + +#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM + /* Deinitialize PMSM model */ + + ret = foc_model_deinit_b16(&motor->model); + if (ret < 0) + { + PRINTF("ERROR: foc_model_deinit failed %d\n", ret); + goto errout; + } +#endif + + /* Deinitialize FOC handler */ + + ret = foc_handler_deinit_b16(&motor->handler); + if (ret < 0) + { + PRINTF("ERROR: foc_handler_deinit failed %d\n", ret); + goto errout; + } + + /* Reset data */ + + memset(motor, 0, sizeof(struct foc_motor_b16_s)); + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_motor_get + ****************************************************************************/ + +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; + + DEBUGASSERT(motor); + + /* Update open-loop angle handler */ + + ain.vel = motor->vel_set; + ain.angle = motor->angle_now; + ain.dir = motor->dir; + +#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP + foc_angle_run_b16(&motor->openloop, &ain, &aout); + + /* Store open-loop angle */ + + motor->angle_ol = aout.angle; + + /* Get phase angle now */ + + if (motor->openloop_now == true) + { + motor->angle_now = motor->angle_ol; + } + else +#endif + { + /* TODO: get phase angle from observer or sensor */ + + ASSERT(0); + } + + return ret; +} + +/**************************************************************************** + * Name: foc_motor_control + ****************************************************************************/ + +int foc_motor_control(FAR struct foc_motor_b16_s *motor) +{ + int ret = OK; + + DEBUGASSERT(motor); + + /* No velocity feedback - assume that velocity now is velocity set + * TODO: velocity observer or sensor + */ + + motor->vel_now = motor->vel_set; + + /* Run velocity ramp controller */ + + ret = foc_ramp_run_b16(&motor->ramp, motor->vel_des, + motor->vel_now, &motor->vel_set); + if (ret < 0) + { + PRINTF("ERROR: foc_ramp_run failed %d\n", ret); + goto errout; + } + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_motor_handle + ****************************************************************************/ + +int foc_motor_handle(FAR struct foc_motor_b16_s *motor, + FAR struct foc_mq_s *handle) +{ + int ret = OK; + + DEBUGASSERT(motor); + DEBUGASSERT(handle); + + /* Terminate */ + + if (handle->quit == true) + { + motor->mq.quit = true; + } + + /* Update motor VBUS */ + + if (motor->mq.vbus != handle->vbus) + { + PRINTFV("Set vbus=%" PRIu32 " for FOC driver %d!\n", + handle->vbus, motor->envp->id); + + ret = foc_motor_vbus(motor, handle->vbus); + if (ret < 0) + { + PRINTF("ERROR: foc_motor_vbus failed %d!\n", ret); + goto errout; + } + + motor->mq.vbus = handle->vbus; + } + + /* Update motor velocity destination */ + + if (motor->mq.setpoint != handle->setpoint) + { + PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n", + handle->setpoint, motor->envp->id); + + ret = foc_motor_vel(motor, handle->setpoint); + if (ret < 0) + { + PRINTF("ERROR: foc_motor_vel failed %d!\n", ret); + goto errout; + } + + motor->mq.setpoint = handle->setpoint; + } + + /* Update motor state */ + + if (motor->mq.app_state != handle->app_state) + { + PRINTFV("Set app_state=%d for FOC driver %d!\n", + handle->app_state, motor->envp->id); + + ret = foc_motor_state(motor, handle->app_state); + if (ret < 0) + { + PRINTF("ERROR: foc_motor_state failed %d!\n", ret); + goto errout; + } + + motor->mq.app_state = handle->app_state; + } + + /* Start/stop controller */ + + if (motor->mq.start != handle->start) + { + PRINTFV("Set start=%d for FOC driver %d!\n", + handle->start, motor->envp->id); + + /* Start/stop motor controller */ + + ret = foc_motor_start(motor, handle->start); + if (ret < 0) + { + PRINTF("ERROR: foc_motor_start failed %d!\n", ret); + goto errout; + } + + motor->mq.start = handle->start; + } + +errout: + return ret; +} diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h new file mode 100644 index 000000000..0be4aa824 --- /dev/null +++ b/examples/foc/foc_motor_b16.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * apps/examples/foc/foc_motor_b16.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __EXAMPLES_FOC_FOC_MOTOR_B16_H +#define __EXAMPLES_FOC_FOC_MOTOR_B16_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include "foc_mq.h" +#include "foc_thr.h" + +#include "industry/foc/fixed16/foc_handler.h" +#include "industry/foc/fixed16/foc_ramp.h" +#include "industry/foc/fixed16/foc_angle.h" +#include "industry/foc/fixed16/foc_velocity.h" +#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM +# include "industry/foc/fixed16/foc_model.h" +#endif + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Type Definition + ****************************************************************************/ + +/* FOC motor data (fixed16) */ + +struct foc_motor_b16_s +{ + FAR struct foc_ctrl_env_s *envp; /* Thread env */ + bool fault; /* Fault flag */ + bool startstop; /* Start/stop request */ +#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP + bool openloop_now; /* Open-loop now */ + b16_t angle_ol; /* Phase angle open-loop */ + foc_angle_b16_t openloop; /* Open-loop angle handler */ +#endif + int foc_mode; /* FOC mode */ + b16_t vbus; /* Power bus voltage */ + b16_t angle_now; /* Phase angle now */ + b16_t vel_set; /* Velocity setting now */ + b16_t vel_now; /* Velocity now */ + b16_t vel_des; /* Velocity destination */ + b16_t dir; /* Motor's direction */ + b16_t per; /* Controller period in seconds */ + b16_t iphase_adc; /* Iphase ADC scaling factor */ + b16_t pwm_duty_max; /* PWM duty max */ + dq_frame_b16_t dq_ref; /* DQ reference */ + dq_frame_b16_t vdq_comp; /* DQ voltage compensation */ + foc_handler_b16_t handler; /* FOC controller */ + struct foc_mq_s mq; /* MQ data */ + struct foc_state_b16_s foc_state; /* FOC controller sate */ + struct foc_ramp_b16_s ramp; /* Velocity ramp data */ +#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM + struct foc_model_b16_s model; /* Model handler */ + struct foc_model_state_b16_s model_state; /* PMSM model state */ +#endif +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +int foc_motor_init(FAR struct foc_motor_b16_s *motor, + FAR struct foc_ctrl_env_s *envp); +int foc_motor_deinit(FAR struct foc_motor_b16_s *motor); +int foc_motor_get(FAR struct foc_motor_b16_s *motor); +int foc_motor_control(FAR struct foc_motor_b16_s *motor); +int foc_motor_handle(FAR struct foc_motor_b16_s *motor, + FAR struct foc_mq_s *handle); + +#endif /* __EXAMPLES_FOC_FOC_MOTOR_B16_H */ diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c new file mode 100644 index 000000000..3c9536b92 --- /dev/null +++ b/examples/foc/foc_motor_f32.c @@ -0,0 +1,543 @@ +/**************************************************************************** + * apps/examples/foc/foc_motor_f32.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include "foc_motor_f32.h" + +#include "foc_cfg.h" +#include "foc_adc.h" +#include "foc_debug.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Type Definition + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_motor_configure + ****************************************************************************/ + +static int foc_motor_configure(FAR struct foc_motor_f32_s *motor) +{ +#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI + struct foc_initdata_f32_s ctrl_cfg; +#endif +#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 + struct foc_mod_cfg_f32_s mod_cfg; +#endif +#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM + struct foc_model_pmsm_cfg_f32_s pmsm_cfg; +#endif + int ret = OK; + + DEBUGASSERT(motor); + + /* Initialize velocity ramp */ + + ret = foc_ramp_init_f32(&motor->ramp, + motor->per, + RAMP_CFG_THR, + RAMP_CFG_ACC, + RAMP_CFG_ACC); + if (ret < 0) + { + PRINTF("ERROR: foc_ramp_init failed %d\n", ret); + goto errout; + } + + /* Initialize FOC handler */ + + ret = foc_handler_init_f32(&motor->handler, + &g_foc_control_pi_f32, + &g_foc_mod_svm3_f32); + if (ret < 0) + { + PRINTF("ERROR: foc_handler_init failed %d\n", ret); + goto errout; + } + +#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI + /* Get PI controller configuration */ + + ctrl_cfg.id_kp = (motor->envp->pi_kp / 1000.0f); + ctrl_cfg.id_ki = (motor->envp->pi_ki / 1000.0f); + ctrl_cfg.iq_kp = (motor->envp->pi_kp / 1000.0f); + ctrl_cfg.iq_ki = (motor->envp->pi_ki / 1000.0f); +#endif + +#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3 + /* Get SVM3 modulation configuration */ + + mod_cfg.pwm_duty_max = motor->pwm_duty_max; +#endif + + /* Configure FOC handler */ + + foc_handler_cfg_f32(&motor->handler, &ctrl_cfg, &mod_cfg); + +#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM + /* Initialize PMSM model */ + + ret = foc_model_init_f32(&motor->model, + &g_foc_model_pmsm_ops_f32); + if (ret < 0) + { + PRINTF("ERROR: foc_model_init failed %d\n", ret); + goto errout; + } + + /* Get PMSM model configuration */ + + pmsm_cfg.poles = FOC_MODEL_POLES; + pmsm_cfg.res = FOC_MODEL_RES; + pmsm_cfg.ind = FOC_MODEL_IND; + pmsm_cfg.iner = FOC_MODEL_INER; + pmsm_cfg.flux_link = FOC_MODEL_FLUX; + pmsm_cfg.ind_d = FOC_MODEL_INDD; + pmsm_cfg.ind_q = FOC_MODEL_INDQ; + pmsm_cfg.per = motor->per; + pmsm_cfg.iphase_adc = motor->iphase_adc; + + /* Configure PMSM model */ + + foc_model_cfg_f32(&motor->model, &pmsm_cfg); +#endif + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_motor_vbus + ****************************************************************************/ + +static int foc_motor_vbus(FAR struct foc_motor_f32_s *motor, uint32_t vbus) +{ + DEBUGASSERT(motor); + + /* Update motor VBUS */ + + motor->vbus = (vbus * VBUS_ADC_SCALE); + + return OK; +} + +/**************************************************************************** + * Name: foc_motor_vel + ****************************************************************************/ + +static int foc_motor_vel(FAR struct foc_motor_f32_s *motor, uint32_t vel) +{ + DEBUGASSERT(motor); + + /* Update motor velocity destination */ + + motor->vel_des = (vel * SETPOINT_ADC_SCALE * + motor->envp->velmax / 1000.0f); + + return OK; +} + +/**************************************************************************** + * Name: foc_motor_state + ****************************************************************************/ + +static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state) +{ + int ret = OK; + + DEBUGASSERT(motor); + + /* Get open-loop currents + * NOTE: Id always set to 0 + */ + + motor->dq_ref.q = (motor->envp->qparam / 1000.0f); + motor->dq_ref.d = 0.0f; + + /* Update motor state */ + + switch (state) + { + case FOC_EXAMPLE_STATE_FREE: + { + motor->vel_set = 0.0f; + motor->dir = DIR_NONE; + + /* Force DQ vector to zero */ + + motor->dq_ref.q = 0.0f; + motor->dq_ref.d = 0.0f; + + break; + } + + case FOC_EXAMPLE_STATE_STOP: + { + motor->vel_set = 0.0f; + motor->dir = DIR_NONE; + + /* DQ vector not zero - active brake */ + + break; + } + + case FOC_EXAMPLE_STATE_CW: + { + motor->vel_set = 0.0f; + motor->dir = DIR_CW; + + break; + } + + case FOC_EXAMPLE_STATE_CCW: + { + motor->vel_set = 0.0f; + motor->dir = DIR_CCW; + + break; + } + + default: + { + ret = -EINVAL; + goto errout; + } + } + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_motor_start + ****************************************************************************/ + +static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start) +{ + int ret = OK; + + DEBUGASSERT(motor); + + if (start == true) + { + /* Start motor if VBUS data present */ + + if (motor->mq.vbus > 0) + { + /* Configure motor controller */ + + PRINTF("Configure motor %d!\n", motor->envp->id); + + ret = foc_motor_configure(motor); + if (ret < 0) + { + PRINTF("ERROR: foc_motor_configure failed %d!\n", ret); + goto errout; + } + + /* Start/stop FOC dev request */ + + motor->startstop = true; + } + else + { + /* Return error if no VBUS data */ + + PRINTF("ERROR: start request without VBUS!\n"); + goto errout; + } + } + else + { + /* Start/stop FOC dev request */ + + motor->startstop = true; + } + +errout: + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_motor_init + ****************************************************************************/ + +int foc_motor_init(FAR struct foc_motor_f32_s *motor, + FAR struct foc_ctrl_env_s *envp) +{ +#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP + struct foc_openloop_cfg_f32_s ol_cfg; +#endif + int ret = OK; + + DEBUGASSERT(motor); + DEBUGASSERT(envp); + + /* Reset data */ + + memset(motor, 0, sizeof(struct foc_motor_f32_s)); + + /* Connect envp with motor handler */ + + motor->envp = envp; + + /* Initialize motor data */ + + motor->per = (float)(1.0f / CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ); + motor->iphase_adc = ((CONFIG_EXAMPLES_FOC_IPHASE_ADC) / 100000.0f); + +#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP + /* Initialize open-loop angle handler */ + + foc_angle_init_f32(&motor->openloop, + &g_foc_angle_ol_f32); + + /* Configure open-loop angle handler */ + + ol_cfg.per = motor->per; + foc_angle_cfg_f32(&motor->openloop, &ol_cfg); +#endif + + return ret; +} + +/**************************************************************************** + * Name: foc_motor_deinit + ****************************************************************************/ + +int foc_motor_deinit(FAR struct foc_motor_f32_s *motor) +{ + int ret = OK; + + DEBUGASSERT(motor); + +#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM + /* Deinitialize PMSM model */ + + ret = foc_model_deinit_f32(&motor->model); + if (ret < 0) + { + PRINTF("ERROR: foc_model_deinit failed %d\n", ret); + goto errout; + } +#endif + + /* Deinitialize FOC handler */ + + ret = foc_handler_deinit_f32(&motor->handler); + if (ret < 0) + { + PRINTF("ERROR: foc_handler_deinit failed %d\n", ret); + goto errout; + } + + /* Reset data */ + + memset(motor, 0, sizeof(struct foc_motor_f32_s)); + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_motor_get + ****************************************************************************/ + +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; + + DEBUGASSERT(motor); + + /* Update open-loop angle handler */ + + ain.vel = motor->vel_set; + ain.angle = motor->angle_now; + ain.dir = motor->dir; + +#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP + foc_angle_run_f32(&motor->openloop, &ain, &aout); + + /* Store open-loop angle */ + + motor->angle_ol = aout.angle; + + /* Get phase angle now */ + + if (motor->openloop_now == true) + { + motor->angle_now = motor->angle_ol; + } + else +#endif + { + /* TODO: get phase angle from observer or sensor */ + + ASSERT(0); + } + + return ret; +} + +/**************************************************************************** + * Name: foc_motor_control + ****************************************************************************/ + +int foc_motor_control(FAR struct foc_motor_f32_s *motor) +{ + int ret = OK; + + DEBUGASSERT(motor); + + /* No velocity feedback - assume that velocity now is velocity set + * TODO: velocity observer or sensor + */ + + motor->vel_now = motor->vel_set; + + /* Run velocity ramp controller */ + + ret = foc_ramp_run_f32(&motor->ramp, motor->vel_des, + motor->vel_now, &motor->vel_set); + if (ret < 0) + { + PRINTF("ERROR: foc_ramp_run failed %d\n", ret); + goto errout; + } + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_motor_handle + ****************************************************************************/ + +int foc_motor_handle(FAR struct foc_motor_f32_s *motor, + FAR struct foc_mq_s *handle) +{ + int ret = OK; + + DEBUGASSERT(motor); + DEBUGASSERT(handle); + + /* Terminate */ + + if (handle->quit == true) + { + motor->mq.quit = true; + } + + /* Update motor VBUS */ + + if (motor->mq.vbus != handle->vbus) + { + PRINTFV("Set vbus=%" PRIu32 " for FOC driver %d!\n", + handle->vbus, motor->envp->id); + + ret = foc_motor_vbus(motor, handle->vbus); + if (ret < 0) + { + PRINTF("ERROR: foc_motor_vbus failed %d!\n", ret); + goto errout; + } + + motor->mq.vbus = handle->vbus; + } + + /* Update motor velocity destination + * NOTE: only velocity control supported for now + */ + + if (motor->mq.setpoint != handle->setpoint) + { + PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n", + handle->setpoint, motor->envp->id); + + ret = foc_motor_vel(motor, handle->setpoint); + if (ret < 0) + { + PRINTF("ERROR: foc_motor_vel failed %d!\n", ret); + goto errout; + } + + motor->mq.setpoint = handle->setpoint; + } + + /* Update motor state */ + + if (motor->mq.app_state != handle->app_state) + { + PRINTFV("Set app_state=%d for FOC driver %d!\n", + handle->app_state, motor->envp->id); + + ret = foc_motor_state(motor, handle->app_state); + if (ret < 0) + { + PRINTF("ERROR: foc_motor_state failed %d!\n", ret); + goto errout; + } + + motor->mq.app_state = handle->app_state; + } + + /* Start/stop controller */ + + if (motor->mq.start != handle->start) + { + PRINTFV("Set start=%d for FOC driver %d!\n", + handle->start, motor->envp->id); + + /* Start/stop motor controller */ + + ret = foc_motor_start(motor, handle->start); + if (ret < 0) + { + PRINTF("ERROR: foc_motor_start failed %d!\n", ret); + goto errout; + } + + motor->mq.start = handle->start; + } + +errout: + return ret; +} diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h new file mode 100644 index 000000000..1ab5f9c4f --- /dev/null +++ b/examples/foc/foc_motor_f32.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * apps/examples/foc/foc_motor_f32.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __EXAMPLES_FOC_FOC_MOTOR_F32_H +#define __EXAMPLES_FOC_FOC_MOTOR_F32_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include "foc_mq.h" +#include "foc_thr.h" + +#include "industry/foc/float/foc_handler.h" +#include "industry/foc/float/foc_ramp.h" +#include "industry/foc/float/foc_angle.h" +#include "industry/foc/float/foc_velocity.h" + +#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM +# include "industry/foc/float/foc_model.h" +#endif + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Type Definition + ****************************************************************************/ + +/* FOC motor data (float32) */ + +struct foc_motor_f32_s +{ + FAR struct foc_ctrl_env_s *envp; /* Thread env */ + bool fault; /* Fault flag */ + bool startstop; /* Start/stop request */ +#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP + bool openloop_now; /* Open-loop now */ + float angle_ol; /* Phase angle open-loop */ + foc_angle_f32_t openloop; /* Open-loop angle handler */ +#endif + int foc_mode; /* FOC mode */ + float vbus; /* Power bus voltage */ + float angle_now; /* Phase angle now */ + float vel_set; /* Velocity setting now */ + float vel_now; /* Velocity now */ + float vel_des; /* Velocity destination */ + float dir; /* Motor's direction */ + float per; /* Controller period in seconds */ + float iphase_adc; /* Iphase ADC scaling factor */ + float pwm_duty_max; /* PWM duty max */ + dq_frame_f32_t dq_ref; /* DQ reference */ + dq_frame_f32_t vdq_comp; /* DQ voltage compensation */ + foc_handler_f32_t handler; /* FOC controller */ + struct foc_mq_s mq; /* MQ data */ + struct foc_state_f32_s foc_state; /* FOC controller sate */ + struct foc_ramp_f32_s ramp; /* Velocity ramp data */ +#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM + struct foc_model_f32_s model; /* Model handler */ + struct foc_model_state_f32_s model_state; /* PMSM model state */ +#endif +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +int foc_motor_init(FAR struct foc_motor_f32_s *motor, + FAR struct foc_ctrl_env_s *envp); +int foc_motor_deinit(FAR struct foc_motor_f32_s *motor); +int foc_motor_get(FAR struct foc_motor_f32_s *motor); +int foc_motor_control(FAR struct foc_motor_f32_s *motor); +int foc_motor_handle(FAR struct foc_motor_f32_s *motor, + FAR struct foc_mq_s *handle); + +#endif /* __EXAMPLES_FOC_FOC_MOTOR_F32_H */