欠压限速，限母线电流

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

Applications/app/nv_storage.c

@@ -170,9 +170,9 @@ void nv_storage_init(void) {
 	nv_read_motor_params();
 	nv_read_foc_params();
 	sys_debug("encoder_off = %f\n", m_params.offset);
-	//nv_default_motor_params();
-	//nv_default_foc_params();
-	//nv_save_foc_params();
-	//nv_save_motor_params();
+	nv_default_motor_params();
+	nv_default_foc_params();
+	nv_save_foc_params();
+	nv_save_motor_params();
 }
 

Applications/bsp/board_mc_v1.h

@@ -20,6 +20,9 @@
 #define CONFIG_BEEP 
 #define CONFIG_STALL_MAX_CURRENT 100.0f //最大堵转相电流电流
 #define CONFIG_STALL_MAX_TIME    3000   //ms, 超过最大堵转电流持续时间，判断堵转
+#define CONFIG_UNDER_VOL_RPM     1000
+#define CONFIG_UNDER_VOL_PHASE_CURR 100.0F
+#define CONFIG_UNDER_VOL_DC_CURR 15.0F
 
 #define SCHED_TIMER TIMER5
 #define SCHED_TIMER_RCU RCU_TIMER5

Applications/foc/commands.c

@@ -108,10 +108,10 @@ static void process_foc_command(foc_cmd_body_t *command) {
 				u8  maxiDC = decode_u8((u8 *)command->data + 4);
 				PMSM_FOC_SpeedLimit((float)maxRPM);
 				PMSM_FOC_PhaseCurrLim((float)maxPhaseCurr);
-				PMSM_FOC_iBusLimit((float)maxiDC);
+				PMSM_FOC_DCCurrLimit((float)maxiDC);
 				encode_u16(response + 3, (u16)PMSM_FOC_GetSpeedLimit());
 				encode_u16(response + 5, (u16)PMSM_FOC_GetPhaseCurrLim());
-				encode_u8(response + 7, (u8)PMSM_FOC_GetiBusLimit());
+				encode_u8(response + 7, (u8)PMSM_FOC_GetDCCurrLimit());
 				len += 5;
 			}
 			break;
@@ -127,8 +127,8 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Set_iDC_Limit:
 		{
 			u8 current = decode_u8(((u8 *)command->data));
-			PMSM_FOC_iBusLimit((float)current);
-			encode_u8(response + 3, (u8)PMSM_FOC_GetiBusLimit());
+			PMSM_FOC_DCCurrLimit((float)current);
+			encode_u8(response + 3, (u8)PMSM_FOC_GetDCCurrLimit());
 			len += 1;
 			break;
 		}

Applications/foc/core/PMSM_FOC_Core.c

@@ -176,6 +176,7 @@ static void PMSM_FOC_UserInit(void) {
 	gFoc_Ctrl.userLim.s_PhaseVoleBrkLim = gFoc_Ctrl.hwLim.s_PhaseVolMax;
 	eRamp_init_target(&gFoc_Ctrl.userLim.rpmLimRamp, gFoc_Ctrl.userLim.s_motRPMLim);
 	eRamp_init_target(&gFoc_Ctrl.userLim.phaseCurrLimRamp, gFoc_Ctrl.userLim.s_PhaseCurrLim);
+	eRamp_init_target(&gFoc_Ctrl.userLim.DCCurrLimRamp, gFoc_Ctrl.userLim.s_iDCLim);
 }
 
 void PMSM_FOC_CoreInit(void) {
@@ -387,7 +388,7 @@ u8 PMSM_FOC_CtrlMode(void) {
 static __INLINE float PMSM_FOC_Limit_Power(float maxTrq) {
 #if 0
 	PI_Ctrl_Power.max = maxTrq;
-	float errRef = gFoc_Ctrl.userLim.s_iDCLim - gFoc_Ctrl.out.s_FilteriDC;
+	float errRef = eRamp_get_intepolation(&gFoc_Ctrl.userLim.DCCurrLimRamp) - gFoc_Ctrl.out.s_FilteriDC;
 	return PI_Controller_run(gFoc_Ctrl.pi_power, errRef);
 #else
 	return maxTrq;
@@ -463,6 +464,20 @@ void PMSM_FOC_idqCalc(void) {
 void PMSM_FOC_Slow_Task(void) {
 	eRamp_running(&gFoc_Ctrl.userLim.phaseCurrLimRamp);
 	eRamp_running(&gFoc_Ctrl.userLim.rpmLimRamp);
+	eRamp_running(&gFoc_Ctrl.userLim.DCCurrLimRamp);
+
+	if (gFoc_Ctrl.out.n_CritiCalErrMask != gFoc_Ctrl.out.n_CritiCalErrPrev) {
+		/* 欠压降速，限制相电流 */
+		if (!(gFoc_Ctrl.out.n_CritiCalErrPrev & gFoc_Ctrl.out.n_CritiCalErrMask) && (FOC_CRIT_UN_Vol_Err & gFoc_Ctrl.out.n_CritiCalErrMask)) {
+			if (gFoc_Ctrl.userLim.s_motRPMLim > CONFIG_UNDER_VOL_RPM) {
+				PMSM_FOC_SpeedLimit(CONFIG_UNDER_VOL_RPM);
+			}
+			if (gFoc_Ctrl.userLim.s_iDCLim > CONFIG_UNDER_VOL_DC_CURR) {
+				PMSM_FOC_DCCurrLimit(CONFIG_UNDER_VOL_DC_CURR);
+			}
+		}
+		gFoc_Ctrl.out.n_CritiCalErrPrev = gFoc_Ctrl.out.n_CritiCalErrMask;
+	}
 	PMSM_FOC_idqCalc();
 }
 
@@ -491,19 +506,32 @@ bool PMSM_FOC_Is_Start(void) {
 	return gFoc_Ctrl.in.b_motEnable;
 }
 
-void PMSM_FOC_iBusLimit(float ibusLimit) {
+void PMSM_FOC_DCCurrLimit(float ibusLimit) {
+	if (ibusLimit > gFoc_Ctrl.hwLim.s_iDCMax) {
+		ibusLimit = gFoc_Ctrl.hwLim.s_iDCMax;
+	}
+	if (gFoc_Ctrl.out.n_CritiCalErrMask & FOC_CRIT_UN_Vol_Err) {
+		if (ibusLimit > CONFIG_UNDER_VOL_DC_CURR) {
+			ibusLimit = CONFIG_UNDER_VOL_DC_CURR;
+		}
+	}
 	gFoc_Ctrl.userLim.s_iDCLim = (ibusLimit);
+	eRamp_set_step_target(&gFoc_Ctrl.userLim.DCCurrLimRamp, ibusLimit, CONFIG_eCTRL_STEP_TS, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
 }
 
-float PMSM_FOC_GetiBusLimit(void) {
+float PMSM_FOC_GetDCCurrLimit(void) {
 	return gFoc_Ctrl.userLim.s_iDCLim;
 }
 
-
 void PMSM_FOC_SpeedLimit(float speedLimit) {
 	if (speedLimit > gFoc_Ctrl.hwLim.s_motRPMMax) {
 		speedLimit = gFoc_Ctrl.hwLim.s_motRPMMax;
 	}
+	if (gFoc_Ctrl.out.n_CritiCalErrMask & FOC_CRIT_UN_Vol_Err) {
+		if (speedLimit > CONFIG_UNDER_VOL_RPM) {
+			speedLimit = CONFIG_UNDER_VOL_RPM;
+		}
+	}
 	gFoc_Ctrl.userLim.s_motRPMLim = (speedLimit);
 	eRamp_set_step_target(&gFoc_Ctrl.userLim.rpmLimRamp, speedLimit, CONFIG_eCTRL_STEP_TS, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
 }
@@ -568,6 +596,11 @@ void PMSM_FOC_PhaseCurrLim(float lim) {
 	if (lim > gFoc_Ctrl.hwLim.s_PhaseCurrMax) {
 		lim = gFoc_Ctrl.hwLim.s_PhaseCurrMax;
 	}
+	if (gFoc_Ctrl.out.n_CritiCalErrMask & FOC_CRIT_UN_Vol_Err) {
+		if (lim > CONFIG_UNDER_VOL_PHASE_CURR) {
+			lim = CONFIG_UNDER_VOL_PHASE_CURR;
+		}
+	}
 	gFoc_Ctrl.userLim.s_PhaseCurrLim = lim;
 	eRamp_set_step_target(&gFoc_Ctrl.userLim.phaseCurrLimRamp, lim, CONFIG_eCTRL_STEP_TS, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
 }

Applications/foc/core/PMSM_FOC_Core.h

@@ -67,6 +67,7 @@ typedef struct {
 	float s_vDCMaxLim;
 	e_Ramp rpmLimRamp;
 	e_Ramp phaseCurrLimRamp;
+	e_Ramp DCCurrLimRamp;
 }FOC_UserLimit;
 
 typedef struct {
@@ -101,6 +102,7 @@ typedef struct {
 	float cos;
 	u8    n_Error;
 	u32   n_CritiCalErrMask;
+	u32   n_CritiCalErrPrev;
 }FOC_OutP;
 
 typedef struct {
@@ -200,7 +202,7 @@ u8 PMSM_FOC_CtrlMode(void);
 void PMSM_FOC_idqCalc(void);
 void PMSM_FOC_Start(u8 nCtrlMode);
 void PMSM_FOC_Stop(void);
-void PMSM_FOC_iBusLimit(float ibusLimit);
+void PMSM_FOC_DCCurrLimit(float ibusLimit);
 void PMSM_FOC_SpeedLimit(float speedLimit);
 float PMSM_FOC_GetSpeedLimit(void);
 float PMSM_FOC_GetVbusVoltage(void);
@@ -239,7 +241,7 @@ void PMSM_FOC_ClrCriticalError(u8 err);
 u32 PMSM_FOC_GetCriticalError(void);
 void PMSM_FOC_PhaseCurrLim(float lim);
 float PMSM_FOC_GetPhaseCurrLim(void);
-float PMSM_FOC_GetiBusLimit(void);
+float PMSM_FOC_GetDCCurrLimit(void);
 void PMSM_FOC_GetRunningStatus(u8 *data);
 bool PMSM_FOC_Is_CruiseEnabled(void);
 void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb);

Applications/foc/motor/motor.c

@@ -41,16 +41,18 @@ static void mc_gpio_init(void) {
 #endif
 }
 
-static void MC_Mos_Check_error(void) {
+static void MC_Check_MosVbusThrottle(void) {
 	int count = 1000;
 	gpio_phase_u_detect(true);
 	while(count-- >= 0) {
 		task_udelay(20);
 		sample_uvw_phase();
+		sample_throttle();
+		sample_vbus();
 	}
+	gpio_phase_u_detect(false);
 	float abc[3];
 	get_phase_vols(abc);
-	gpio_phase_u_detect(false);
 	if (abc[0] > 10 || abc[1] > 10 || abc[2] > 10) {
 		PMSM_FOC_SetCriticalError(FOC_CRIT_H_MOS_Err);
 	}else if (abc[0] < 0.001f){
@@ -58,7 +60,12 @@ static void MC_Mos_Check_error(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("vol %f, %f, %f\n", abc[0], abc[1], abc[2]);
+	if (get_vbus_float() < nv_get_foc_params()->s_minDCVol) {
+		PMSM_FOC_SetCriticalError(FOC_CRIT_UN_Vol_Err);
+	}
+	if (get_vbus_float() > nv_get_foc_params()->s_maxDCVol) {
+		PMSM_FOC_SetCriticalError(FOC_CRIT_OV_Vol_Err);
+	}
 }
 
 static u32 _self_check_task(void *p) {
@@ -66,6 +73,10 @@ static u32 _self_check_task(void *p) {
 		err_add_record(FOC_CRIT_Encoder_Err, 0);
 		PMSM_FOC_SetCriticalError(FOC_CRIT_Encoder_Err);
 	}
+	if (get_vbus_float() < nv_get_foc_params()->s_minDCVol) {
+		PMSM_FOC_SetCriticalError(FOC_CRIT_UN_Vol_Err);
+	}
+
 	return 0;
 }
 
@@ -77,7 +88,7 @@ void mc_init(void) {
 	foc_command_init();
 	PMSM_FOC_CoreInit();
 	mc_gpio_init();
-	MC_Mos_Check_error();
+	MC_Check_MosVbusThrottle();
 	sched_timer_enable(CONFIG_SPD_CTRL_US);
 	shark_task_create(_self_check_task, NULL);
 }
@@ -91,7 +102,7 @@ bool mc_start(u8 mode) {
 		return true;
 	}
 
-	MC_Mos_Check_error();
+	MC_Check_MosVbusThrottle();
 
 	if (PMSM_FOC_GetCriticalError() != 0) {
 		PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);

Applications/foc/samples.c

@@ -15,8 +15,6 @@ typedef struct {
 	float lowpass;
 }samples_t;
 
-static void sample_vbus(void);
-static void sample_throttle(void);
 static u32 sample_task(void *);
 static samples_t _vbus;
 #ifdef THROTTLE_CHAN
@@ -28,7 +26,7 @@ static samples_t _uvw_phase[3];
 void samples_init(void){
 	_vbus.filted_value = (CONFIG_RATED_DC_VOL);
 	_vbus.value = (CONFIG_RATED_DC_VOL);
-	_vbus.lowpass = (0.001f); 
+	_vbus.lowpass = (0.01f); 
 	sample_vbus();
 #ifdef THROTTLE_CHAN
 	_throttle.filted_value = (0);
@@ -78,7 +76,7 @@ static u32 sample_task(void *param) {
 	return 0;
 }
 
-static void sample_vbus(void){
+void sample_vbus(void){
 	u32 ticks = task_ticks_abs();
 	s16 vadc = adc_get_vbus();
 	_vbus.value = (float)vadc * VBUS_VOL_CEOF;
@@ -86,7 +84,7 @@ static void sample_vbus(void){
 	_vbus.filted_int = (int)_vbus.filted_value;
 }
 
-static void sample_throttle(void){
+void sample_throttle(void){
 #ifdef THROTTLE_CHAN
 	s16 vadc = adc_get_throttle();
 	_throttle.value = (float)vadc * THROTTLE_VOL_CEOF;

Applications/foc/samples.h

@@ -11,6 +11,8 @@ float get_throttle_sfix5(void);
 float get_throttle_float(void);
 void get_phase_vols(float *uvw);
 void sample_uvw_phase(void);
+void sample_vbus(void);
+void sample_throttle(void);
 
 #endif /* _SAMPLES_H__ */