examples/foc: move the motor controller code to separate files
This commit is contained in:
raiden00pl
Xiang Xiao
parent
22ed7da99c
commit
f9cec1c770
@ -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
|
||||
|
||||
@ -30,22 +30,12 @@
|
||||
|
||||
#include <dspb16.h>
|
||||
|
||||
#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
|
||||
****************************************************************************/
|
||||
|
||||
@ -30,22 +30,12 @@
|
||||
|
||||
#include <dsp.h>
|
||||
|
||||
#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
|
||||
****************************************************************************/
|
||||
|
||||
541
examples/foc/foc_motor_b16.c
Normal file
541
examples/foc/foc_motor_b16.c
Normal file
@ -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 <nuttx/config.h>
|
||||
|
||||
#include <assert.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
99
examples/foc/foc_motor_b16.h
Normal file
99
examples/foc/foc_motor_b16.h
Normal file
@ -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 <nuttx/config.h>
|
||||
|
||||
#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 */
|
||||
543
examples/foc/foc_motor_f32.c
Normal file
543
examples/foc/foc_motor_f32.c
Normal file
@ -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 <nuttx/config.h>
|
||||
|
||||
#include <assert.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
100
examples/foc/foc_motor_f32.h
Normal file
100
examples/foc/foc_motor_f32.h
Normal file
@ -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 <nuttx/config.h>
|
||||
|
||||
#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 */
|
||||
Loading…
x
Reference in New Issue
Block a user