硬件欠压快速判断

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

Applications/bsp/gd32/board_mc105_v3.h

@@ -9,7 +9,7 @@
 #define CONFIG_MOS_MAX_VOL 125.0F
 #define CONFIG_MAX_DC_VOL 110.0F
 #define CONFIG_RATED_DC_VOL (96.0f)   /* 母线最大电压 V*/
-#define CONFIG_MIN_DC_VOL   (36.0f)
+#define CONFIG_MIN_DC_VOL   (60.0f)
 
 #define CONFIG_MAX_VBUS_CURRENT 250.0f
 #define CONFIG_MAX_CHRG_CURRENT (-100.0f)

Applications/foc/core/PMSM_FOC_Core.c

@@ -193,8 +193,8 @@ void PMSM_FOC_CoreInit(void) {
 
 	PMSM_FOC_Conf_PID();
 	
-	memset(&gFoc_Ctrl.in, 0, sizeof(gFoc_Ctrl.in));
-	memset(&gFoc_Ctrl.out, 0, sizeof(gFoc_Ctrl.out));
+	memset(&gFoc_Ctrl.in, 0, sizeof(FOC_InP));
+	memset(&gFoc_Ctrl.out, 0, sizeof(FOC_OutP));
 	
 	gFoc_Ctrl.hwLim.s_iDCMax = CONFIG_MAX_VBUS_CURRENT;
 	gFoc_Ctrl.hwLim.s_motRPMMax = CONFIG_MAX_MOT_RPM;
@@ -355,6 +355,7 @@ static __INLINE bool PMSM_FOC_Update_Input(void) {
 	if (!foc_observer_diagnostic(enc_angle, enc_vel)){
 		/* detect encoder angle error, do something here */
 		if (!foc_observer_sensorless_stable()) {
+			gFoc_Ctrl.in.s_motVelocity = 0;
 			return false;
 		}
 		enc_angle = foc_observer_sensorless_angle();

Applications/foc/core/PMSM_FOC_Core.h

@@ -67,6 +67,7 @@ typedef enum {
 	FOC_CRIT_IDC_OV,
 	FOC_CRIT_THRO_Err,
 	FOC_CRIT_ENC_AB_Err,
+	FOC_CRIT_Vol_HW_Err,
 	FOC_CRIT_Err_Max = 32,
 }FOC_CritiCal_Ebit_t;
 

Applications/foc/motor/encoder.c

@@ -238,7 +238,7 @@ static void encoder_detect_error(s16 cnt) {
 					g_encoder.enc_maybe_err = ENCODER_AB_ERR;
 					enc_delta_err1 = g_encoder.last_delta_cnt;
 					enc_delta_err2 = (s16)((g_encoder.est_vel_counts/g_encoder.cpr) * 60.0f);
-					enc_r = r*10;
+					enc_r = r*100;
 					enc_cnt = cnt;
 					enc_last_cnt = g_encoder.last_cnt;
 				}

Applications/foc/motor/motor.c

@@ -176,6 +176,7 @@ static void _mc_internal_init(u8 mode, bool start) {
 	motor.b_limit_pending = false;
 	motor.f_epm_trq = 0;
 	motor.f_epm_vel = 0;
+	motor.s_vbus_hw_min = nv_get_foc_params()->s_minDCVol;
 }
 
 static void _led_off_timer_handler(shark_timer_t *t) {
@@ -244,7 +245,7 @@ mc_gear_t *mc_get_gear_config(void) {
 }
 
 bool mc_critical_can_not_run(void) {
-	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_IDC_OV) | FOC_Cri_Err_Mask(FOC_CRIT_MOTOR_TEMP_Err) | FOC_Cri_Err_Mask(FOC_CRIT_MOS_TEMP_Err) | FOC_Cri_Err_Mask(FOC_CRIT_Angle_Err);
+	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_IDC_OV) | FOC_Cri_Err_Mask(FOC_CRIT_MOTOR_TEMP_Err) | FOC_Cri_Err_Mask(FOC_CRIT_MOS_TEMP_Err) | FOC_Cri_Err_Mask(FOC_CRIT_Angle_Err) | FOC_Cri_Err_Mask(FOC_CRIT_Vol_HW_Err);
 	u32 err = motor.n_CritiCalErrMask & mask;
 	return (err != 0);
 }
@@ -341,7 +342,7 @@ bool mc_stop(void) {
 		return false;
 	}
 
-	if (PMSM_FOC_GetSpeed() > CONFIG_ZERO_SPEED_RPM) {
+	if (motor_encoder_get_speed() > CONFIG_ZERO_SPEED_RPM) {
 		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 		return false;
 	}
@@ -989,13 +990,34 @@ void motor_debug(void) {
 	sys_debug("err3: %f, %d, %d, %d, %d\n", (float)mc_error.ibus_x10/10.0f, mc_error.sensorless_rpm, mc_error.mos_temp, mc_error.mot_temp, mc_error.enc_error);
 }
 
+static void motor_vbus_crit_low(s16 curr_vbus) {
+	static u16 _vbus_e_count = 0;
+
+	if (curr_vbus < motor.s_vbus_hw_min) {
+		_vbus_e_count ++;
+		if (_vbus_e_count >= 2) {
+			if (PMSM_FOC_Is_Start()) {
+				pwm_disable_channel();
+				mc_save_err_runtime();
+				PMSM_FOC_Stop();
+			}
+			if (mc_set_critical_error(FOC_CRIT_Vol_HW_Err)) {
+				mc_crit_err_add_s16(FOC_CRIT_Vol_HW_Err, curr_vbus);
+			}
+		}
+	}else {
+		_vbus_e_count = 0;
+	}
+}
+
 void TIMER_UP_IRQHandler(void){
 	if (!motor.b_start && !PMSM_FOC_Is_Start()) {
 		motor_encoder_update();
+		motor_vbus_crit_low((s16)get_vbus_int());
 	}
 }
 
-measure_time_t g_meas_foc = {.exec_max_time = 20, .intval_max_time = 62,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
+measure_time_t g_meas_foc = {.exec_max_time = 25, .intval_max_time = 62,  .intval_low_err = 0, .intval_hi_err = 0, .first = true,};
 #define TIME_MEATURE_START() time_measure_start(&g_meas_foc)
 #define TIME_MEATURE_END() time_measure_end(&g_meas_foc)
 
@@ -1010,22 +1032,28 @@ void ADC_IRQHandler(void) {
 		return;
 	}
 	TIME_MEATURE_START();
+
+	motor_vbus_crit_low((s16)sample_vbus_raw()); //need fast detect vbus very low, to stop the motor
+
 	if (!PMSM_FOC_Schedule()) {/* FOC 角度错误，立即停机 */
 		if (PMSM_FOC_Is_Start()) {
 			pwm_disable_channel();
 			/* 记录错误 */
 			mc_save_err_runtime();
-			mc_set_critical_error(FOC_CRIT_Angle_Err);
-			mc_crit_err_add_s16(FOC_CRIT_Angle_Err, (s16)motor_encoder_get_speed());
+			PMSM_FOC_Stop();
+			g_meas_foc.first = true;
+			if (mc_set_critical_error(FOC_CRIT_Angle_Err)) {
+				mc_crit_err_add_s16(FOC_CRIT_Angle_Err, (s16)motor_encoder_get_speed());
+			}
 			if (motor_encoder_may_error() == ENCODER_PWM_ERR) {
-				mc_set_critical_error(FOC_CRIT_Encoder_Err);
-				mc_crit_err_add(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms, enc_delta_err2);
+				if (mc_set_critical_error(FOC_CRIT_Encoder_Err)) {
+					mc_crit_err_add(FOC_CRIT_Encoder_Err, (s16)enc_pwm_err_ms, enc_delta_err2);
+				}
 			}else if (motor_encoder_may_error() == ENCODER_AB_ERR) {
-				mc_set_critical_error(FOC_CRIT_ENC_AB_Err);
-				mc_crit_err_add(FOC_CRIT_ENC_AB_Err, enc_delta_err1, enc_delta_err2);
+				if (mc_set_critical_error(FOC_CRIT_ENC_AB_Err)) {
+					mc_crit_err_add(FOC_CRIT_ENC_AB_Err, enc_delta_err1, enc_delta_err2);
+				}
 			}
-			PMSM_FOC_Stop();
-			g_meas_foc.first = true;
 		}
 	}
 	TIME_MEATURE_END();

Applications/foc/motor/motor.h

@@ -55,6 +55,7 @@ typedef struct {
 	bool   b_wait_brk_release;
 	u8     mos_lim;
 	u8     motor_lim;
+	s16    s_vbus_hw_min;
 	user_rt_set u_set; //用户运行时设置
 	fan_t  fan[2];
 }motor_t;