坡起和锁电机功能

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

Applications/app/key_process.c

@@ -40,7 +40,7 @@ static u32 key_task(void *p) {
 		if (value) {
 			PMSM_FOC_SetCtrlMode(CTRL_MODE_SPD);
 			//PMSM_FOC_SpeedLimit(100);
-			//PMSM_FOC_LockMotor(true);
+			//PMSM_FOC_AutoHold(true);
 		}
 		key_value[KEY_STOP] = value;
 	}

Applications/app/nv_storage.c

@@ -35,7 +35,7 @@ static void nv_default_motor_params(void) {
 	m_params.offset = MOTOR_ENC_OFFSET;//180;//(69.0f);
 	//m_params.offset = (360-128);
 	m_params.est_pll_band = 100;
-	m_params.pos_lock_pll_band = 100;
+	m_params.pos_lock_pll_band = 500;
 	m_params.flux_linkage = 0.0f;
 }
 
@@ -77,8 +77,8 @@ static void nv_default_foc_params(void) {
 	foc_params.pid_conf[PID_Pow_id].ki = 4.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].kp = (0.01f);
+	foc_params.pid_conf[PID_Lock_id].ki = (0.15f);
 	foc_params.pid_conf[PID_Lock_id].kb = 0;
 
 }

Applications/foc/commands.c

@@ -178,6 +178,28 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			}
 			break;
 		}
+		case Foc_Lock_Motor:
+		{
+			u8 lock = decode_u8((u8 *)command->data);
+			if (lock == Foc_Start) {
+				mc_lock_motor(true);
+			}else {
+				mc_lock_motor(false);
+			}
+			erroCode = PMSM_FOC_GetErrCode();
+			break;
+		}
+		case Foc_Auto_Hold:
+		{
+			u8 hold = decode_u8((u8 *)command->data);
+			if (hold == Foc_Start) {
+				mc_auto_hold(true);
+			}else {
+				mc_auto_hold(false);
+			}
+			erroCode = PMSM_FOC_GetErrCode();
+			break;
+		}
 		case Foc_Start_EPM_Move:
 		{
 			EPM_Dir_t dir = (EPM_Dir_t)decode_u8((u8 *)command->data);
@@ -241,6 +263,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			break;
 		}
 	}
+	sys_debug("err = %d\n", erroCode);
 	response[0] = command->cmd;
 	response[1] = CAN_MY_ADDRESS;
 	response[2] = erroCode;

Applications/foc/commands.h

@@ -24,6 +24,8 @@ typedef enum {
 	Foc_Set_Plot_Type,
 	Foc_Set_Throttle_throld,
 	Foc_Set_eBrake_Throld,
+	Foc_Lock_Motor,
+	Foc_Auto_Hold,
 	Foc_Hall_Phase_Cali_Result = 160,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,

Applications/foc/core/PMSM_FOC_Core.c

@@ -465,7 +465,9 @@ static __INLINE void PMSM_FOC_idq_Assign(void) {
 
 /*called in media task */
 void PMSM_FOC_idqCalc(void) {
-	if (gFoc_Ctrl.in.b_motLock) {
+	if (gFoc_Ctrl.in.b_AutoHold) {
+		gFoc_Ctrl.pi_lock->max = CONFIG_DEFAULT_LOCK_PHASE_CURR_LIM;
+		gFoc_Ctrl.pi_lock->min = -CONFIG_DEFAULT_LOCK_PHASE_CURR_LIM;
 		float vel_count = motor_encoder_get_vel_count();
 		float errRef = 0 - vel_count;
 		gFoc_Ctrl.in.s_targetTorque = PI_Controller_run(gFoc_Ctrl.pi_lock ,errRef);
@@ -794,17 +796,24 @@ float PMSM_FOC_GetSpeed(void) {
 }
 
 
-void PMSM_FOC_LockMotor(bool lock) {
-	if (gFoc_Ctrl.in.b_motLock != lock) {		
+void PMSM_FOC_AutoHold(bool lock) {
+	if (gFoc_Ctrl.in.b_AutoHold != lock) {
 		motor_encoder_lock_pos(lock);
 		PI_Controller_Reset(gFoc_Ctrl.pi_lock, 0);
-		gFoc_Ctrl.in.b_motLock = lock;
+		if (!lock) {
+			if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
+				PI_Controller_Reset(gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
+			}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD) {
+				PI_Controller_Reset(gFoc_Ctrl.pi_speed, gFoc_Ctrl.in.s_targetTorque);
+			}
+		}
+		gFoc_Ctrl.in.b_AutoHold = lock;
 	}
 }
 
 
-bool PMSM_FOC_MotorLocking(void) {
-	return gFoc_Ctrl.in.b_motLock;
+bool PMSM_FOC_AutoHoldding(void) {
+	return gFoc_Ctrl.in.b_AutoHold;
 }
 
 static PI_Controller *_pid(u8 id) {

Applications/foc/core/PMSM_FOC_Core.h

@@ -90,7 +90,7 @@ typedef struct {
 	u8      n_ctlMode;
 	bool    b_motEnable;
 	bool    b_cruiseEna;
-	bool    b_motLock;
+	bool    b_AutoHold;
 	bool    b_eBrake;
 	bool    b_epmMode;
 	volatile bool    b_MTPA_calibrate;
@@ -244,7 +244,7 @@ float PMSM_FOC_GetSpeed(void);
 bool PMSM_FOC_Lock_Motor(bool lock);
 void PMSM_FOC_Brake(bool brake);
 float PMSM_FOC_Calc_iDC(void);
-void PMSM_FOC_LockMotor(bool lock);
+void PMSM_FOC_AutoHold(bool lock);
 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);
@@ -270,7 +270,7 @@ 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);
-bool PMSM_FOC_MotorLocking(void);
+bool PMSM_FOC_AutoHoldding(void);
 void PMSM_FOC_Slow_Task(void);
 void PMSM_FOC_Set_PlotType(Plot_t t);
 void PMSM_FOC_RunTime_Limit(void);

Applications/foc/foc_config.h

@@ -4,6 +4,7 @@
 #define CONFIG_DEFAULT_IDC_LIM 200
 #define CONFIG_DEFAULT_PHASE_CURR_LIM 200
 #define CONFIG_DEFAULT_RPM_LIM       6000
+#define CONFIG_DEFAULT_LOCK_PHASE_CURR_LIM 100
 
 #define CONFIG_DEFAULT_EPM_PHASE_CURR 50
 #define CONFIG_DEFAULT_EPM_RPM        200

Applications/foc/motor/motor.c

@@ -102,6 +102,10 @@ bool mc_start(u8 mode) {
 #ifdef CONFIG_DQ_STEP_RESPONSE
 	mode = CTRL_MODE_CURRENT;
 #endif
+	if (motor.b_lock_motor) {
+		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		return false;
+	}
 	MC_Check_MosVbusThrottle();
 
 	if (PMSM_FOC_GetCriticalError() != 0) {
@@ -156,6 +160,12 @@ bool mc_stop(void) {
 	if (!motor.b_start) {
 		return true;
 	}
+
+	if (motor.b_lock_motor) {
+		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		return false;
+	}
+
 	if (motor_encoder_get_speed() > 10.0f) {
 		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 		sys_debug("speed error\n");
@@ -292,11 +302,12 @@ void mc_use_throttle(void) {
 
 void mc_get_running_status(u8 *data) {
 	data[0] = motor.mode;
-	data[0] |= PMSM_FOC_Get()->out.n_RunMode << 2;
+	data[0] |= (PMSM_FOC_AutoHoldding()?1:0) << 2;
+	data[0] |= (motor.b_break?1:0) << 3;
 	data[0] |= (PMSM_FOC_Get()->in.b_cruiseEna?1:0) << 4;
 	data[0] |= (motor.b_start?1:0) << 5;
 	data[0] |= (mc_is_epm()?1:0) << 6;
-	data[0] |= (0) << 7; //motor locked
+	data[0] |= (motor.b_lock_motor) << 7; //motor locked
 }
 
 
@@ -397,23 +408,53 @@ bool mc_current_sensor_calibrate(float current) {
 }
 
 bool mc_lock_motor(bool lock) {
+	if (motor.b_lock_motor == lock) {
+		return true;
+	}
+	if (motor.b_start) {
+		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		return false;
+	}
 	if (lock && (PMSM_FOC_GetSpeed() > 10)) {
 		PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 		return false;
 	}
-	PMSM_FOC_LockMotor(lock); //if mot enabled, foc core will do lock
-	if (!motor.b_start) {
-		if (lock) {
-			pwm_start();
-			pwm_update_duty(0, 0, 0);
-		}else {
-			pwm_update_duty(FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2);
-			pwm_stop();
-		}
+	motor.b_lock_motor = lock;
+	if (lock) {
+		pwm_start();
+		pwm_update_duty(FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2, FOC_PWM_Half_Period/2);
+		pwm_enable_channel();
+	}else {
+		pwm_stop();
 	}
 	return true;
 }
 
+bool mc_auto_hold(bool hold) {
+	if (motor.b_auto_hold == hold) {
+		return true;
+	}
+	if (!motor.b_start) {
+		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		return false;
+	}
+	if (hold && !mc_throttle_released()) {
+		PMSM_FOC_SetErrCode(FOC_Throttle_Err);
+		return false;
+	}
+	motor.b_auto_hold = hold;
+
+	u32 mask = cpu_enter_critical();
+	if (!PMSM_FOC_Is_Start()) {
+		PMSM_FOC_Start(motor.mode);
+		PMSM_FOC_AutoHold(hold);
+		pwm_enable_channel();
+	}else {
+		PMSM_FOC_AutoHold(hold);
+	}
+	cpu_exit_critical(mask);
+	return true;
+}
 
 bool mc_throttle_released(void) {
 	return get_throttle_float() <= nv_get_foc_params()->n_minThroVol;
@@ -457,8 +498,10 @@ void MC_Brake_IRQHandler(void) {
 		return;
 	}
 	if (mc_is_hwbrake()) {
+		motor.b_break = true;
 		PMSM_FOC_Brake(true);
 	}else {
+		motor.b_break = false;
 		PMSM_FOC_Brake(false);
 	}
 }
@@ -504,7 +547,7 @@ void ADC_IRQHandler(void) {
 #ifndef CONFIG_DQ_STEP_RESPONSE
 static bool mc_can_stop_foc(void) {
 	if (mc_throttle_released() && PMSM_FOC_GetSpeed() == 0.0f) {
-		if (!PMSM_FOC_MotorLocking() && motor.epm_dir == EPM_Dir_None) {
+		if (!PMSM_FOC_AutoHoldding() && motor.epm_dir == EPM_Dir_None) {
 			return true;
 		}
 	}
@@ -512,7 +555,7 @@ static bool mc_can_stop_foc(void) {
 }
 #endif
 static bool mc_run_stall_process(u8 run_mode) {
-	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !PMSM_FOC_MotorLocking()) {
+	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !PMSM_FOC_AutoHoldding()) {
 		//堵转判断
 		if (motor.b_runStall) {
 			if (!mc_throttle_released()) {
@@ -561,6 +604,9 @@ void Sched_MC_mTask(void) {
 
 	if ((runMode != CTRL_MODE_OPEN) || (motor.mode != CTRL_MODE_OPEN)) {
 #ifndef CONFIG_DQ_STEP_RESPONSE
+		if (PMSM_FOC_AutoHoldding() && !mc_throttle_released()) {
+			mc_auto_hold(false);
+		}
 		if (motor.mode != CTRL_MODE_OPEN) {
 			u32 mask;
 			if (mc_can_stop_foc()) {

Applications/foc/motor/motor.h

@@ -11,6 +11,9 @@ typedef struct {
 	bool   b_ignor_throttle;
 	bool   b_calibrate;
 	bool   b_runStall; //是否堵转
+	bool   b_lock_motor;
+	bool   b_auto_hold;
+	bool   b_break;
 	bool   b_epm;
 	EPM_Dir_t epm_dir;
 	bool   b_epm_cmd_move;
@@ -32,6 +35,7 @@ bool mc_stop(void);
 void mc_encoder_off_calibrate(s16 vd);
 bool mc_throttle_released(void);
 bool mc_lock_motor(bool lock);
+bool mc_auto_hold(bool hold);
 void mc_set_spd_torque(s32 target);
 void mc_use_throttle(void);
 bool mc_current_sensor_calibrate(float current);