Merge branch 'running_test' into tcs_dev

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

Applications/bsp/gd32/board_mc105_v3.h

@@ -53,7 +53,7 @@
 #define TSampleBefore (TDead + TRise) //采样开始前需要等待的时间
 
 //#define CONFIG_START_LINE_DTC_CURRENT 5.0F /* 死区补偿开始电流，取决于电流噪声 */
-//#define COMFIG_END_LINE_DTC_CURRENT   15.0F
+//#define CONFIG_END_LINE_DTC_CURRENT   15.0F
 
 #define ADC_REFERENCE_VOLTAGE  (3.3F)
 #define ADC_FULL_MAX          (4095.0F)

Applications/foc/core/PMSM_FOC_Core.c

@@ -26,8 +26,6 @@
 PMSM_FOC_Ctrl gFoc_Ctrl;
 static bool g_focinit = false;
 
-static u32 PMSM_FOC_Debug_Task(void *p);
-
 static __INLINE void RevPark(DQ_t *dq, float angle, AB_t *alpha_beta) {
 	float c,s;
 #if 0
@@ -73,28 +71,7 @@ void PMSM_FOC_ABC2Dq(float a, float b, float c, float *d, float *q) {
 	*q = dq.q;
 }
 
-#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 * SQRT3_BY_2;
-	float sq_vDC = vDC_m * vDC_m;
-	if (sq_vdq > sq_vDC) {
-#ifdef VD_PRIO_HIGH		
-		out->d = vdq->d;
-		out->q = sqrtf(sq_vDC - out->d*out->d);
-#else
-		float r = sqrtf(sq_vDC / sq_vdq);
-		out->d = vdq->d * r;
-		out->q = vdq->q * r;
-#endif
-	}else {
-		out->d = vdq->d;
-		out->q = vdq->q;
-	}
-	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; 
@@ -224,7 +201,6 @@ void PMSM_FOC_CoreInit(void) {
 	if (!g_focinit) {
 		PMSM_FOC_UserInit();
 		PMSM_FOC_RT_LimInit();
-		shark_task_create(PMSM_FOC_Debug_Task, NULL);
 		g_focinit = true;
 		//_DEBUG("User Limit:\n");
 		//_DEBUG("dc %f, rpm %f, torque %f, phase %f, vDCmax %f, vDCmin %f, ebrk %f\n", gFoc_Ctrl.userLim.s_iDCLim, gFoc_Ctrl.userLim.s_motRPMLim, gFoc_Ctrl.userLim.s_torqueLim,
@@ -259,99 +235,6 @@ void PMSM_FOC_CoreInit(void) {
 	gFoc_Ctrl.plot_type = Plot_None;
 }
 
-/* 通过三相电流重构母线电流，和单电阻采样正好相反，原理一致 */
-static __INLINE void PMSM_FOC_Calc_iDC_Fast(void) {
-#if 0
-	float deadtime = (float)(NS_2_TCLK(PWM_DEAD_TIME_NS + HW_DEAD_TIME_NS))/(float)FOC_PWM_Half_Period;
-	float duty_pu[3];
-	duty_pu[0] = (float)gFoc_Ctrl.out.n_Duty[0] / (float)FOC_PWM_Half_Period;
-	duty_pu[1] = (float)gFoc_Ctrl.out.n_Duty[1] / (float)FOC_PWM_Half_Period;
-	duty_pu[2] = (float)gFoc_Ctrl.out.n_Duty[2] / (float)FOC_PWM_Half_Period;
-	
-	float *iABC = gFoc_Ctrl.in.s_iABC;
-	float iDC;
-	if ((duty_pu[0] >= duty_pu[1]) && (duty_pu[1] >= duty_pu[2])) {
-		iDC = iABC[0] * MAX(duty_pu[0] - duty_pu[1] - deadtime, 0) + (iABC[0] + iABC[1]) * MAX(duty_pu[1] - duty_pu[2] - deadtime, 0);
-		if (iABC[0] < 0) {
-			iDC = iDC + iABC[0] * deadtime;
-		}
-		if (iABC[1] > 0) {
-			iDC = iDC + iABC[0] * deadtime;
-		}else {
-			iDC = iDC + (iABC[0] + iABC[1]) * deadtime;
-		}
-		if (iABC[2] > 0) {
-			iDC = iDC + (iABC[0] + iABC[1]) * deadtime;
-		}
-	}else if ((duty_pu[0] >= duty_pu[2]) && (duty_pu[2] >= duty_pu[1])) {
-		iDC = iABC[0] * MAX(duty_pu[0] - duty_pu[2] - deadtime, 0) + (iABC[0] + iABC[2]) * MAX(duty_pu[2] - duty_pu[1] - deadtime, 0);
-		if (iABC[0] < 0) {
-			iDC = iDC + iABC[0] * deadtime;
-		}
-		if (iABC[2] > 0) {
-			iDC = iDC + iABC[0] * deadtime;
-		}else {
-			iDC = iDC + (iABC[0] + iABC[2]) * deadtime;
-		}
-		if (iABC[1] > 0) {
-			iDC = iDC + (iABC[0] + iABC[2]) * deadtime;
-		}
-	}else if ((duty_pu[1] >= duty_pu[0]) && (duty_pu[0] >= duty_pu[2])) {
-		iDC = iABC[1] * MAX(duty_pu[1] - duty_pu[0] - deadtime, 0) + (iABC[1] + iABC[0]) * MAX(duty_pu[0] - duty_pu[2] - deadtime, 0);
-		if (iABC[1] < 0) {
-			iDC = iDC + iABC[1] * deadtime;
-		}
-		if (iABC[0] > 0) {
-			iDC = iDC + iABC[1] * deadtime;
-		}else {
-			iDC = iDC + (iABC[1] + iABC[0]) * deadtime;
-		}
-		if (iABC[2] > 0) {
-			iDC = iDC + (iABC[1] + iABC[0]) * deadtime;
-		}
-	}else if ((duty_pu[1] >= duty_pu[2]) && (duty_pu[2] >= duty_pu[0])) {
-		iDC = iABC[1] * MAX(duty_pu[1] - duty_pu[2] - deadtime, 0) + (iABC[1] + iABC[2]) * MAX(duty_pu[2] - duty_pu[0] - deadtime, 0);
-		if (iABC[1] < 0) {
-			iDC = iDC + iABC[1] * deadtime;
-		}
-		if (iABC[2] > 0) {
-			iDC = iDC + iABC[1] * deadtime;
-		}else {
-			iDC = iDC + (iABC[1] + iABC[2]) * deadtime;
-		}
-		if (iABC[0] > 0) {
-			iDC = iDC + (iABC[1] + iABC[2]) * deadtime;
-		}
-	}else if ((duty_pu[2] >= duty_pu[0]) && (duty_pu[0] >= duty_pu[1])) {
-		iDC = iABC[2] * MAX(duty_pu[2] - duty_pu[0] - deadtime, 0) + (iABC[2] + iABC[0]) * MAX(duty_pu[0] - duty_pu[1] - deadtime, 0);
-		if (iABC[2] < 0) {
-			iDC = iDC + iABC[2] * deadtime;
-		}
-		if (iABC[0] > 0) {
-			iDC = iDC + iABC[2] * deadtime;
-		}else {
-			iDC = iDC + (iABC[2] + iABC[0]) * deadtime;
-		}
-		if (iABC[1] > 0) {
-			iDC = iDC + (iABC[2] + iABC[0]) * deadtime;
-		}
-	}else { // duty_pu[2] >= duty_pu[1] && duty_pu[1] >= duty_pu[0]
-		iDC = iABC[2] * MAX(duty_pu[2] - duty_pu[1] - deadtime, 0) + (iABC[2] + iABC[1]) * MAX(duty_pu[1] - duty_pu[0] - deadtime, 0);
-		if (iABC[2] < 0) {
-			iDC = iDC + iABC[2] * deadtime;
-		}
-		if (iABC[1] > 0) {
-			iDC = iDC + iABC[2] * deadtime;
-		}else {
-			iDC = iDC + (iABC[2] + iABC[1]) * deadtime;
-		}
-		if (iABC[0] > 0) {
-			iDC = iDC + (iABC[2] + iABC[1]) * deadtime;
-		}
-	}
-	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC2, iDC, 0.005f);
-#endif
-}
 
 #define CONFIG_PEAK_CNT 3 //计算经过的电周期内的最大值(peak 峰值)
 #define CONFIG_PHASE_UNBALANCE_THROLD 4.0F
@@ -447,7 +330,7 @@ static  __INLINE void PMSM_FOC_DeadTime_Compensate(s32 PWM_Half_Period) {
 	float iabs = ABS(gFoc_Ctrl.in.s_iABC_DT[0]);
 	if (iabs > CONFIG_START_LINE_DTC_CURRENT) {
 		delta = iabs - CONFIG_START_LINE_DTC_CURRENT;
-		r = delta / (COMFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
+		r = delta / (CONFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
 		if (r > 1.0f) {
 			r = 1.0f;
 		}
@@ -459,7 +342,7 @@ static  __INLINE void PMSM_FOC_DeadTime_Compensate(s32 PWM_Half_Period) {
 	iabs = ABS(gFoc_Ctrl.in.s_iABC_DT[1]);
 	if (iabs > CONFIG_START_LINE_DTC_CURRENT) {
 		delta = iabs - CONFIG_START_LINE_DTC_CURRENT;
-		r = delta / (COMFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
+		r = delta / (CONFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
 		if (r > 1.0f) {
 			r = 1.0f;
 		}
@@ -471,7 +354,7 @@ static  __INLINE void PMSM_FOC_DeadTime_Compensate(s32 PWM_Half_Period) {
 	iabs = ABS(gFoc_Ctrl.in.s_iABC_DT[2]);
 	if (iabs > CONFIG_START_LINE_DTC_CURRENT) {
 		delta = iabs - CONFIG_START_LINE_DTC_CURRENT;
-		r = delta / (COMFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
+		r = delta / (CONFIG_END_LINE_DTC_CURRENT - CONFIG_START_LINE_DTC_CURRENT);
 		if (r > 1.0f) {
 			r = 1.0f;
 		}
@@ -513,8 +396,6 @@ static __INLINE bool PMSM_FOC_Update_Input(void) {
 
 	phase_current_get(iabc);
 
-	PMSM_FOC_Calc_iDC_Fast();
-
 	Clark(iabc[0], iabc[1], iabc[2], &iAB);
 
 	foc_observer_update(gFoc_Ctrl.out.s_OutVAB.a * TWO_BY_THREE, gFoc_Ctrl.out.s_OutVAB.b * TWO_BY_THREE, iAB.a, iAB.b);
@@ -599,29 +480,6 @@ float target_d = 0.0f;
 float target_q = 0.0f;
 #endif
 
-static u32 PMSM_FOC_Debug_Task(void *p) {
-	if (gFoc_Ctrl.in.b_motEnable) {
-#ifdef CONFIG_DQ_STEP_RESPONSE
-		if (gFoc_Ctrl.plot_type == Plot_D_Step) {
-			plot_2data16(FtoS16x10(target_d), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.d));
-		}if (gFoc_Ctrl.plot_type == Plot_Q_Step) {
-			plot_2data16(FtoS16x10(target_q), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q));
-		}
-#else
-		if (gFoc_Ctrl.plot_type == Plot_D_flow) {
-			plot_2data16(FtoS16x10(gFoc_Ctrl.idq_ctl[0].s_Cp), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.d));
-		}else if (gFoc_Ctrl.plot_type == Plot_Q_flow) {
-			plot_2data16(FtoS16x10(gFoc_Ctrl.idq_ctl[1].s_Cp), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q));
-		}else if (gFoc_Ctrl.plot_type == Plot_DQ_Curr) {
-			plot_3data16(FtoS16x10(gFoc_Ctrl.out.s_RealIdq.d), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q), FtoS16x10(gFoc_Ctrl.out.s_FilteriDC));
-		}else if (gFoc_Ctrl.plot_type == Plot_Spd_flow) {
-			plot_2data16(gFoc_Ctrl.in.s_targetRPM, gFoc_Ctrl.in.s_motVelocity);
-		}
-#endif
-	}
-	return 1;
-}
-
 static __INLINE float id_feedforward(float eW) {
 #ifdef CONFIG_CURRENT_LOOP_DECOUPE
 	return -(gFoc_Ctrl.params.lq * gFoc_Ctrl.out.s_RealIdq.q * eW);
@@ -688,14 +546,9 @@ bool PMSM_FOC_Schedule(void) {
 		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);
@@ -816,89 +669,6 @@ static void crosszero_step_towards(float *value, float target) {
 
 
 /* MPTA, 弱磁, 功率限制，主要是分配DQ轴电流 */
-
-#define CHANGE_MAX_CNT 3
-static __INLINE void PMSM_FOC_VelCtrl_Decide(void) {
-#if 0
-	static int change_cnt = 0;
-	static bool change_done = false;
-	static u32  change_time = 0xFFFFFFFF;
-	float f_te = F_get_Te();
-	float f_accl = F_get_accl();
-	if (mc_is_epm()) {
-		change_cnt = 0;
-		change_time = 0xFFFFFFFF;
-		change_done = false;
-		gFoc_Ctrl.out.empty_load = false;
-		return;
-	}
-	if (gFoc_Ctrl.in.s_motVelocity == 0.0f || gFoc_Ctrl.out.n_RunMode == CTRL_MODE_OPEN) {
-		change_cnt = 0;
-		change_time = 0xFFFFFFFF;
-		change_done = false;
-		gFoc_Ctrl.out.empty_load = false;
-		return;
-	}
-	if (f_te <= 0.0f) {
-		change_cnt = 0;
-		change_time = 0xFFFFFFFF;
-		return;
-	}
-	if (change_done) { 
-		/* 误判空转，发现电机给定的N大于空气阻力，说明不是空转 */
-		if (gFoc_Ctrl.out.empty_load) {
-			float f_air = F_get_air();
-			if ((f_accl > 1.0f) && (f_te >= (f_air + f_accl))) {
-				change_cnt ++;
-			}else {
-				change_cnt = 0;
-			}
-			if (change_cnt >= 500) {
-				gFoc_Ctrl.out.empty_load = false;
-#ifdef CONFIG_SPEED_LADRC
-				PMSM_FOC_Change_TrqLoop_Params(nv_get_foc_params()->f_adrc_vel_lim_Wcv, nv_get_foc_params()->f_adrc_vel_lim_B0);
-				PMSM_FOC_Change_VelLoop_Params(nv_get_foc_params()->f_adrc_vel_Wcv, nv_get_foc_params()->f_adrc_vel_B0);
-#endif
-			}
-		}
-		return;
-	}
-	if (change_time == 0xFFFFFFFF) {
-		change_time = get_tick_ms();
-	}else { //起步3s内检测是否空转
-		if (get_delta_ms(change_time) > 3000) {
-			return;
-		}
-	}
-
-	if ((f_accl > 200.0f) && (f_accl/f_te > 3.0f )) {
-		change_cnt++;
-	}else if ((F_get_MotAccl() >= 10.0f) && (f_accl/f_te > 1.2f )) {
-		change_cnt = CHANGE_MAX_CNT;
-	}
-	else {
-		if ((f_te > 50) && (f_accl > 0) && (f_te > f_accl)) {
-			change_cnt --;
-		}else {
-			change_cnt = 0;
-		}
-	}
-	if (!change_done && (change_cnt >= CHANGE_MAX_CNT)) {
-		change_done = true;
-		change_cnt = 0;
-		gFoc_Ctrl.out.empty_load = change_done;
-#ifdef CONFIG_SPEED_LADRC
-		PMSM_FOC_Change_TrqLoop_Params(CONFIG_LADRC_NOLOAD_Wcv, CONFIG_LADRC_NOLOAD_B0);
-		PMSM_FOC_Change_VelLoop_Params(CONFIG_LADRC_NOLOAD_Wcv, CONFIG_LADRC_NOLOAD_B0);
-#endif
-	}else if (!change_done && (change_cnt <= -200)) {
-		change_done = true;
-		change_cnt = 0;
-		gFoc_Ctrl.out.empty_load = false;
-	}
-#endif
-}
-
 static __INLINE float PMSM_FOC_Limit_iDC(float maxTrq) {
 #if 1
 	gFoc_Ctrl.pi_power.max = maxTrq;
@@ -1052,7 +822,6 @@ void PMSM_FOC_Slow_Task(void) {
 	eRamp_running(&gFoc_Ctrl.rtLim.DCCurrLimRamp);
 	eRamp_running(&gFoc_Ctrl.rtLim.rpmLimRamp);
 	eRamp_running(&gFoc_Ctrl.in.cruiseRpmRamp);
-	PMSM_FOC_VelCtrl_Decide();
 	PMSM_FOC_idqCalc();
 }
 
@@ -1476,12 +1245,14 @@ void PMSM_FOC_Calc_Current(void) {
 		raw_idc = m_pow / v_dc;
 	}
 	LowPass_Filter(gFoc_Ctrl.out.s_CalciDC2, raw_idc, 0.02f);
-#ifdef VBUS_I_CHAN
+
 	raw_idc = get_vbus_current();
-	LowPass_Filter(gFoc_Ctrl.out.s_FilteriDC, raw_idc, 0.05f);
-#else
-	gFoc_Ctrl.out.s_FilteriDC = gFoc_Ctrl.out.s_CalciDC;
-#endif
+	if (raw_idc != NO_VALID_CURRENT) {
+		LowPass_Filter(gFoc_Ctrl.out.s_FilteriDC, raw_idc, 0.05f);
+	}else {
+		gFoc_Ctrl.out.s_FilteriDC = gFoc_Ctrl.out.s_CalciDC;
+	}
+
 	gFoc_Ctrl.out.s_RealCurrentFiltered = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
 
 }

Applications/foc/samples.c

@@ -117,7 +117,7 @@ float get_vbus_current(void) {
 	s16 ibus = _ibus.filted_value * 10.0f;
 	return (float)ibus/10.0f;
 #else
-	return 0;
+	return NO_VALID_CURRENT;
 #endif
 }
 

Applications/os/os_types.h

@@ -54,6 +54,9 @@ typedef unsigned int	uint32;
 #define MAX_S16 (0x7FFF)
 #define MAX_U16 (0xFFFF)
 #define MAX_U8  (0xFF)
+
+#define NO_VALID_CURRENT 800000.0f
+
 extern void *pvPortMalloc( size_t xWantedSize );
 #define os_alloc pvPortMalloc
 extern void vPortFree(void *);