1. pwm 停止后，关闭上下桥

2. 开机和启动前检测mos的上下桥状态

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

Applications/app/app.c

@@ -89,8 +89,10 @@ void app_start(void){
 
 static u32 _app_report_task(void *p) {
 	static u32 loop = 0;
-	can_report_power(0x45, (s16)PMSM_FOC_GetSpeed(), PMSM_FOC_GetVbusVoltage(), PMSM_FOC_GetVbusCurrent());
-	can_report_dq_current(0x45, PMSM_FOC_GetDQCurrent()->d, PMSM_FOC_GetDQCurrent()->q);
+	can_report_power(0x45);
+	can_report_dq_current(0x45);
+	can_report_foc_status(0x45);
+	can_report_phase_voltage(0x45);
 	if (++loop % 10 == 0) {
 		can_report_pid_value(0x45, PID_TRQ_id);
 		sys_debug("start time %d\n", shark_get_seconds());

Applications/app/nv_storage.c

@@ -46,7 +46,7 @@ static void nv_default_foc_params(void) {
 	foc_params.s_PhaseCurrLim = 180;
 	foc_params.s_maxRPM = 8000;
 	foc_params.s_maxEpmRPM = 133;
-	foc_params.s_maxTorque = 50;
+	foc_params.s_maxTorque = 50;//foc_params.s_PhaseCurrLim;
 	foc_params.s_PhaseCurreBrkLim = 0.0f;
 	foc_params.n_currentBand = 500;
 	foc_params.n_modulation = 1.0f;
@@ -64,12 +64,12 @@ static void nv_default_foc_params(void) {
 	foc_params.pid_conf[PID_Q_id].kb = 0;
 
 	foc_params.pid_conf[PID_TRQ_id].kp = 0.13f;
-	foc_params.pid_conf[PID_TRQ_id].ki = 0.4f;
+	foc_params.pid_conf[PID_TRQ_id].ki = 0.3f;
 	foc_params.pid_conf[PID_TRQ_id].kb = 1.0f;
 
 	foc_params.pid_conf[PID_Spd_id].kp = 0.13f;
 	foc_params.pid_conf[PID_Spd_id].ki = 0.08f;
-	foc_params.pid_conf[PID_Spd_id].kb = 1.2f;
+	foc_params.pid_conf[PID_Spd_id].kb = 1.0f;
 
 	foc_params.pid_conf[PID_Pow_id].kp = 0.5f;
 	foc_params.pid_conf[PID_Pow_id].ki = 100.0f;
@@ -154,6 +154,6 @@ void nv_storage_init(void) {
 	nv_read_motor_params();
 	nv_read_foc_params();
 	nv_default_motor_params();
-	//nv_default_foc_params();
+	nv_default_foc_params();
 }
 

Applications/bsp/gpio.c

@@ -1,6 +1,7 @@
 #include "bsp/gpio.h"
 #include "libs/utils.h"
 
+#ifdef GD32_FOC_DEMO
 #define IR2136S_Enable_pin 0
 #define LED1_Enable_pin 1
 #define LED2_Enable_pin 2
@@ -8,7 +9,7 @@
 #define KEY_Start_pin   4
 #define KEY_Stop_pin    5
 #define KEY_Func_pin   6
-
+#endif
 /*
 * gpio.c
 * all pins used as gpio(in/out/irq) must be init&accessed here
@@ -21,7 +22,6 @@ const static gpio_pin_config_t _pins[] = {
 	[LED3_Enable_pin] = {GPIOC, GPIO_PIN_5, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, 0},
 	[KEY_Start_pin] = {GPIOB, GPIO_PIN_5, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, 0},
 	[KEY_Stop_pin] = {GPIOB, GPIO_PIN_11, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, 0},
-//	[KEY_Func_pin] = {GPIOC, GPIO_PIN_0, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, 0},
 #endif	
 };
 void gpio_pin_init(void){
@@ -44,7 +44,6 @@ void gpio_pin_init(void){
 	gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_2);
 	gpio_beep(500);
 #endif
-	
 }
 
 
@@ -56,6 +55,17 @@ void gpio_beep(u32 ms) {
 #endif
 }
 
+void gpio_phase_u_detect(bool enable) {
+	if (enable) {
+		gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_14);
+		gpio_pin_remap_config(GPIO_SWJ_DISABLE_REMAP, ENABLE);
+		gpio_bit_write(GPIOA, GPIO_PIN_14, SET);
+	}else {
+		gpio_init(GPIOA, GPIO_MODE_IN_FLOATING, GPIO_OSPEED_50MHZ, GPIO_PIN_14);
+		gpio_pin_remap_config(GPIO_SWJ_SWDPENABLE_REMAP, ENABLE);
+	}
+}
+
 bool gpio_get_brake(void) {
 	return 0;
 }

Applications/bsp/gpio.h

@@ -23,5 +23,6 @@ int gpio_startkey_value(void);
 int gpio_stopkey_value(void);
 int gpio_funckey_value(void);
 void gpio_beep(u32 ms);
+void gpio_phase_u_detect(bool enable);
 
 #endif /* _GPIO_PIN_H__ */

Applications/bsp/pwm.c

@@ -114,7 +114,7 @@ static void _init_pwm_timer(void) {
     timer_ocintpara.ocpolarity   = TIMER_OC_POLARITY_HIGH;
     timer_ocintpara.ocnpolarity  = TIMER_OCN_POLARITY_HIGH;
     timer_ocintpara.ocidlestate  = TIMER_OC_IDLE_STATE_LOW;
-    timer_ocintpara.ocnidlestate = TIMER_OCN_IDLE_STATE_HIGH;
+    timer_ocintpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW;
 
     timer_channel_output_config(timer,TIMER_CH_0,&timer_ocintpara);
     timer_channel_output_pulse_value_config(timer,TIMER_CH_0,half_period/2);

Applications/foc/commands.h

@@ -30,6 +30,7 @@ typedef enum {
 	Foc_Report_Dq_Vol,			//u32, u32
 	Foc_Report_Power,
 	Foc_Report_Pid,
+	Foc_Report_Status,
 	Foc_Cmd_Max
 }foc_cmd_t;
 #define CMD_2_CAN_KEY(cmd) ((u16)(((u16)cmd) | (CAN_MY_ADDRESS<<8)))

Applications/foc/core/PMSM_FOC_Core.c

@@ -13,6 +13,7 @@
 #include "bsp/pwm.h"
 #include "libs/logger.h"
 #include "math/fir.h"
+
 PMSM_FOC_Ctrl _gFOC_Ctrl;
 static Fir_t phase1, phase2;
 TD_t speed_td;
@@ -173,7 +174,7 @@ static void PMSM_FOC_UserInit(void) {
 	_gFOC_Ctrl.userLim.s_vDCMinLim = _gFOC_Ctrl.userLim.s_vDCMaxLim / 3;
 	_gFOC_Ctrl.userLim.s_iDCeBrkLim = nv_get_foc_params()->s_iDCeBrkLim;
 	_gFOC_Ctrl.userLim.s_PhaseCurreBrkLim = nv_get_foc_params()->s_PhaseCurreBrkLim;
-	_gFOC_Ctrl.userLim.s_PhaseeVoleBrkLim = _gFOC_Ctrl.hwLim.s_PhaseVolMax - 20;
+	_gFOC_Ctrl.userLim.s_PhaseVoleBrkLim = _gFOC_Ctrl.hwLim.s_PhaseVolMax - 20;
 }
 
 void PMSM_FOC_CoreInit(void) {
@@ -431,7 +432,7 @@ void PMSM_FOC_idqCalc(void) {
 	}else if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
 		float refTorque = eCtrl_get_FinalTorque();
 		_gFOC_Ctrl.pi_ctl_trq->max = refTorque;
-		_gFOC_Ctrl.pi_ctl_trq->min = 0;
+		_gFOC_Ctrl.pi_ctl_trq->min = -_gFOC_Ctrl.userLim.s_PhaseCurreBrkLim;
 		if ((eCtrl_get_FinalTorque() <= 0.0001f) && (_gFOC_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
 			_gFOC_Ctrl.pi_ctl_trq->max = 0;
 			_gFOC_Ctrl.pi_ctl_trq->min = 0; //防止倒转
@@ -460,6 +461,10 @@ void PMSM_FOC_idqCalc(void) {
 	PMSM_FOC_idq_Assign();
 }
 
+PMSM_FOC_Ctrl *PMSM_FOC_Get(void) {
+	return &_gFOC_Ctrl;
+}
+
 void PMSM_FOC_Start(u8 nCtrlMode) {
 	if (_gFOC_Ctrl.in.b_motEnable) {
 		return;
@@ -540,7 +545,7 @@ bool PMSM_FOC_SetCtrlMode(u8 mode) {
 	return true;
 }
 
-void PMSM_FOC_GetCurrentMode(u8 *data) {
+void PMSM_FOC_GetRunningStatus(u8 *data) {
 	data[0] = _gFOC_Ctrl.in.n_ctlMode;
 	data[0] |= _gFOC_Ctrl.out.n_RunMode << 2;
 	data[0] |= (_gFOC_Ctrl.in.b_cruiseEna?1:0) << 4;
@@ -640,11 +645,21 @@ EPM_Dir_t PMSM_FOC_Get_epmDir(void) {
 }
 
 bool PMSM_FOC_Set_Current(float is) {
+	if (is > _gFOC_Ctrl.userLim.s_PhaseCurrLim) {
+		is = _gFOC_Ctrl.userLim.s_PhaseCurrLim;
+	}else if (is < -_gFOC_Ctrl.userLim.s_PhaseCurrLim) {
+		is = -_gFOC_Ctrl.userLim.s_PhaseCurrLim;
+	}
 	eCtrl_set_TgtCurrent(is);
 	return true;
 }
 
 bool PMSM_FOC_Set_Torque(float trq) {
+	if (trq > _gFOC_Ctrl.userLim.s_torqueLim) {
+		trq = _gFOC_Ctrl.userLim.s_torqueLim;
+	}else if (trq < -_gFOC_Ctrl.userLim.s_torqueLim) {
+		trq = -_gFOC_Ctrl.userLim.s_torqueLim;
+	}
 	eCtrl_set_TgtTorque(trq);
 	return true;
 }

Applications/foc/core/PMSM_FOC_Core.h

@@ -60,7 +60,7 @@ typedef struct {
 	float s_PhaseCurrLim; //最大相电流
 	float s_iDCeBrkLim; //最大母线回收电流
 	float s_PhaseCurreBrkLim;
-	float s_PhaseeVoleBrkLim;
+	float s_PhaseVoleBrkLim;
 	float s_vDCMinLim;
 	float s_vDCMaxLim;
 }FOC_UserLimit;
@@ -146,6 +146,8 @@ typedef enum {
 	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_Err_Max = 32,	
 }FOC_CritiCal_Ebit_t;
 
@@ -186,6 +188,7 @@ typedef enum {
 	PID_Max_id
 }PID_id_t;
 
+PMSM_FOC_Ctrl *PMSM_FOC_Get(void);
 void PMSM_FOC_CoreInit(void);
 void PMSM_FOC_Schedule(void);
 u8 PMSM_FOC_CtrlMode(void);
@@ -232,7 +235,7 @@ u32 PMSM_FOC_GetCriticalError(void);
 void PMSM_FOC_PhaseCurrLim(float lim);
 float PMSM_FOC_GetPhaseCurrLim(void);
 float PMSM_FOC_GetiBusLimit(void);
-void PMSM_FOC_GetCurrentMode(u8 *data);
+void PMSM_FOC_GetRunningStatus(u8 *data);
 bool PMSM_FOC_Is_CruiseEnabled(void);
 void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb);
 void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kb);

Applications/foc/motor/motor.c

@@ -14,6 +14,7 @@
 #include "libs/logger.h"
 #include "bsp/sched_timer.h"
 #include "foc/core/e_ctrl.h"
+#include "foc/samples.h"
 #include "foc/motor/motor_param.h"
 #include "foc/core/torque.h"
 #include "app/nv_storage.h"
@@ -40,6 +41,24 @@ static void mc_gpio_init(void) {
 #endif
 }
 
+static void MC_Mos_Check_error(void) {
+	int count = 1000;
+	gpio_phase_u_detect(true);
+	while(count-- >= 0) {
+		task_udelay(20);
+		sample_uvw_phase();
+	}
+	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.01f/* || abc[1] < 0.01f || abc[2] < 0.01f*/){
+		PMSM_FOC_SetCriticalError(FOC_CRIT_L_MOS_Err);
+	}
+	sys_debug("vol %f, %f, %f\n", abc[0], abc[1], abc[2]);
+}
+
 static u32 _self_check_task(void *p) {
 	if (ENC_Check_error()) {
 		err_add_record(FOC_CRIT_Encoder_Err, 0);
@@ -56,6 +75,7 @@ void mc_init(void) {
 	foc_command_init();
 	PMSM_FOC_CoreInit();
 	mc_gpio_init();
+	MC_Mos_Check_error();
 	sched_timer_enable(SPD_CTRL_MS);
 	shark_task_create(_self_check_task, NULL);
 }
@@ -68,6 +88,9 @@ bool mc_start(u8 mode) {
 	if (motor.b_start) {
 		return true;
 	}
+
+	MC_Mos_Check_error();
+
 	if (PMSM_FOC_GetCriticalError() != 0) {
 		PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
 		return false;
@@ -85,11 +108,11 @@ bool mc_start(u8 mode) {
 		return false;
 	}
 	motor.mode = mode;
-	eCtrl_init(300, 3000);
+	eCtrl_init(1000, 3000);
 	motor_encoder_start(motor.s_direction);
 	PMSM_FOC_Start(mode);
 	pwm_turn_on_low_side();
-	delay_ms(500);
+	delay_ms(100);
 	phase_current_offset_calibrate();
 	pwm_start();
 	adc_start_convert();
@@ -334,9 +357,6 @@ void MC_Protect_IRQHandler(void){
 		return;
 	}
 	PMSM_FOC_SetCriticalError(FOC_CRIT_Phase_Err);
-	PMSM_FOC_Stop(); //三相50%占空比输出，防止mos过压击穿
-	pwm_start();
-	adc_start_convert();
 }
 
 measure_time_t g_meas_timeup = {.intval_max_time = 62, .intval_low_err = 0, .intval_hi_err = 0, .first = true,};

Applications/foc/samples.c

@@ -16,7 +16,6 @@ typedef struct {
 
 static void sample_vbus(void);
 static void sample_throttle(void);
-static void sample_uvw_phase(void);
 static u32 sample_task(void *);
 static samples_t _vbus;
 #ifdef THROTTLE_CHAN
@@ -93,7 +92,7 @@ static void sample_throttle(void){
 	LowPass_Filter(_throttle.filted_value, _throttle.value, _throttle.lowpass);
 #endif
 }
-static void sample_uvw_phase(void) {
+void sample_uvw_phase(void) {
 #ifdef U_VOL_ADC_CHAN
 	u16 uvw[3];
 	adc_get_uvw_phaseV(uvw);

Applications/foc/samples.h

@@ -10,6 +10,7 @@ float get_vbus_float(void);
 float get_throttle_sfix5(void);
 float get_throttle_float(void);
 void get_phase_vols(float *uvw);
+void sample_uvw_phase(void);
 
 #endif /* _SAMPLES_H__ */
 

Applications/prot/can_foc_msg.c

@@ -2,6 +2,7 @@
 #include "prot/can_message.h"
 #include "prot/can_foc_msg.h"
 #include "foc/commands.h"
+#include "foc/samples.h"
 #include "foc/core/PMSM_FOC_Core.h"
 
 void can_report_speed(u8 can, s16 rpm) {
@@ -11,8 +12,11 @@ void can_report_speed(u8 can, s16 rpm) {
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }
 
-void can_report_power(u8 can, s16 rpm, float vDC, float iDC) {
+void can_report_power(u8 can) {
 	u8 data[8];
+	s16 rpm = (s16)PMSM_FOC_GetSpeed();
+	float vDC = PMSM_FOC_GetVbusVoltage();
+	float iDC = PMSM_FOC_GetVbusCurrent();	
 	s16 v = (s16)(vDC * 10.0f);
 	s16 i = (s16)(iDC * 10.0f);
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Power));
@@ -23,28 +27,31 @@ void can_report_power(u8 can, s16 rpm, float vDC, float iDC) {
 }
 
 
-void can_report_phase_current(u8 can, float iA, float iB, float iC) {
+void can_report_phase_current(u8 can) {
 	u8 data[14];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Current));
-	encode_float(data + 2, iA);
-	encode_float(data + 6, iB);
-	encode_float(data + 10, iC);
+	encode_float(data + 2, PMSM_FOC_Get()->in.s_iABCFilter[0]);
+	encode_float(data + 6, PMSM_FOC_Get()->in.s_iABCFilter[1]);
+	encode_float(data + 10, PMSM_FOC_Get()->in.s_iABCFilter[2]);
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }
 
-void can_report_phase_voltage(u8 can, float vA, float vB, float vC) {
+void can_report_phase_voltage(u8 can) {
 	u8 data[14];
+	float s_vABC[3];
+	get_phase_vols(s_vABC);
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Phase_Vol));
-	encode_float(data + 2, vA);
-	encode_float(data + 6, vB);
-	encode_float(data + 10, vC);
+	encode_float(data + 2, s_vABC[0]);
+	encode_float(data + 6, s_vABC[1]);
+	encode_float(data + 10, s_vABC[2]);
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
-
 }
 
-void can_report_dq_current(u8 can, float id, float iq) {
+void can_report_dq_current(u8 can) {
 	u8 data[10];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Dq_Current));
+	float id = PMSM_FOC_GetDQCurrent()->d;
+	float iq = PMSM_FOC_GetDQCurrent()->q;
 	encode_float(data + 2, id);
 	encode_float(data + 6, iq);
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
@@ -69,3 +76,12 @@ void can_report_pid_value(u8 can, u8 id) {
 	encode_float(data + 11, kb);
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }
+
+void can_report_foc_status(u8 can) {
+	u8 data[8];
+	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Status));
+	PMSM_FOC_GetRunningStatus(data+2);
+	u32 errMask = PMSM_FOC_GetCriticalError();
+	encode_u32(data + 3, errMask);
+	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
+}

Applications/prot/can_foc_msg.h

@@ -3,12 +3,13 @@
 #include "os/os_types.h"
 
 void can_report_speed(u8 can, s16 rpm);
-void can_report_phase_current(u8 can, float iA, float iB, float iC);
-void can_report_phase_voltage(u8 can, float vA, float vB, float vC);
-void can_report_dq_current(u8 can, float id, float iq);
+void can_report_phase_current(u8 can);
+void can_report_phase_voltage(u8 can);
+void can_report_dq_current(u8 can);
 void can_response_hall_offset(u8 can, int offset);
-void can_report_power(u8 can, s16 rpm, float vDC, float iDC);
+void can_report_power(u8 can);
 void can_report_pid_value(u8 can, u8 id);
+void can_report_foc_status(u8 can);
 
 #endif	/*_Can_Foc_Msg_H__ */