加入nv配置PID

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

Applications/app/app.c

@@ -25,12 +25,12 @@ int jtag_data = 0;
 int jtag_plot = 0;
 void fetch_jtag_cmd(void) {
 	foc_cmd_body_t foc_cmd;
-	u8 cmd_data[32];
+
 	if (jtag_cmd == 1 || jtag_cmd == 2) {
 		jtag_plot = 2;
 		foc_cmd.cmd = Foc_Start_Motor;
-		foc_cmd.data = cmd_data;
-		cmd_data[0] = jtag_cmd;
+		foc_cmd.data = (void *)os_alloc(4);
+		encode_u8(foc_cmd.data, jtag_cmd);
 		foc_send_command(&foc_cmd);
 		jtag_cmd = 0;
 	}else if (jtag_cmd == 3) {
@@ -41,8 +41,8 @@ void fetch_jtag_cmd(void) {
 	}else if (jtag_cmd == 4) {
 		jtag_plot = 1;
 		foc_cmd.cmd = Foc_Cali_Hall_Phase;
-		foc_cmd.data = cmd_data;
-		encode_s16(cmd_data, jtag_data);
+		foc_cmd.data = (void *)os_alloc(4);;
+		encode_s16(foc_cmd.data, jtag_data);
 		foc_send_command(&foc_cmd);
 		jtag_cmd = 0;
 	}else if (jtag_cmd == 5) {

Applications/app/nv_storage.c

@@ -49,15 +49,33 @@ static void nv_default_foc_params(void) {
 	foc_params.n_currentBand = 500;
 	foc_params.n_modulation = 1.0f;
 	foc_params.n_PhaseFilterCeof = 0.2f;
-	foc_params.n_TrqVelLimGain = 1.0f;
+	//foc_params.n_TrqVelLimGain = 1.0f;
 	foc_params.s_LimitiDC = 20.0f;
-	foc_params.spd_kp = 0.001f;
-	foc_params.spd_ki = 0.01;
-	foc_params.trq_kp = 0.001f;
-	foc_params.trq_ki = 0.01;
-	foc_params.fw_kp = 0.1f;
-	foc_params.fw_ki = 0.01;
-	foc_params.fw_baseSpd = 5000;
+
+	foc_params.pid_conf[PID_D_id].kp = (CONFIG_CURRENT_BANDWITH * MOTOR_Ld);
+	foc_params.pid_conf[PID_D_id].ki = (MOTOR_R/MOTOR_Ld);
+	foc_params.pid_conf[PID_D_id].kb = 0;
+	
+	foc_params.pid_conf[PID_Q_id].kp = (CONFIG_CURRENT_BANDWITH * MOTOR_Lq);
+	foc_params.pid_conf[PID_Q_id].ki = (MOTOR_R/MOTOR_Lq);
+	foc_params.pid_conf[PID_Q_id].kb = 0;
+
+	foc_params.pid_conf[PID_TRQ_id].kp = 0.1f;
+	foc_params.pid_conf[PID_TRQ_id].ki = 0.5f;
+	foc_params.pid_conf[PID_TRQ_id].kb = 1.2f;
+
+	foc_params.pid_conf[PID_Spd_id].kp = 0.1f;
+	foc_params.pid_conf[PID_Spd_id].ki = 0.5f;
+	foc_params.pid_conf[PID_Spd_id].kb = 1.2f;
+
+	foc_params.pid_conf[PID_Pow_id].kp = 0.5f;
+	foc_params.pid_conf[PID_Pow_id].ki = 100.0f;
+	foc_params.pid_conf[PID_Pow_id].kb = 0;
+
+	foc_params.pid_conf[PID_Lock_id].kp = (0.1f);
+	foc_params.pid_conf[PID_Lock_id].ki = (0.5f);
+	foc_params.pid_conf[PID_Lock_id].kb = 0;
+
 }
 
 void nv_save_motor_params(void) {
@@ -106,6 +124,14 @@ void nv_read_foc_params(void) {
 	}
 }
 
+void nv_set_pid(u8 id, pid_conf_t *pid) {
+	foc_params.pid_conf[id] = *pid;
+}
+
+void nv_get_pid(u8 id, pid_conf_t *pid) {
+	*pid = foc_params.pid_conf[id];
+}
+
 
 void nv_storage_init(void) {
 	nv_read_motor_params();

Applications/app/nv_storage.h

@@ -1,7 +1,8 @@
 #ifndef _NV_Storage_H__
 #define _NV_Storage_H__
 #include "os/os_types.h"
-
+#include "foc/core/PMSM_FOC_Core.h"
+#include "foc/commands.h"
 #pragma  pack (push,1)
 
 typedef struct {
@@ -18,14 +19,8 @@ typedef struct {
 	float n_modulation;
 	float n_PhaseFilterCeof;
 	float n_currentBand; //电流环带宽
-	float n_TrqVelLimGain;
-	float spd_kp;
-	float spd_ki;
-	float trq_kp;
-	float trq_ki;
-	float fw_baseSpd;   //弱磁基速
-	float fw_kp;
-	float fw_ki;
+	u8    n_brkShutPower;
+	pid_conf_t pid_conf[PID_Max_id];
 	u16   crc16;
 }foc_params_t;
 
@@ -68,6 +63,7 @@ void nv_read_motor_params(void);
 void nv_save_foc_params(void);
 void nv_read_foc_params(void);
 void nv_save_hall_table(s32 *hall_table);
-
+void nv_set_pid(u8 id, pid_conf_t *pid);
+void nv_get_pid(u8 id, pid_conf_t *pid);
 #endif /* _NV_Storage_H__ */
 

Applications/bsp/board_mc_v1.h

@@ -32,6 +32,8 @@
 #define CONFIG_MAX_PHASE_VOL    140.0F
 #define CONFIG_MAX_VBUS_VOLTAGE 100.0F
 #define CONFIG_MAX_TORQUE       50.0F
+#define CONFIG_CURRENT_BANDWITH 500 /* 电流环带宽 */
+
 /* MOS驱动 */
 #define pwm_timer TIMER0
 #define PWM_MODE TIMER_OC_MODE_PWM0

Applications/foc/commands.c

@@ -22,7 +22,13 @@ void foc_command_init(void) {
 }
 
 bool foc_send_command(foc_cmd_body_t *command) {
-	return queue_put(_cmd_queue, command);
+	if (!queue_put(_cmd_queue, command)) {
+		if (command->data) {
+			os_free(command->data);
+		}
+		return false;
+	}
+	return true;
 }
 
 static u32 foc_command_task(void *args) {
@@ -36,8 +42,8 @@ static u32 foc_command_task(void *args) {
 	return 0;
 }
 
-static void conf_foc_pid(pid_conf_t *pid) {
-	
+static void conf_foc_pid(u8 id, pid_conf_t *pid) {
+	nv_set_pid(id, pid);
 }
 
 static void process_foc_command(foc_cmd_body_t *command) {
@@ -86,16 +92,18 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		}
 		case Foc_Set_Open_Dq_Vol:
 		{
+			s16 vd = decode_s16(((u8 *)command->data));
 			s16 vq = decode_s16(((u8 *)command->data) + 2);
 			sys_debug("set v_q %d\n", vq);
-			PMSM_FOC_SetOpenVdq((0), (vq));
+			PMSM_FOC_SetOpenVdq(vd, (vq));
 			break;
 		}
 		case Foc_Conf_Pid:
 		{
-			pid_conf_t pid[5];
-			memcpy((char *)pid, (char *)command->data, 5 * sizeof(pid_conf_t));
-			conf_foc_pid(pid);
+			pid_conf_t pid;
+			u8 id =   decode_u8((u8 *)command->data);
+			memcpy((char *)&pid, (char *)command->data + 1, sizeof(pid_conf_t));
+			conf_foc_pid(id, &pid);
 		}
 		case Foc_Start_EPM_Move:
 		{

Applications/foc/commands.h

@@ -55,10 +55,9 @@ typedef struct {
 }hall_offset_cali_cmd_t;
 
 typedef struct {
-	s32 kp;
-	s32 ki;
-	s32 kb;
-	s32 gain;
+	float kp;
+	float ki;
+	float kb;
 }pid_conf_t;
 
 #pragma pack(pop)

Applications/foc/core/PI_Controller.h

@@ -28,8 +28,9 @@ static __INLINE s16q5_t PI_Controller_run(PI_Controller *pi, s16q5_t err) {
 typedef struct {
 	float  kp;
 	float  ki;
-	float  kp_s;
-	float  ki_s;
+	float  kb;
+	//float  kp_s;
+	//float  ki_s;
 	float  max;
 	float  min;
 	float  Ui;
@@ -43,6 +44,7 @@ static __INLINE void PI_Controller_max(PI_Controller *pi, float max, float min)
 }
 static __INLINE void PI_Controller_Reset(PI_Controller *pi, float init) {
 	pi->Ui = (init);
+	pi->sat = 0.0f;
 }
 
 static __INLINE float PI_Controller_run(PI_Controller *pi, float err) {
@@ -54,21 +56,24 @@ static __INLINE float PI_Controller_run(PI_Controller *pi, float err) {
 	return (MATH_sat(out, pi->min, pi->max));
 }
 
-static __INLINE float PI_Controller_RunSat(PI_Controller *pi, float err, float sat_gain) {
+static __INLINE float PI_Controller_RunSat(PI_Controller *pi, float err) {
 	float kp_err = (err) * pi->kp;
 	float ki_err = (err) * pi->ki;
 	float integral = ki_err * pi->DT;
-	pi->Ui = pi->Ui + integral + pi->sat * sat_gain;
+	pi->Ui = pi->Ui + integral + pi->sat * pi->kb;
 	float out = pi->Ui + kp_err ;
 	float out_sat = MATH_sat(out, pi->min, pi->max);
 	pi->sat = out_sat - out;
+	if (pi->sat >= 0.0f) {
+		pi->sat = 0.0f;
+	}
 	return out_sat;
 }
 
 
 static __INLINE float PI_Controller_RunSerial(PI_Controller *pi, float err) {
-	float kp_err = (err) * pi->kp_s;//S16_mul(err,pi->kp, 5);
-	float ki_err = (kp_err) * pi->ki_s;
+	float kp_err = (err) * pi->kp;//S16_mul(err,pi->kp, 5);
+	float ki_err = (kp_err) * pi->ki;
 	float integral = ki_err * pi->DT;
 	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
 	float out = pi->Ui + kp_err;

Applications/foc/core/PMSM_FOC_Core.c

@@ -125,7 +125,38 @@ static void PMSM_FOC_Reset_PID(void) {
 	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_power, 0);
 }
 
+static void PMSM_FOC_Conf_PID(void) {
+	_gFOC_Ctrl.pi_ctl_id->kp = nv_get_foc_params()->pid_conf[PID_D_id].kp;
+	_gFOC_Ctrl.pi_ctl_id->ki = nv_get_foc_params()->pid_conf[PID_D_id].ki;
+	_gFOC_Ctrl.pi_ctl_id->kb = nv_get_foc_params()->pid_conf[PID_D_id].kb;
+	_gFOC_Ctrl.pi_ctl_id->DT = (1.0f/(float)IDQ_CTRL_TS);
+
+	_gFOC_Ctrl.pi_ctl_iq->kp = nv_get_foc_params()->pid_conf[PID_Q_id].kp;
+	_gFOC_Ctrl.pi_ctl_iq->ki = nv_get_foc_params()->pid_conf[PID_Q_id].ki;
+	_gFOC_Ctrl.pi_ctl_iq->kb = nv_get_foc_params()->pid_conf[PID_Q_id].kb;
+	_gFOC_Ctrl.pi_ctl_iq->DT = (1.0f/(float)IDQ_CTRL_TS);
+
+	_gFOC_Ctrl.pi_ctl_trq->kp = nv_get_foc_params()->pid_conf[PID_TRQ_id].kp;
+	_gFOC_Ctrl.pi_ctl_trq->ki = nv_get_foc_params()->pid_conf[PID_TRQ_id].ki;
+	_gFOC_Ctrl.pi_ctl_trq->kb = nv_get_foc_params()->pid_conf[PID_TRQ_id].kb;
+	_gFOC_Ctrl.pi_ctl_trq->DT = (1.0f/(float)SPD_CTRL_TS);
+
+	_gFOC_Ctrl.pi_ctl_spd->kp = nv_get_foc_params()->pid_conf[PID_Spd_id].kp;
+	_gFOC_Ctrl.pi_ctl_spd->ki = nv_get_foc_params()->pid_conf[PID_Spd_id].ki;
+	_gFOC_Ctrl.pi_ctl_spd->kb = nv_get_foc_params()->pid_conf[PID_Spd_id].kb;
+	_gFOC_Ctrl.pi_ctl_spd->DT = (1.0f/(float)SPD_CTRL_TS);
+
+	_gFOC_Ctrl.pi_ctl_power->kp = nv_get_foc_params()->pid_conf[PID_Pow_id].kp;
+	_gFOC_Ctrl.pi_ctl_power->ki = nv_get_foc_params()->pid_conf[PID_Pow_id].ki;
+	_gFOC_Ctrl.pi_ctl_power->kb = nv_get_foc_params()->pid_conf[PID_Pow_id].kb;
+	_gFOC_Ctrl.pi_ctl_power->DT = (1.0f/(float)SPD_CTRL_TS);
+
+	_gFOC_Ctrl.pi_ctl_lock->kp = nv_get_foc_params()->pid_conf[PID_Lock_id].kp;
+	_gFOC_Ctrl.pi_ctl_lock->ki = nv_get_foc_params()->pid_conf[PID_Lock_id].ki;
+	_gFOC_Ctrl.pi_ctl_lock->kb = nv_get_foc_params()->pid_conf[PID_Lock_id].kb;
+	_gFOC_Ctrl.pi_ctl_lock->DT = (1.0f/(float)SPD_CTRL_TS);
 
+}
 void PMSM_FOC_CoreInit(void) {
 	Fir_init(&phase1);
 	Fir_init(&phase2);
@@ -136,6 +167,9 @@ void PMSM_FOC_CoreInit(void) {
 	_gFOC_Ctrl.pi_ctl_trq = &PI_Ctrl_trq;
 	_gFOC_Ctrl.pi_ctl_lock = &PI_Ctrl_lock;
 	_gFOC_Ctrl.pi_ctl_power = &PI_Ctrl_Power;
+
+	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.userLim, 0, sizeof(_gFOC_Ctrl.userLim));
@@ -157,7 +191,7 @@ void PMSM_FOC_CoreInit(void) {
 	
 	_gFOC_Ctrl.params.n_modulation = nv_get_foc_params()->n_modulation;//SVM_Modulation;
 	_gFOC_Ctrl.params.n_PhaseFilterCeof = nv_get_foc_params()->n_PhaseFilterCeof;//(0.2f);
-	_gFOC_Ctrl.params.n_TrqVelLimGain = nv_get_foc_params()->n_TrqVelLimGain;
+	//_gFOC_Ctrl.params.n_TrqVelLimGain = nv_get_foc_params()->n_TrqVelLimGain;
 	_gFOC_Ctrl.params.n_poles = nv_get_motor_params()->poles;//MOTOR_POLES;
 	_gFOC_Ctrl.in.s_manualAngle = INVALID_ANGLE;
 	_gFOC_Ctrl.in.s_vDC = nv_get_foc_params()->s_maxvDC;//(MAX_vDC);
@@ -402,7 +436,7 @@ void PMSM_FOC_idqCalc(void) {
 			_gFOC_Ctrl.pi_ctl_trq->min = 0; //防止倒转
 		}
 		float errRef = _gFOC_Ctrl.userLim.s_motRPMLim - _gFOC_Ctrl.in.s_motRPM;
-		float maxTrq = PI_Controller_RunSat(_gFOC_Ctrl.pi_ctl_trq, errRef, _gFOC_Ctrl.params.n_TrqVelLimGain);
+		float maxTrq = PI_Controller_RunSat(_gFOC_Ctrl.pi_ctl_trq, errRef);
 		_gFOC_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_Power(maxTrq);
 	}else if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_SPD){
 		_gFOC_Ctrl.pi_ctl_spd->max = _gFOC_Ctrl.userLim.s_PhaseCurrLim;
@@ -416,7 +450,7 @@ void PMSM_FOC_idqCalc(void) {
 			_gFOC_Ctrl.pi_ctl_spd->min = 0; //防止倒转
 		}
 		float errRef = refSpeed - _gFOC_Ctrl.in.s_motRPM;
-		float maxTrq = PI_Controller_RunSat(_gFOC_Ctrl.pi_ctl_spd, errRef, _gFOC_Ctrl.params.n_TrqVelLimGain);
+		float maxTrq = PI_Controller_RunSat(_gFOC_Ctrl.pi_ctl_spd, errRef);
 		_gFOC_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_Power(maxTrq);
 	}
 	PMSM_FOC_idq_Assign();
@@ -615,25 +649,10 @@ void PMSM_FOC_LockMotor(bool lock) {
 	}
 }
 
-void PMSM_FOC_SetSpdPid(float kp, float ki, float max, float min) {
-
-}
-
-void PMSM_FOC_SetIDPid(float kp, float ki, float max, float min) {
+void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb) {
 
 }
 
-void PMSM_FOC_SetIQPid(float kp, float ki, float max, float min) {
-
-}
-
-void PMSM_FOC_SetTrqPid(float kp, float ki, float max, float min) {
-
-}
-
-void PMSM_FOC_SetFW_I(float kp, float ki, float max, float min) {
-
-}
 
 void PMSM_FOC_SetErrCode(u8 error) {
 	if (_gFOC_Ctrl.out.n_Error != error) {
@@ -645,6 +664,17 @@ u8 PMSM_FOC_GetErrCode(void) {
 	return _gFOC_Ctrl.out.n_Error;
 }
 
+void PMSM_FOC_SetCriticalError(u8 err) {
+	_gFOC_Ctrl.out.n_CritiCalErrMask |= (1u << err);
+}
+
+void PMSM_FOC_ClrCriticalError(u8 err) {
+	_gFOC_Ctrl.out.n_CritiCalErrMask &= ~(1u << err);
+}
+
+u32 PMSM_FOC_GetCriticalError(void) {
+	return _gFOC_Ctrl.out.n_CritiCalErrMask;
+}
 //获取母线电流
 float PMSM_FOC_Get_iDC(void) {
 	float vd = _gFOC_Ctrl.out.s_OutVdq.d;

Applications/foc/core/PMSM_FOC_Core.h

@@ -23,7 +23,7 @@ typedef struct {
 	u8 	  n_poles;
 	float n_modulation;
 	float n_PhaseFilterCeof;
-	float n_TrqVelLimGain;
+	//float n_TrqVelLimGain;
 	DQ_t  maxvDQ;
 	DQ_t  minvDQ;
 }FOC_Params;
@@ -95,6 +95,7 @@ typedef struct {
 	float sin;
 	float cos;
 	u8    n_Error;
+	u32   n_CritiCalErrMask;
 }FOC_OutP;
 
 typedef struct {
@@ -126,23 +127,26 @@ typedef struct {
 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_Critical_Err_Start = 150,
-	FOC_OV_Vol_Err = FOC_Critical_Err_Start,
-	FOC_UN_Vol_Err,
-	FOC_DC_Curr_OV_Err,
-	FOC_Phase_Curr_OV_Err,
-	FOC_Phase_Err,
-	FOC_Encoder_Err,
-	FOC_Brake_Err,
-	FOC_Err_Max = 0xFF,
-}foc_fault_t;
+}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_Err_Max = 32,	
+}FOC_CritiCal_Ebit_t;
 
 #define CTRL_MODE_OPEN                      ((u8)0U)
 #define CTRL_MODE_SPD                       ((u8)1U)
@@ -170,6 +174,16 @@ typedef enum {
 #define SECTOR_6  2u
 #endif
 
+typedef enum {
+	PID_Q_id,
+	PID_D_id,
+	PID_Spd_id,
+	PID_TRQ_id,
+	PID_Pow_id,
+	PID_Lock_id,
+	PID_FW_id,
+	PID_Max_id
+}PID_id_t;
 
 void PMSM_FOC_CoreInit(void);
 void PMSM_FOC_Schedule(void);
@@ -194,11 +208,7 @@ bool PMSM_FOC_Lock_Motor(bool lock);
 void PMSM_FOC_Brake(bool brake);
 float PMSM_FOC_Get_iDC(void);
 void PMSM_FOC_LockMotor(bool lock);
-void PMSM_FOC_SetSpdPid(float kp, float ki, float max, float min);
-void PMSM_FOC_SetIDPid(float kp, float ki, float max, float min);
-void PMSM_FOC_SetIQPid(float kp, float ki, float max, float min);
-void PMSM_FOC_SetTrqPid(float kp, float ki, float max, float min);
-void PMSM_FOC_SetFW_I(float kp, float ki, float max, float min);
+void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb);
 void PMSM_FOC_Set_Angle(float angle);
 bool PMSM_FOC_Is_Start(void);
 void PMSM_FOC_SetErrCode(u8 error);
@@ -213,6 +223,9 @@ bool PMSM_FOC_Start_epmMove(bool move, EPM_Dir_t dir);
 EPM_Dir_t PMSM_FOC_Get_epmDir(void);
 void PMSM_FOC_SeteBrkPhaseCurrent(float curr);
 float PMSM_FOC_GeteBrkPhaseCurrent(void);
+void PMSM_FOC_SetCriticalError(u8 err) ;
+void PMSM_FOC_ClrCriticalError(u8 err);
+u32 PMSM_FOC_GetCriticalError(void);
 
 #endif /* _PMSM_FOC_Core_H__ */
 

Applications/foc/core/PMSM_FOC_Params.h

@@ -3,10 +3,10 @@
 #include "foc/motor/motor_param.h"
 
 static PI_Controller PI_Ctrl_ID = {
-	.kp = (CURRENT_BANDWITH * MOTOR_Ld),
-	.ki = (CURRENT_BANDWITH * MOTOR_R),
-	.kp_s = (CURRENT_BANDWITH * MOTOR_Ld),
-	.ki_s = (MOTOR_R/MOTOR_Ld),
+	//.kp = (CURRENT_BANDWITH * MOTOR_Ld),
+	//.ki = (CURRENT_BANDWITH * MOTOR_R),
+	//.kp = (CURRENT_BANDWITH * MOTOR_Ld),
+	//.ki = (MOTOR_R/MOTOR_Ld),
 	.max = (MAX_vDC),
 	.min = (-MAX_vDC),
 	.DT  = (1.0f/(float)IDQ_CTRL_TS),
@@ -14,10 +14,10 @@ static PI_Controller PI_Ctrl_ID = {
 };
 
 static PI_Controller PI_Ctrl_IQ = {
-	.kp = (CURRENT_BANDWITH * MOTOR_Lq),
-	.ki = (CURRENT_BANDWITH * MOTOR_R),
-	.kp_s = (CURRENT_BANDWITH * MOTOR_Lq),
-	.ki_s = (MOTOR_R/MOTOR_Lq),
+	//.kp = (CURRENT_BANDWITH * MOTOR_Lq),
+	//.ki = (CURRENT_BANDWITH * MOTOR_R),
+	//.kp = (CURRENT_BANDWITH * MOTOR_Lq),
+	//.ki = (MOTOR_R/MOTOR_Lq),
 	.max = (MAX_vDC),
 	.min = (-MAX_vDC),
 	.DT  = (1.0f/(float)IDQ_CTRL_TS),
@@ -25,8 +25,9 @@ static PI_Controller PI_Ctrl_IQ = {
 };
 
 static PI_Controller PI_Ctrl_trq = {
-	.kp = 0.1f,
-	.ki = 0.5f,
+	//.kp = 0.1f,
+	//.ki = 0.5f,
+	//.kb = 1.2f,
 	.max = (MAX_TORQUE),
 	.min = (-MAX_TORQUE),
 	.DT  = (1.0f/(float)SPD_CTRL_TS),
@@ -34,39 +35,41 @@ static PI_Controller PI_Ctrl_trq = {
 };
 
 static PI_Controller PI_Ctrl_Spd = {
-	.kp = 0.1f,
-	.ki = 0.5f,
+	//.kp = 0.1f,
+	//.ki = 0.5f,
+	//.kb = 1.2f,
 	.max = (MAX_TORQUE),
 	.min = (-MAX_TORQUE),
 	.DT  = (1.0f/(float)SPD_CTRL_TS),
 	.Ui = 0,
 };
 
-static PI_Controller PI_Ctrl_fw = {
-	.kp = (0.001f),
-	.ki = (0.003f),
-	.max = (20),
-	.min = (0),
-	.DT  = (1.0f/(float)SPD_CTRL_TS),
-	.Ui = 0,
-};
 
 static PI_Controller PI_Ctrl_Power = {
-	.kp = (0.5f),
-	.ki = (100.0f),
+	//.kp = (0.5f),
+	//.ki = (100.0f),
 	.max = (0),
 	.min = 0,
 	.DT  = (1.0f/(float)SPD_CTRL_TS),
 	.Ui = 0,
 };
 
-
 static PI_Controller PI_Ctrl_lock = {
-	.kp = (0.0001f),
-	.ki = (0.01f),
+	//.kp = (0.0001f),
+	//.ki = (0.01f),
 	.max = (MAX_TORQUE),
 	.min = (-MAX_TORQUE),
 	.DT  = (1.0f/(float)SPD_CTRL_TS),
 	.Ui = 0,
 };
 
+static PI_Controller PI_Ctrl_fw = {
+	//.kp = (0.001f),
+	//.ki = (0.003f),
+	.max = (20),
+	.min = (0),
+	.DT  = (1.0f/(float)SPD_CTRL_TS),
+	.Ui = 0,
+};
+
+

Applications/foc/foc_config.h

@@ -39,7 +39,6 @@
 #define eCTRL_NEG_TORQUE (-100)   /* ebrake 的最大方向DQ电流，单位 ACKED_KEY*/
 
 
-#define CURRENT_BANDWITH 500 /* 电流环带宽 */
 #define CURRENT_LOOP_RAMP_COUNT 300
 
 #define SVM_Modulation 1.0f //(0.96f)

Applications/foc/mc_error.c

@@ -1,10 +1,10 @@
 #include "foc/mc_error.h"
 #include "foc/core/PMSM_FOC_Core.h"
 
-static err_record_t err[FOC_Err_Max];
+static err_record_t err[FOC_CRIT_Err_Max];
 
 void err_add_record(u8 err_code, s16 err_value) {
-	if (err_code >= FOC_Err_Max) {
+	if (err_code >= FOC_CRIT_Err_Max) {
 		return;
 	}
 	err[err_code].err_value = err_value;
@@ -12,7 +12,7 @@ void err_add_record(u8 err_code, s16 err_value) {
 }
 
 int err_get_record(u8 index, int size, err_record_t *err_out) {
-	size = min(size, (FOC_Err_Max -  index));
+	size = min(size, (FOC_CRIT_Err_Max -  index));
 	for (int i = index; i < index + size; i++) {
 		err_out[i].err_value = err[i].err_value;
 		err_out[i].err_time = err[i].err_time;

Applications/foc/motor/current.c

@@ -3,6 +3,7 @@
 #include "bsp/pwm.h"
 #include "foc/motor/current.h"
 #include "foc/core/PMSM_FOC_Core.h"
+#include "foc/mc_error.h"
 #include "libs/utils.h"
 #include "libs/logger.h"
 #include "math/fast_math.h"
@@ -155,6 +156,18 @@ bool phase_current_offset(void) {
 }
 
 
+bool phase_curr_offset_check(void) {
+	if ((g_cs.adc_offset_b > ADC_FULL_MAX/2 + 100) || (g_cs.adc_offset_c > ADC_FULL_MAX/2 + 100)) {
+		err_add_record(FOC_CRIT_CURR_OFF_Err, MAX(g_cs.adc_offset_c, g_cs.adc_offset_b));
+		return true;
+	}
+	if ((g_cs.adc_offset_b < ADC_FULL_MAX/2 - 100) || (g_cs.adc_offset_c < ADC_FULL_MAX/2 - 100)) {
+		err_add_record(FOC_CRIT_CURR_OFF_Err, min(g_cs.adc_offset_c, g_cs.adc_offset_b));
+		return true;
+	}
+	return false;
+}
+
 #if 0
 void phase_current_get(float *iABC){
 	current_samp_t *cs = &g_cs;

Applications/foc/motor/current.h

@@ -56,6 +56,7 @@ void phase_current_offset_calibrate(void);
 void phase_current_calibrate_wait(void);
 void phase_current_sensor_start_calibrate(float calibrate_current);
 bool phase_current_sensor_do_calibrate(void);
+bool phase_curr_offset_check(void);
 
 #endif /* _PHASE_CURRENT_H__ */
 

Applications/foc/motor/encoder.c

@@ -286,6 +286,7 @@ void ENC_TIMER_Overflow(void) {
 
 /*PWM 信号捕获一个周期的处理 */
 static int pwm_count = 0;
+static int pwm_check_count = 0;
 void ENC_PWM_Duty_Handler(float t, float d) {
 	float duty = ENC_Duty(d, t);
 	if (duty < ENC_PWM_Min_P || duty > 1.0f) {
@@ -306,8 +307,20 @@ void ENC_PWM_Duty_Handler(float t, float d) {
 	if (!g_encoder.b_index_found && pwm_count++ >= 10) {
 		encoder_sync_pwm_abs();
 	}
+	pwm_check_count ++;
+}
+static u32 _check_time = 0;
+bool ENC_Check_error(void) {
+	bool error = false;
+	if (get_delta_ms(_check_time) > 1000) {
+		if (pwm_check_count == 0) {
+			error = true;
+		}
+		pwm_check_count = 0;
+		_check_time = get_tick_ms();
+	}
+	return error;
 }
-
 
 float encoder_get_pwm_angle(void) {
 	return g_encoder.pwm_angle;

Applications/foc/motor/encoder.h

@@ -25,7 +25,7 @@ typedef struct {
 	float est_angle_counts;
 	float rpm;
 	u8   *encoder_off_count;
-	s16  *encoder_off_map;	
+	s16  *encoder_off_map;
 }encoder_t;
 
 void encoder_init(void);
@@ -39,5 +39,7 @@ void encoder_lock_position(bool enable);
 bool encoder_detect_finish(void);
 float encoder_get_pwm_angle(void);
 float encoder_zero_phase_detect(void);
+bool ENC_Check_error(void);
+
 #endif /* _Encoder_H__ */
 

Applications/foc/motor/motor.c

@@ -35,6 +35,14 @@ static void mc_gpio_init(void) {
 #endif
 }
 
+static u32 _self_check_task(void *p) {
+	if (ENC_Check_error()) {
+		err_add_record(FOC_CRIT_Encoder_Err, 0);
+		PMSM_FOC_SetCriticalError(FOC_CRIT_Encoder_Err);
+	}
+	return 0;
+}
+
 void mc_init(void) {
 	adc_init();
 	pwm_3phase_init();
@@ -44,6 +52,7 @@ void mc_init(void) {
 	PMSM_FOC_CoreInit();
 	mc_gpio_init();
 	sched_timer_enable(SPD_CTRL_MS);
+	shark_task_create(_self_check_task, NULL);
 }
 
 motor_t * mc_params(void) {
@@ -54,18 +63,20 @@ bool mc_start(u8 mode) {
 	if (motor.b_start) {
 		return true;
 	}
+	if (PMSM_FOC_GetCriticalError() != 0) {
+		PMSM_FOC_SetErrCode(FOC_Have_CritiCal_Err);
+		return false;
+	}
 	if (mode > CTRL_MODE_CURRENT) {
 		PMSM_FOC_SetErrCode(FOC_Param_Err);
 		return false;
 	}
 	if (PMSM_FOC_GetSpeed() > 10.0f) {
 		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
-		err_add_record(FOC_NowAllowed_With_Speed, (s16)PMSM_FOC_GetSpeed());
 		return false;
 	}
 	if (!mc_throttle_released()) {
 		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
-		err_add_record(FOC_Throttle_Err, get_throttle_float() * 10);
 		return false;
 	}
 	motor.mode = mode;
@@ -80,7 +91,11 @@ bool mc_start(u8 mode) {
 	phase_current_calibrate_wait();
 	motor.throttle = 0;
 	motor.b_start = true;
-	gpio_led2_enable(true);
+	if (phase_curr_offset_check()) {
+		PMSM_FOC_SetCriticalError(FOC_CRIT_CURR_OFF_Err);
+		mc_stop();
+		return false;
+	}
 	return true;
 }
 
@@ -90,12 +105,10 @@ bool mc_stop(void) {
 	}
 	if (PMSM_FOC_GetSpeed() > 10.0f) {
 		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
-		err_add_record(FOC_NowAllowed_With_Speed, (s16)PMSM_FOC_GetSpeed());
 		return false;
 	}
 	if (!mc_throttle_released()) {
 		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
-		err_add_record(FOC_Throttle_Err, get_throttle_float() * 10);
 		return false;
 	}
 	motor.mode = CTRL_MODE_OPEN;
@@ -255,7 +268,6 @@ void MC_Protect_IRQHandler(void){
 	PMSM_FOC_Stop(); //三相50%占空比输出，防止mos过压击穿
 	pwm_start();
 	adc_start_convert();
-	PMSM_FOC_SetErrCode(FOC_Phase_Curr_OV_Err);
 }
 
 measure_time_t g_meas_timeup = {.intval_max_time = 62, .intval_low_err = 0, .intval_hi_err = 0, .first = true,};

Applications/os/os_task.c

@@ -6,6 +6,7 @@
 extern uint32_t get_system_sleep_time(void);
 
 static u64 shark_mseconds;
+static u32 _g_ticks;
 static u32 shark_timer_task_handler(void *);
 
 static shark_timer_t shark_timer_head = {
@@ -23,6 +24,7 @@ static shark_task_t shark_task_head = {
 void SysTick_Handler(void)
 {
 	shark_mseconds++;
+	_g_ticks ++;
 }
 
 void os_disable_irq(void) {
@@ -43,6 +45,20 @@ u32 shark_get_seconds(void){
 	return shark_mseconds/1000;
 }
 
+u32 get_tick_ms(void) {
+	return _g_ticks;
+}
+
+u32 get_delta_ms(uint32_t prev_ms){
+	uint32_t current = get_tick_ms();
+	if (current >= prev_ms){
+		return current - prev_ms;
+	}
+	//32λwrapper�����current < prev_ms
+	return 0xFFFFFFFF - prev_ms + current;
+}
+
+
 static inline void shark_timer_sync(void)
 {
 	shark_task_head.time = shark_timer_head.next->time;

Applications/os/os_task.h

@@ -33,7 +33,8 @@ static inline bool shark_timer_stopped(shark_timer_t *timer)
 
 void os_enable_irq(void);
 void os_disable_irq(void);
-
+u32 get_tick_ms(void);
+u32 get_delta_ms(uint32_t prev_ms);
 u64 shark_get_mseconds(void);
 u32 shark_get_seconds(void);
 void shark_timer_post(shark_timer_t *timer, u32 delay);

Project/MC100.uvoptx

@@ -120,6 +120,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
+          <Name>?</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -211,7 +212,7 @@
       <DebugFlag>
         <trace>0</trace>
         <periodic>1</periodic>
-        <aLwin>0</aLwin>
+        <aLwin>1</aLwin>
         <aCover>0</aCover>
         <aSer1>0</aSer1>
         <aSer2>0</aSer2>