加入母线电流限流的iq前馈控制

Signed-off-by: kevin <huhui@sharkgulf.com>

Applications/app/app.c

@@ -179,7 +179,8 @@ static u32 app_plot_task(void * args) {
 		}
 		can_plot2(plot_arg1, plot_arg2);
 	}else if (plot_type == 2) {
-		can_plot2(mot_contrl_get_final_torque(&motor.controller), mot_contrl()->target_torque);
+		//can_plot2(mot_contrl_get_final_torque(&motor.controller), mot_contrl()->target_torque);
+		can_plot2(mot_contrl()->dc_lim_iq_ff, mot_contrl()->target_torque);
 	}else if (plot_type == 3) {
 		plot_smo_angle();
 	}else if (plot_type == 4) {
@@ -202,6 +203,8 @@ static u32 app_plot_task(void * args) {
 		s16 duty_filterd = (s16)(motor.controller.duty_filterd * 100);
 		can_plot2(duty, duty_filterd);
 	}else if (plot_type == 11) {
+		//do it in other place, phase voltage
+	}else if (plot_type == 12) {
 		can_plot2((s16)motor.controller.dc_curr_filted, (s16)motor.controller.dc_curr_calc);
 	}
 	return 20;

Applications/foc/core/PI_Controller.h

@@ -31,13 +31,13 @@ static __INLINE void PI_Controller_Reset(PI_Controller *pi, float init) {
 	pi->is_sat = false;
 }
 
-static __INLINE float PI_Controller_Run(PI_Controller *pi, float err) {
+static __INLINE float PI_Controller_Run(PI_Controller *pi, float err, float ff) {
 	float kp_err = (err) * pi->kp;
 	float ki_err = (err) * pi->ki;
 	float integral = ki_err * pi->ts;
 
 	pi->ui = pi->ui + integral;
-	float out = pi->ui + kp_err;
+	float out = pi->ui + kp_err + ff;
 	if (out > pi->max) {
 		out = pi->max;
 		pi->ui = out - kp_err;

Applications/foc/core/controller.c

@@ -71,6 +71,7 @@ bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
 	ctrl->dc_curr_calc = 0;
 	ctrl->phase_curr_filted[0] = ctrl->phase_curr_filted[1] = 0;
 	ctrl->out_idq_filterd.d = ctrl->out_idq_filterd.q = 0;
+	ctrl->out_vdq_filterd.d = ctrl->out_vdq_filterd.q = 0;
 	ctrl->autohold_torque = 0;
 	ctrl->out_current_vec = 0;
 	ctrl->target_idq.d = 0;
@@ -289,17 +290,51 @@ bool mot_contrl_update(mot_contrl_t *ctrl) {
 
 	ctrl->duty_raw = NORM2_f(foc->out.vol_dq.d, foc->out.vol_dq.q)/(foc->in.dc_vol * CONFIG_SVM_MODULATION * SQRT3_BY_2);
 	LowPass_Filter(ctrl->duty_filterd, ctrl->duty_raw, 0.01f);
-	float lowpass = foc->mot_vel_radusPers * FOC_CTRL_US * 2;
-	lowpass = fclamp(lowpass, 0.001f, 1.0f);
+	float lowpass = 0.001f;
 	LowPass_Filter(ctrl->out_idq_filterd.d, foc->out.curr_dq.d ,lowpass);
 	LowPass_Filter(ctrl->out_idq_filterd.q, foc->out.curr_dq.q ,lowpass);
+	LowPass_Filter(ctrl->out_vdq_filterd.d, foc->out.vol_dq.d ,lowpass);
+	LowPass_Filter(ctrl->out_vdq_filterd.q, foc->out.vol_dq.q ,lowpass);
+
+	/* 计算母线电流 */
+	float vd = ctrl->out_vdq_filterd.d;
+	float vq = ctrl->out_vdq_filterd.q;
+	float id = ctrl->out_idq_filterd.d;
+	float iq = ctrl->out_idq_filterd.q;
+    /*
+		根据公式（等幅值变换，功率不等）:
+		iDC x vDC = 3/2(iq x vq + id x vd);
+	*/
+	float m_pow = (vd * id + vq * iq);
+	float raw_idc = 0.0f;
+	float v_dc = get_vbus_float();
+	if (v_dc != 0.0f) {
+		raw_idc = m_pow / v_dc;
+	}
+	ctrl->dc_curr_calc = raw_idc;
 	return true;
 }
 
 static __INLINE float mot_contrl_dc_curr_limiter(mot_contrl_t *ctrl, float maxTrq) {
-	ctrl->pi_power.max = maxTrq;
-	float errRef = line_ramp_get_interp(&ctrl->ramp_dc_curr_lim) - ctrl->dc_curr_filted;
-	return PI_Controller_Run(&ctrl->pi_power, errRef);
+	float dc_lim = line_ramp_get_interp(&ctrl->ramp_dc_curr_lim);
+	/* 计算iq的前馈,(母线功率 - d轴功率)/vq
+	 * 加上0.0001f 避免除0
+	*/
+	float r = ctrl->dc_curr_filted / (ctrl->dc_curr_calc + 0.0001f) + 0.0001f;
+	r = fclamp(r, 0.5f, 1.5f);
+	float q_axis_power = dc_lim * get_vbus_float() / r - ctrl->out_idq_filterd.d * ctrl->out_vdq_filterd.d;
+	float iq_ff = q_axis_power / (ctrl->out_vdq_filterd.q + 0.0001f);
+	iq_ff = ABS(iq_ff);
+	if (iq_ff > maxTrq) {
+		ctrl->dc_lim_iq_ff = maxTrq;
+	}else if (iq_ff < 0) {
+		ctrl->dc_lim_iq_ff = 0;
+	}else {
+		ctrl->dc_lim_iq_ff = iq_ff;
+	}
+	ctrl->pi_power.max = min(maxTrq, ctrl->dc_lim_iq_ff);
+	float errRef = dc_lim - ctrl->dc_curr_filted;
+	return PI_Controller_Run(&ctrl->pi_power, errRef, ctrl->dc_lim_iq_ff);
 }
 
 static __INLINE float mot_contrl_vel_limiter(mot_contrl_t *ctrl, float maxTrq) {
@@ -391,7 +426,7 @@ void mot_contrl_dq_calc(mot_contrl_t *ctrl) {
 		ctrl->pi_lock.min = -hold_torque;
 		float vel_count = motor_encoder_get_vel_count();
 		float errRef = 0 - vel_count;
-		ctrl->target_torque = PI_Controller_Run(&ctrl->pi_lock ,errRef);
+		ctrl->target_torque = PI_Controller_Run(&ctrl->pi_lock ,errRef, 0);
 		mot_contrl_dq_assign(ctrl);
 		return;
 	}
@@ -844,24 +879,10 @@ void mot_contrl_get_pid(mot_contrl_t *ctrl, u8 id, float *kp, float *ki, float *
 }
 
 void mot_contrl_calc_current(mot_contrl_t *ctrl) {
-	float vd = ctrl->foc.out.vol_dq.d;
-	float vq = ctrl->foc.out.vol_dq.q;
-
 	float id = ctrl->out_idq_filterd.d;
 	float iq = ctrl->out_idq_filterd.q;
-    /*
-		根据公式（等幅值变换，功率不等）:
-		iDC x vDC = 3/2(iq x vq + id x vd);
-	*/
-	float m_pow = (vd * id + vq * iq);
-	float raw_idc = 0.0f;
-	float v_dc = get_vbus_float();
-	if (v_dc != 0.0f) {
-		raw_idc = m_pow / v_dc;
-	}
-	LowPass_Filter(ctrl->dc_curr_calc, raw_idc, 0.02f);
 
-	raw_idc = get_vbus_current();
+	float raw_idc = get_vbus_current();
 	if (raw_idc != NO_VALID_CURRENT) {
 		LowPass_Filter(ctrl->dc_curr_filted, raw_idc, 0.05f);
 	}else {

Applications/foc/core/controller.h

@@ -129,6 +129,7 @@ typedef struct{
 	float           target_torque_raw; //扭矩过零点处理前的扭矩
 	dq_t			target_idq;
 	dq_t            out_idq_filterd;
+	dq_t            out_vdq_filterd;
 	albt_t			phase_v_ab; //sampled
 	dq_t			phase_v_dq;
 	float 			out_current_vec;
@@ -137,6 +138,7 @@ typedef struct{
 	u32				torque_dec_time;
 	float   		dc_curr_filted;
 	float			dc_curr_calc;
+	float 			dc_lim_iq_ff;
 	float   		phase_curr_filted[3];
 	float           autohold_torque;
 	float			duty_raw;

Applications/foc/motor/motor.c

@@ -1248,7 +1248,7 @@ void TIMER_UP_IRQHandler(void){
 	}
 }
 
-measure_time_t g_meas_foc = {.exec_max_time = 25, .intval_max_time = 62,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
+measure_time_t g_meas_foc = {.exec_max_time = 31, .intval_max_time = 62,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
 #define TIME_MEATURE_START() time_measure_start(&g_meas_foc)
 #define TIME_MEATURE_END() time_measure_end(&g_meas_foc)
 #if (CONFIG_ENABLE_IAB_REC==1)