电压圆限制幅度在电流环的时候处理

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

Applications/app/nv_storage.c

@@ -121,10 +121,6 @@ static void nv_default_foc_params(void) {
 	foc_params.pid_conf[PID_Lock_id].kp = (0.01f);
 	foc_params.pid_conf[PID_Lock_id].ki = (0.20f);
 	foc_params.pid_conf[PID_Lock_id].kd = 0;
-
-	foc_params.pid_conf[PID_FW_id].kp = 0;
-	foc_params.pid_conf[PID_FW_id].ki = 0.01f;
-	foc_params.pid_conf[PID_FW_id].kd = 0;
 }
 
 static void nv_default_gear_config(void) {

Applications/foc/core/PMSM_FOC_Core.c

@@ -54,10 +54,11 @@ static __INLINE void Park(AB_t *alpha_beta, float angle, DQ_t *dq) {
 	dq->q = -alpha_beta->a * s + alpha_beta->b * c;
 }
 
+#if 0
 #define VD_PRIO_HIGH
 static __INLINE float Circle_Limitation(DQ_t *vdq, float vDC, float module, DQ_t *out) {
 	float sq_vdq = vdq->d * vdq->d + vdq->q * vdq->q;
-	float vDC_m = vDC * module;
+	float vDC_m = vDC * module * SQRT3_BY_2;
 	float sq_vDC = vDC_m * vDC_m;
 	if (sq_vdq > sq_vDC) {
 #ifdef VD_PRIO_HIGH		
@@ -74,7 +75,7 @@ static __INLINE float Circle_Limitation(DQ_t *vdq, float vDC, float module, DQ_t
 	}
 	return sqrtf(sq_vdq/sq_vDC);
 }
-
+#endif
 static __INLINE void FOC_Set_DqRamp(dq_Rctrl *c, float target, int time) {	
 	float cp = c->s_Cp;
 	c->s_FinalTgt = target; 
@@ -118,7 +119,6 @@ static __INLINE void FOC_Set_vDqRamp(dq_Rctrl *c, float target) {
 static void PMSM_FOC_Reset_PID(void) {
 	PI_Controller_Reset(&gFoc_Ctrl.pi_id, 0);
 	PI_Controller_Reset(&gFoc_Ctrl.pi_iq, 0);
-	PI_Controller_Reset(&gFoc_Ctrl.pi_fw, 0);
 	PI_Controller_Reset(&gFoc_Ctrl.pi_lock, 0);
 	PI_Controller_Reset(&gFoc_Ctrl.pi_power, 0);
 #ifdef CONFIG_SPEED_LADRC
@@ -151,11 +151,6 @@ static void PMSM_FOC_Conf_PID(void) {
 	gFoc_Ctrl.pi_lock.kd = nv_get_foc_params()->pid_conf[PID_Lock_id].kd;
 	gFoc_Ctrl.pi_lock.DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
 
-	gFoc_Ctrl.pi_fw.kp = nv_get_foc_params()->pid_conf[PID_FW_id].kp;
-	gFoc_Ctrl.pi_fw.ki = nv_get_foc_params()->pid_conf[PID_FW_id].ki;
-	gFoc_Ctrl.pi_fw.kd = nv_get_foc_params()->pid_conf[PID_FW_id].kd;
-	gFoc_Ctrl.pi_fw.DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
-	PI_Controller_max(&gFoc_Ctrl.pi_fw, 0, -CONFIG_MAX_FW_D_CURR);
 #ifdef CONFIG_SPEED_LADRC
 	ladrc_init(&gFoc_Ctrl.vel_lim_adrc, 1.0f/(float)CONFIG_SPD_CTRL_TS, CONFIG_LADRC_Wo, CONFIG_LADRC_Wcv, CONFIG_LADRC_B0);
 	ladrc_init(&gFoc_Ctrl.vel_adrc, 1.0f/(float)CONFIG_SPD_CTRL_TS, CONFIG_LADRC_Wo, CONFIG_LADRC_Wcv, CONFIG_LADRC_B0);
@@ -194,15 +189,7 @@ void PMSM_FOC_RT_LimInit(void) {
 }
 
 void PMSM_FOC_CoreInit(void) {
-#if 0
-	gFoc_Ctrl.pi_id = &PI_Ctrl_ID;
-	gFoc_Ctrl.pi_iq = &PI_Ctrl_IQ;
-	gFoc_Ctrl.pi_speed = &PI_Ctrl_Spd;
-	gFoc_Ctrl.pi_fw = &PI_Ctrl_fw;
-	gFoc_Ctrl.pi_torque = &PI_Ctrl_trq;
-	gFoc_Ctrl.pi_lock = &PI_Ctrl_lock;
-	gFoc_Ctrl.pi_power = &PI_Ctrl_Power;
-#endif
+
 	PMSM_FOC_Conf_PID();
 	
 	memset(&gFoc_Ctrl.in, 0, sizeof(gFoc_Ctrl.in));
@@ -233,7 +220,7 @@ void PMSM_FOC_CoreInit(void) {
 	gFoc_Ctrl.in.s_manualAngle = INVALID_ANGLE;
 	gFoc_Ctrl.in.b_fwEnable = nv_get_foc_params()->n_FwEnable;
 	gFoc_Ctrl.in.s_vDC = nv_get_foc_params()->s_maxDCVol;//(CONFIG_RATED_DC_VOL);
-	gFoc_Ctrl.params.f_DCLim = get_vbus_float();
+//	gFoc_Ctrl.params.f_DCLim = get_vbus_float();
 	eRamp_init_target(&gFoc_Ctrl.in.cruiseRpmRamp, 0, CONFIG_ACC_TIME, CONFIG_DEC_TIME);
 	
 	gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
@@ -252,14 +239,12 @@ void PMSM_FOC_CoreInit(void) {
 }
 
 //#define CONFIG_USER_PHASE_LFP
-static __INLINE void PMSM_FOC_Update_Hardware(void) {
+static __INLINE void PMSM_FOC_Update_Input(void) {
 	AB_t iAB;
-#ifdef CONFIG_USER_PHASE_LFP
-	float *iabc_filted = gFoc_Ctrl.in.s_iABCComp;
-#endif
+
 	float *iabc = gFoc_Ctrl.in.s_iABC;
 
-	phase_current_get(gFoc_Ctrl.in.s_iABC);
+	phase_current_get(iabc);
 
 	Clark(iabc[0], iabc[1], iabc[2], &iAB);
 
@@ -280,63 +265,12 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	gFoc_Ctrl.in.s_vDC = get_vbus_float();
 	
 	get_phase_vols(gFoc_Ctrl.in.s_vABC);
-	
-#ifdef CONFIG_USER_PHASE_LFP
-	float e_freq = gFoc_Ctrl.in.s_motRPM / 60.0f * gFoc_Ctrl.params.n_poles;
-	float lpf_wc;
-	float lpf_comp = 1.0f;
-	if (e_freq <= 150.0f) {
-		lpf_wc = 600.0f;
-		if (e_freq >= 100) {
-			lpf_comp = 1.02f;
-		}else if (e_freq >= 50) {
-			lpf_comp = 1.01;
-		}
-	}else {
-		lpf_wc = e_freq * 4.0f;
-		lpf_comp = 1.03f;
-	}
-	gFoc_Ctrl.params.n_PhaseFilterCeof = (lpf_wc*2*M_PI*FOC_CTRL_US);
-	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[0], gFoc_Ctrl.in.s_iABC[0], gFoc_Ctrl.params.n_PhaseFilterCeof);
-	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[1], gFoc_Ctrl.in.s_iABC[1], gFoc_Ctrl.params.n_PhaseFilterCeof);
-	gFoc_Ctrl.in.s_iABCFilter[2] = - (gFoc_Ctrl.in.s_iABCFilter[0] + gFoc_Ctrl.in.s_iABCFilter[1]);
-	
-	gFoc_Ctrl.in.s_iABCComp[0] = gFoc_Ctrl.in.s_iABCFilter[0] * lpf_comp;
-	gFoc_Ctrl.in.s_iABCComp[1] = gFoc_Ctrl.in.s_iABCFilter[1] * lpf_comp;
-	gFoc_Ctrl.in.s_iABCComp[2] = gFoc_Ctrl.in.s_iABCFilter[2] * lpf_comp;
-
-	Clark(iabc_filted[0], iabc_filted[1], iabc_filted[2], &iAB);
-#endif
 
 	SinCos_Lut(gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
 
 	Park(&iAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealIdq);
 }
 
-static __INLINE void PMSM_FOC_Update_PI_Idq(void) {
-	/* update id pi ctrl */
-	gFoc_Ctrl.params.f_DCLim = gFoc_Ctrl.in.s_vDC;
-	gFoc_Ctrl.params.maxvDQ.d = gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-	gFoc_Ctrl.params.minvDQ.d = -gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-	gFoc_Ctrl.params.maxvDQ.q = gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-	gFoc_Ctrl.params.minvDQ.q = -gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-
-	if (gFoc_Ctrl.params.maxvDQ.d != gFoc_Ctrl.pi_id.max) {
-		gFoc_Ctrl.pi_id.max = gFoc_Ctrl.params.maxvDQ.d;
-	}
-	if (gFoc_Ctrl.params.minvDQ.d != gFoc_Ctrl.pi_id.min) {
-		gFoc_Ctrl.pi_id.min = gFoc_Ctrl.params.minvDQ.d;
-	}
-	/* update iq pi ctrl */
-	if (gFoc_Ctrl.params.maxvDQ.q != gFoc_Ctrl.pi_iq.max) {
-		gFoc_Ctrl.pi_iq.max = gFoc_Ctrl.params.maxvDQ.q;
-	}
-	if (gFoc_Ctrl.params.minvDQ.q != gFoc_Ctrl.pi_iq.min) {
-		gFoc_Ctrl.pi_iq.min = gFoc_Ctrl.params.minvDQ.q;
-	}	
-}
-
-
 #ifdef CONFIG_DQ_STEP_RESPONSE
 float target_d = 0.0f;
 float target_q = 0.0f;
@@ -369,37 +303,50 @@ void PMSM_FOC_Schedule(void) {
 
 	gFoc_Ctrl.ctrl_count++;
 
-	PMSM_FOC_Update_Hardware();
+	PMSM_FOC_Update_Input();
 
 	if (gFoc_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {
 
-		PMSM_FOC_Update_PI_Idq();
+		float max_Vdc = gFoc_Ctrl.in.s_vDC * CONFIG_SVM_MODULATION;
+		float max_vd = max_Vdc * SQRT3_BY_2;
+
+		/* limiter Vd output for PI controller */
+		gFoc_Ctrl.pi_id.max = max_vd;
+		gFoc_Ctrl.pi_id.min = -max_vd;
 	#ifndef CONFIG_DQ_STEP_RESPONSE
 		float target_d = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[0]);
 	#endif
 		float err = target_d - gFoc_Ctrl.out.s_RealIdq.d;
 		gFoc_Ctrl.in.s_targetVdq.d = PI_Controller_RunSerial(&gFoc_Ctrl.pi_id, err);
+
+		/* limiter Vq output for PI controller */
+		float max_vq = sqrtf(SQ(max_vd) - SQ(gFoc_Ctrl.in.s_targetVdq.d));
+		gFoc_Ctrl.pi_iq.max = max_vq;
+		gFoc_Ctrl.pi_iq.min = -max_vq;
 	#ifndef CONFIG_DQ_STEP_RESPONSE
 		float target_q = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[1]);
 	#endif
 		err = target_q - gFoc_Ctrl.out.s_RealIdq.q;
 		gFoc_Ctrl.in.s_targetVdq.q = PI_Controller_RunSerial(&gFoc_Ctrl.pi_iq, err);
-
-#if 0
-		float eW = gFoc_Ctrl.in.s_motRPM * PI/ 30.0f * gFoc_Ctrl.params.n_poles; //电角速度
-		float ff_d = -eW * MOTOR_Lq * gFoc_Ctrl.out.s_RealIdq.q;
-		gFoc_Ctrl.in.s_targetVdq.d += fclamp(ff_d, -10.0f, 0);
-#endif
 	}else {
-		gFoc_Ctrl.in.s_targetVdq.d = FOC_Get_DqRamp(&gFoc_Ctrl.vdq_ctl[0]);
-		gFoc_Ctrl.in.s_targetVdq.q = FOC_Get_DqRamp(&gFoc_Ctrl.vdq_ctl[1]);	
-	}
+		float max_Vdc = gFoc_Ctrl.in.s_vDC * CONFIG_SVM_MODULATION;
+		float max_vd = max_Vdc * SQRT3_BY_2;
+		float vd_ref = FOC_Get_DqRamp(&gFoc_Ctrl.vdq_ctl[0]);
+		gFoc_Ctrl.in.s_targetVdq.d = fclamp(vd_ref, -max_vd, max_vd);
 	
-	gFoc_Ctrl.out.f_vdqRation = Circle_Limitation(&gFoc_Ctrl.in.s_targetVdq, gFoc_Ctrl.in.s_vDC, gFoc_Ctrl.params.n_modulation, &gFoc_Ctrl.out.s_OutVdq);
+		float max_vq = sqrtf(SQ(max_vd) - SQ(gFoc_Ctrl.in.s_targetVdq.d));
+		float vq_ref = FOC_Get_DqRamp(&gFoc_Ctrl.vdq_ctl[1]);
+		gFoc_Ctrl.in.s_targetVdq.q = fclamp(vq_ref, -max_vq, max_vq);
+	}
 
+#if 0
+	gFoc_Ctrl.out.f_vdqRation = Circle_Limitation(&gFoc_Ctrl.in.s_targetVdq, gFoc_Ctrl.in.s_vDC, gFoc_Ctrl.params.n_modulation, &gFoc_Ctrl.out.s_OutVdq);
 	gFoc_Ctrl.out.s_OutVdq.d *= SQRT3_BY_2;
 	gFoc_Ctrl.out.s_OutVdq.q *= SQRT3_BY_2;
-
+#else
+	gFoc_Ctrl.out.s_OutVdq.d = gFoc_Ctrl.in.s_targetVdq.d;
+	gFoc_Ctrl.out.s_OutVdq.q = gFoc_Ctrl.in.s_targetVdq.q;
+#endif
 	RevPark(&gFoc_Ctrl.out.s_OutVdq, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_OutVAB);
 	
 	SVM_Duty_Fix(&gFoc_Ctrl.out.s_OutVAB, gFoc_Ctrl.in.s_vDC, FOC_PWM_Half_Period, &gFoc_Ctrl.out);

Applications/foc/core/PMSM_FOC_Core.h

@@ -75,10 +75,6 @@ typedef struct {
 	u8 	  n_poles;
 	float n_modulation;
 	float n_PhaseFilterCeof;
-	//float n_TrqVelLimGain;
-	float f_DCLim;
-	DQ_t  maxvDQ;
-	DQ_t  minvDQ;
 }FOC_Params;
 
 typedef struct {
@@ -181,8 +177,7 @@ typedef struct {
 
 typedef struct {
 	PI_Controller pi_id;
-	PI_Controller pi_iq;	
-	PI_Controller pi_fw;
+	PI_Controller pi_iq;
 	PI_Controller pi_lock;
 	PI_Controller pi_power;
 #ifdef CONFIG_SPEED_LADRC

Simulink/motor_data_nihe.m

@@ -1,7 +1,7 @@
 clear;
-y=xlsread('E:\works\项目\MC100\电机台架数据\鲨湾96v电机145外特性.xlsx','MAP1','A97:A102'); % torque
-x=xlsread('E:\works\项目\MC100\电机台架数据\鲨湾96v电机145外特性.xlsx','MAP1','W97:W102'); % D
-z=xlsread('E:\works\项目\MC100\电机台架数据\鲨湾96v电机145外特性.xlsx','MAP1','X97:X102'); % Q
+y=xlsread('E:\works\项目\MC100\电机台架数据\鲨湾96v电机145外特性.xlsx','MAP1','A2:A9'); % torque
+x=xlsread('E:\works\项目\MC100\电机台架数据\鲨湾96v电机145外特性.xlsx','MAP1','W2:W9'); % D
+z=xlsread('E:\works\项目\MC100\电机台架数据\鲨湾96v电机145外特性.xlsx','MAP1','X2:X9'); % Q
 y=abs(y);
 % Te = 1.5*4*(a*iq + (ld - lq)*idiq);
 ft=fittype('6*(a*iq-(l)*id*iq)', 'independent',{'id','iq'}, 'coefficients', {'a', 'l'});