48v过调制OK

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

Applications/app/app.c

@@ -102,9 +102,23 @@ static u32 _app_report_task(void *p) {
 	}
 	return 200;
 }
-
+//static bool _mc_start = false;
+//static float _angle = 0.0f;
 static u32 _app_plot_task(void * args) {
-	can_report_plot_values(0x45);
+	//can_report_plot_values(0x45);
+	can_plot3(PMSM_FOC_Get()->out.n_Duty[0], PMSM_FOC_Get()->out.n_Duty[1], PMSM_FOC_Get()->out.n_Duty[2]);
+#if 0
+	if (!_mc_start) {
+		_mc_start = true;
+		mc_start(CTRL_MODE_OPEN);
+		PMSM_FOC_SetOpenVdq(0, 60);
+	}
+	_angle++;
+	if (_angle >= 360.0f) {
+		_angle = 0.0f;
+	}
+	PMSM_FOC_Set_Angle(_angle);
+#endif
 	return 50;
 }
 static u32 _app_low_task(void *args) {

Applications/foc/core/svpwm.c

@@ -524,7 +524,7 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 }
 
 #else
-#if 1
+#if 0
 #define Duty_Sat(max, t1, t2) \
 	do { \
 		if (t1+t2 > max) { \
@@ -536,12 +536,12 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 #else
 #define Duty_Sat(max, t1, t2) 
 #endif
-void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
+void SVM_Duty_Fix(AB_t *alb, float vbus, u32 PWM_half_period, FOC_OutP *out) {
 	float alpha = (alb->a) * SQRT3_BY_2;
 	float beta  = (alb->b)  * SQRT3_BY_2;
-	u32   PWM_Period = PWM_half_period * 2;
+	s32   PWM_Period = PWM_half_period * 2;
 	u8    sector = 0xFF;
-	int   tA, tB, tC;
+	s32   tA, tB, tC;
 	u32   low, midle;
 	float X, Y, Z;
 	float modu = (float)(PWM_Period) / (vbus);
@@ -587,8 +587,8 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 	Z = (-alpha + ONE_BY_SQRT3 * beta) * modu;
 	switch(sector) {
 		case SECTOR_1: // 3
-		{	u32 T4 = -Z;
-			u32 T6 = X;
+		{	s32 T4 = -Z;
+			s32 T6 = X;
 			Duty_Sat(PWM_Period, T4, T6);
 			tC = (PWM_Period - T4 - T6)/4;
         	tB = tC + T6/2;
@@ -599,8 +599,8 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 		}
 		case SECTOR_2: // 1
 		{
-			u32 T6 = Y;
-			u32 T2 = Z;
+			s32 T6 = Y;
+			s32 T2 = Z;
 			Duty_Sat(PWM_Period, T2, T6);
 			tC = (PWM_Period - T6 - T2)/4;
         	tA = tC + T6/2;
@@ -611,8 +611,8 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 		}
 		case SECTOR_3: // 5
 		{
-			u32 T2 = X;
-			u32 T3 = -Y;
+			s32 T2 = X;
+			s32 T3 = -Y;
 			Duty_Sat(PWM_Period, T2, T3);
 			tA = (PWM_Period - T2 - T3)/4;
 			tC = tA + T3/2;
@@ -623,8 +623,8 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 		}
 		case SECTOR_4: // 4
 		{
-			u32 T1 = -X;
-			u32 T3 = Z;
+			s32 T1 = -X;
+			s32 T3 = Z;
 			Duty_Sat(PWM_Period, T1, T3);
 			tA = (PWM_Period - T1 - T3)/4;
 			tB = tA + T3/2;
@@ -635,8 +635,8 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 		}
 		case SECTOR_5: // 6
 		{
-			u32 T1 = -Y;
-			u32 T5 = -Z;
+			s32 T1 = -Y;
+			s32 T5 = -Z;
 			Duty_Sat(PWM_Period, T1, T5);
 			tB = (PWM_Period - T1 - T5)/4;
 			tA = tB + T5/2;
@@ -647,8 +647,8 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 		}					
 		case SECTOR_6: // 2
 		{
-			u32 T4 = Y;
-			u32 T5 = -X;
+			s32 T4 = Y;
+			s32 T5 = -X;
 			Duty_Sat(PWM_Period, T4, T5);
 			tB = (PWM_Period - T4 - T5)/4;
 			tC = tB + T5/2;
@@ -660,9 +660,9 @@ void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out) {
 		default:
 			break;
 	}	
-	out->n_Duty[0] = fclamp(tA, 0, PWM_half_period);
-	out->n_Duty[1] = fclamp(tB, 0, PWM_half_period);
-	out->n_Duty[2] = fclamp(tC, 0, PWM_half_period);
+	out->n_Duty[0] = sclamp(tA, 0, (s32)PWM_half_period);
+	out->n_Duty[1] = sclamp(tB, 0, (s32)PWM_half_period);
+	out->n_Duty[2] = sclamp(tC, 0, (s32)PWM_half_period);
 	out->n_lowDuty = low;
 	out->n_midDuty = midle;
 	out->n_Sector = sector;

Applications/foc/core/svpwm.h

@@ -7,7 +7,7 @@ void SVPWM_7(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_ti
 void SVPWM_ST(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out);
 void SVM_Get_Phase_Time(alpha_beta_t *alpha_beta, float vbus, u32 PWM_half_period, phase_time_t *phase_out, u8 *sector_out);
 #endif
-void SVM_Duty_Fix(AB_t *alb, s16q5_t vbus, u32 PWM_half_period, FOC_OutP *out);
+void SVM_Duty_Fix(AB_t *alb, float vbus, u32 PWM_half_period, FOC_OutP *out);
 
 #endif /* _SVPWM_H__ */
 

Applications/foc/motor/motor.c

@@ -52,6 +52,7 @@ static void MC_Check_MosVbusThrottle(void) {
 	}else if ((abc[0] > 0.5f) && (abc[1] < 0.001f || abc[2] < 0.001f)) {
 		PMSM_FOC_SetCriticalError(FOC_CRIT_Phase_Conn_Err);
 	}
+	sys_debug("phase vol %f, %f, %f\n", abc[0], abc[1], abc[2]);
 }
 
 static u32 _self_check_task(void *p) {

Applications/math/fast_math.h

@@ -26,7 +26,7 @@ void fast_sincos(float angle, float *sin, float *cos);
 float fast_arctan2(float y, float x);
 void SinCos_Lut(float angle, float *s, float *c);
 
-static __INLINE float fclamp(float v, float minv, float maxv) {
+static __INLINE int32_t sclamp(int32_t v, int32_t minv, int32_t maxv) {
 	if (v < minv) {
 		return minv;
 	}else if (v > maxv) {

Applications/prot/can_foc_msg.c

@@ -140,4 +140,12 @@ void can_plot2(s16 v1, s16 v2) {
 	can_send_message(get_indicator_can_id(0x45), data, sizeof(data), 0);
 }
 
+void can_plot3(s16 v1, s16 v2, s16 v3) {
+	u8 data[8];
+	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Plot));
+	encode_s16(data + 2, v1);
+	encode_s16(data + 4, v2);
+	encode_s16(data + 6, v3);
+	can_send_message(get_indicator_can_id(0x45), data, sizeof(data), 0);
+}
 

Applications/prot/can_foc_msg.h

@@ -15,5 +15,6 @@ void can_report_ext_status(u8 can);
 void can_report_plot_values(u8 can);
 void can_plot1(s16 v1);
 void can_plot2(s16 v1, s16 v2);
+void can_plot3(s16 v1, s16 v2, s16 v3);
 #endif	/*_Can_Foc_Msg_H__ */