加入plot type开启，选择，关闭功能

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

Applications/foc/commands.c

@@ -207,6 +207,16 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			}
 			break;
 		}
+		case Foc_Set_Plot_Type:
+		{
+			u8 plot = decode_u8((u8 *)command->data);
+			if (plot >= Plot_t_Max) {
+				erroCode = FOC_Param_Err;
+			}else {
+				PMSM_FOC_Set_PlotType((Plot_t)plot);
+			}
+			break;
+		}
 		default:
 		{
 			erroCode = FOC_Unknow_Cmd;

Applications/foc/commands.h

@@ -21,6 +21,7 @@ typedef enum {
 	Foc_Conf_Pid,
 	Foc_Start_DQ_Calibrate,
 	Foc_Set_IS_Curr_Angle,  //设置DQ电流矢量和超前角
+	Foc_Set_Plot_Type,
 	Foc_Hall_Phase_Cali_Result = 160,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,

Applications/foc/core/PMSM_FOC_Core.c

@@ -60,7 +60,7 @@ static __INLINE float Circle_Limitation(DQ_t *vdq, float vDC, float module, DQ_t
 #ifdef VD_PRIO_HIGH		
 		out->d = vdq->d;
 		out->q = sqrtf(sq_vDC - out->d*out->d);
-		return 1.1f;
+		return 1.1f; //just return the modulation big than 1.0
 #else
 		float r = sqrtf(sq_vDC / sq_vdq);
 		out->d = vdq->d * r;
@@ -308,12 +308,21 @@ float target_q = 0.0f;
 static u32 PMSM_FOC_Debug_Task(void *p) {
 	if (gFoc_Ctrl.in.b_motEnable) {
 #ifdef CONFIG_DQ_STEP_RESPONSE
-		plot_2data16(FtoS16x10(target_q), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q));
+		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
-		//plot_3data16(FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[0]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[1]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[2]));
-		//plot_3data16(gFoc_Ctrl.in.s_targetTorque, gFoc_Ctrl.in.s_targetRPM, gFoc_Ctrl.in.s_motRPM);
-		//plot_3data16(FtoS16x10(gFoc_Ctrl.out.s_RealIdq.d), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q), FtoS16x10(gFoc_Ctrl.idq_ctl[1].s_FinalTgt));
-		//plot_3data16( gFoc_Ctrl.in.s_motRPM, speed_td.target, speed_td.diff);
+		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_2data16(FtoS16x10(gFoc_Ctrl.out.s_RealIdq.d), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q));
+		}else if (gFoc_Ctrl.plot_type == Plot_Spd_flow) {
+			plot_2data16(gFoc_Ctrl.in.s_targetRPM, gFoc_Ctrl.in.s_motRPM);
+		}
 #endif
 	}
 	return 1;
@@ -339,8 +348,6 @@ void PMSM_FOC_Schedule(void) {
 	#endif
 		err = target_q - gFoc_Ctrl.out.s_RealIdq.q;
 		gFoc_Ctrl.in.s_targetVdq.q = PI_Controller_RunSerial(gFoc_Ctrl.pi_iq, err);
-		gFoc_Ctrl.out.test_targetIQ = target_q;
-
 	}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]);	
@@ -357,9 +364,14 @@ void PMSM_FOC_Schedule(void) {
 	pwm_update_duty(gFoc_Ctrl.out.n_Duty[0], gFoc_Ctrl.out.n_Duty[1], gFoc_Ctrl.out.n_Duty[2]);
 	pwm_update_sample(gFoc_Ctrl.out.n_Sample1, gFoc_Ctrl.out.n_Sample2, gFoc_Ctrl.out.n_CPhases);
 
-	if (gFoc_Ctrl.ctrl_count % 5 == 0) {
-		plot_3data16(FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[0]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[1]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[2]));
-		//plot_3data16((s16)gFoc_Ctrl.out.s_RealIdq.d, (s16)gFoc_Ctrl.out.s_RealIdq.q, (s16)gFoc_Ctrl.idq_ctl[1].s_Cp);
+	if (gFoc_Ctrl.plot_type != Plot_None) {
+		if (gFoc_Ctrl.ctrl_count % 5 == 0) {
+			if (gFoc_Ctrl.plot_type == Plot_Phase_curr) {
+				plot_3data16(FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[0]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[1]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[2]));
+			}else if (gFoc_Ctrl.plot_type == Plot_Phase_vol) {
+				plot_3data16(FtoS16x10(gFoc_Ctrl.in.s_vABC[0]), FtoS16x10(gFoc_Ctrl.in.s_vABC[1]), FtoS16x10(gFoc_Ctrl.in.s_vABC[2]));
+			}
+		}
 	}
 }
 
@@ -842,6 +854,10 @@ void PMSM_FOC_ClrCriticalError(u8 err) {
 u32 PMSM_FOC_GetCriticalError(void) {
 	return gFoc_Ctrl.out.n_CritiCalErrMask;
 }
+
+void PMSM_FOC_Set_PlotType(Plot_t t) {
+	gFoc_Ctrl.plot_type = t;
+}
 //获取母线电流
 float PMSM_FOC_Calc_iDC(void) {
 	float vd = gFoc_Ctrl.out.s_OutVdq.d;

Applications/foc/core/PMSM_FOC_Core.h

@@ -20,6 +20,47 @@ typedef enum {
 	EPM_Dir_Forward,
 }EPM_Dir_t;
 
+typedef enum {
+	Plot_None,
+	Plot_D_Step,
+	Plot_Q_Step,
+	Plot_D_flow,
+	Plot_Q_flow,
+	Plot_Spd_flow,
+	Plot_Phase_curr,
+	Plot_Phase_vol,
+	Plot_DQ_Curr,
+	Plot_t_Max,
+}Plot_t;
+
+typedef enum {
+	FOC_Success = 0,
+	FOC_NotAllowed = 1,
+	FOC_Have_CritiCal_Err,
+	FOC_Throttle_Err, //ready的时候检测到转把信号
+	FOC_NowAllowed_With_Speed,
+	FOC_Speed_TooLow,
+	FOC_NotCruiseMode,
+	FOC_Param_Err,
+	FOC_Unknow_Cmd,
+}FOC_ErrCode_t;
+
+typedef enum {
+	FOC_CRIT_OV_Vol_Err,
+	FOC_CRIT_UN_Vol_Err,
+	FOC_CRIT_DC_Curr_OV_Err,
+	FOC_CRIT_Phase_Curr_OV_Err,
+	FOC_CRIT_Phase_Err,
+	FOC_CRIT_Encoder_Err,
+	FOC_CRIT_Brake_Err,
+	FOC_CRIT_CURR_OFF_Err,
+	FOC_CRIT_H_MOS_Err,
+	FOC_CRIT_L_MOS_Err,
+	FOC_CRIT_Phase_Conn_Err,
+	FOC_CRIT_MOTOR_TEMP_Err,
+	FOC_CRIT_MOS_TEMP_Err,
+	FOC_CRIT_Err_Max = 32,	
+}FOC_CritiCal_Ebit_t;
 
 typedef struct {
 	u8 	  n_poles;
@@ -98,7 +139,6 @@ typedef struct {
 	float s_FilteriDC;
 	float f_vdqRation;
 	s16   test_sample;
-	float test_targetIQ;
 	float sin;
 	float cos;
 	u8    n_Error;
@@ -129,38 +169,10 @@ typedef struct {
 	FOC_Params    params;
 	FOC_UserLimit userLim;
 	FOC_HwLimit   hwLim;
+	Plot_t        plot_type;
 	int           ctrl_count;
 }PMSM_FOC_Ctrl;
 
-typedef enum {
-	FOC_Success = 0,
-	FOC_NotAllowed = 1,
-	FOC_Have_CritiCal_Err,
-	FOC_Throttle_Err, //ready的时候检测到转把信号
-	FOC_NowAllowed_With_Speed,
-	FOC_Speed_TooLow,
-	FOC_NotCruiseMode,
-	FOC_Param_Err,
-	FOC_Unknow_Cmd,
-}FOC_ErrCode_t;
-
-typedef enum {
-	FOC_CRIT_OV_Vol_Err,
-	FOC_CRIT_UN_Vol_Err,
-	FOC_CRIT_DC_Curr_OV_Err,
-	FOC_CRIT_Phase_Curr_OV_Err,
-	FOC_CRIT_Phase_Err,
-	FOC_CRIT_Encoder_Err,
-	FOC_CRIT_Brake_Err,
-	FOC_CRIT_CURR_OFF_Err,
-	FOC_CRIT_H_MOS_Err,
-	FOC_CRIT_L_MOS_Err,
-	FOC_CRIT_Phase_Conn_Err,
-	FOC_CRIT_MOTOR_TEMP_Err,
-	FOC_CRIT_MOS_TEMP_Err,
-	FOC_CRIT_Err_Max = 32,	
-}FOC_CritiCal_Ebit_t;
-
 #define CTRL_MODE_OPEN                      ((u8)0U)
 #define CTRL_MODE_SPD                       ((u8)1U)
 #define CTRL_MODE_TRQ                       ((u8)2U)
@@ -251,6 +263,7 @@ void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb);
 void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kb);
 bool PMSM_FOC_MotorLocking(void);
 void PMSM_FOC_Slow_Task(void);
+void PMSM_FOC_Set_PlotType(Plot_t t);
 
 #endif /* _PMSM_FOC_Core_H__ */