1. 长捏3s刹车进入坡起模式

2. 协议完善

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

Applications/app/nv_storage.c

@@ -33,7 +33,6 @@ static void nv_default_motor_params(void) {
 	m_params.ld = MOTOR_Ld;
 	m_params.lq = MOTOR_Lq;
 	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 = 500;
 	m_params.flux_linkage = 0.0f;
@@ -49,13 +48,12 @@ static void nv_default_foc_params(void) {
 	foc_params.s_maxTorque = foc_params.s_PhaseCurrLim;
 	foc_params.s_PhaseCurreBrkLim = CONFIG_DEFAULT_EBRK_PHASE_CURR;
 	foc_params.n_currentBand = CONFIG_CURRENT_BANDWITH;
-	foc_params.n_modulation = CONFIG_SVM_MODULATION;
-	foc_params.n_PhaseFilterCeof = CONFIG_CURR_LP_PARAM;
 	foc_params.n_brkShutPower = CONFIG_BRK_SHUT_POWER_ENABLE;
 	foc_params.s_LimitiDC = CONFIG_DEFAULT_IDC_LIM;
 	foc_params.s_iDCeBrkLim = CONFIG_DEFAULT_EBRK_IDC_LIM;
 	foc_params.n_minThroVol = CONFIG_THROTTLE_LOW_VALUE;
 	foc_params.n_maxThroVol = CONFIG_THROTTLE_MAX_VALUE;
+	foc_params.n_autoHold = CONFIG_AUTOHOLD_ENABLE;
 	
 	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);
@@ -78,7 +76,7 @@ static void nv_default_foc_params(void) {
 	foc_params.pid_conf[PID_Pow_id].kb = 0;
 
 	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].ki = (0.20f);
 	foc_params.pid_conf[PID_Lock_id].kb = 0;
 
 }
@@ -141,6 +139,7 @@ void nv_read_foc_params(void) {
 			fmc_write_data(focParam_idx_1, (u8 *)&foc_params, sizeof(foc_params));
 		}
 	}
+	foc_params.s_maxTorque = foc_params.s_PhaseCurrLim;//overwrite
 }
 
 void nv_set_pid(u8 id, pid_conf_t *pid) {

Applications/app/nv_storage.h

@@ -16,12 +16,11 @@ typedef struct {
 	float s_PhaseCurreBrkLim;
 	float s_iDCeBrkLim;
 	float s_LimitiDC;
-	float n_modulation;
-	float n_PhaseFilterCeof;
 	float n_currentBand; //电流环带宽
 	float n_minThroVol;
 	float n_maxThroVol;
 	u8    n_brkShutPower;
+	u8    n_autoHold;
 	pid_conf_t pid_conf[PID_Max_id];
 	u16   crc16;
 }foc_params_t;

Applications/bsp/board_mc_v1.h

@@ -173,6 +173,7 @@
 #define GPIO_BRAKE_EXTI EXTI_3
 #define GPIO_BRAKE_EXIT_SRC_GROUP GPIO_PORT_SOURCE_GPIOB
 #define GPIO_BRAKE_EXIT_SRC_PIN GPIO_PIN_SOURCE_3
+
 #define GPIO_BREAK_MODE GPIO_LOW_BRK_MODE      
 
 

Applications/bsp/gpio.c

@@ -42,7 +42,7 @@ void gpio_pin_init(void){
 	}
 #ifdef CONFIG_BEEP
 	gpio_init(GPIOB, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_2);
-	gpio_beep(500);
+	gpio_beep(50);
 #endif
 }
 
@@ -55,6 +55,19 @@ void gpio_beep(u32 ms) {
 #endif
 }
 
+void mc_brk_gpio_init(void) {
+#ifdef GPIO_BRAKE_IN_GROUP
+	rcu_periph_clock_enable(GPIO_BRAKE_IN_RCU);
+	gpio_init(GPIO_BRAKE_IN_GROUP, GPIO_BRAKE_IN_MODE, GPIO_OSPEED_50MHZ, GPIO_BRAKE_IN_PIN);
+	
+	gpio_exti_source_select(GPIO_BRAKE_EXIT_SRC_GROUP, GPIO_BRAKE_EXIT_SRC_PIN);
+	exti_init(GPIO_BRAKE_EXTI, EXTI_INTERRUPT, EXTI_TRIG_BOTH);
+	nvic_irq_enable(GPIO_BRAKE_IRQ, EBREAK_IRQ_PRIORITY, 0U);
+	exti_interrupt_flag_clear(GPIO_BRAKE_EXTI);
+	exti_interrupt_enable(GPIO_BRAKE_EXTI);	
+#endif
+}
+
 void gpio_phase_u_detect(bool enable) {
 	if (enable) {
 		gpio_init(GPIOA, GPIO_MODE_OUT_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_14);
@@ -66,8 +79,8 @@ void gpio_phase_u_detect(bool enable) {
 	}
 }
 
-bool gpio_get_brake(void) {
-	return 0;
+bool mc_get_gpio_brake(void) {
+	return gpio_input_bit_get(GPIO_BRAKE_IN_GROUP, GPIO_BRAKE_IN_PIN) == SET;
 }
 
 void gpio_ir2136_enable(bool enable) {

Applications/bsp/gpio.h

@@ -24,5 +24,7 @@ int gpio_stopkey_value(void);
 int gpio_funckey_value(void);
 void gpio_beep(u32 ms);
 void gpio_phase_u_detect(bool enable);
+void mc_brk_gpio_init(void);
+bool mc_get_gpio_brake(void);
 
 #endif /* _GPIO_PIN_H__ */

Applications/foc/commands.c

@@ -246,16 +246,56 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		}
 		case Foc_Set_Throttle_throld:
 		{
-			u16 min = decode_u16((u8 *)command->data);
-			u16 max = decode_u16((u8 *)command->data + 2);
-			nv_get_foc_params()->n_minThroVol = (float)min/100.0f;
-			nv_get_foc_params()->n_maxThroVol = (float)max/100.0f;
-			nv_save_foc_params();
+			if (mc_is_start()) {
+				erroCode = FOC_NotAllowed;
+			}else {
+				u16 min = decode_u16((u8 *)command->data);
+				u16 max = decode_u16((u8 *)command->data + 2);
+				nv_get_foc_params()->n_minThroVol = (float)min/100.0f;
+				nv_get_foc_params()->n_maxThroVol = (float)max/100.0f;
+				nv_save_foc_params();
+			}
+			break;
+		}
+		case Foc_Get_Config:
+		{
+			len = sizeof(foc_params_t) + 2 - sizeof(pid_conf_t) * PID_Max_id - 2;
+			u8 *config = os_alloc(len);
+			if (config == NULL) {
+				erroCode = FOC_MEM_Err;
+				break;
+			}
+			memcpy((void *)(config + 2), (void *)nv_get_foc_params(), sizeof(foc_params_t) - sizeof(pid_conf_t) * PID_Max_id - 2);
+			config[0] = command->cmd;
+			config[1] = CAN_MY_ADDRESS;
+			can_send_response(command->can_src, config, len);
+			os_free(config);
+			return;
+		}
+		case Foc_Set_Config:
+		{
+			if (mc_is_start()) {
+				erroCode = FOC_NotAllowed;
+			}else if (command->len < 19) {
+				erroCode = FOC_Param_Err;
+			}else {
+				nv_get_foc_params()->s_PhaseCurrLim = decode_s16((u8 *)command->data);
+				nv_get_foc_params()->s_maxRPM = decode_s16((u8 *)command->data + 2);
+				nv_get_foc_params()->s_PhaseCurreBrkLim = decode_s16((u8 *)command->data + 4);
+				nv_get_foc_params()->s_iDCeBrkLim = decode_s16((u8 *)command->data + 6);
+				nv_get_foc_params()->s_LimitiDC = decode_s16((u8 *)command->data + 8);
+				nv_get_foc_params()->n_minThroVol = (float)decode_s16((u8 *)command->data + 10)/100.0f;
+				nv_get_foc_params()->n_maxThroVol = (float)decode_s16((u8 *)command->data + 12)/100.0f;
+				nv_get_foc_params()->s_maxEpmRPM = decode_s16((u8 *)command->data + 14);
+				nv_get_foc_params()->s_maxEpmPhaseCurrLim = decode_s16((u8 *)command->data + 16);
+				nv_get_foc_params()->n_brkShutPower = decode_s16((u8 *)command->data + 18);
+				nv_save_foc_params();
+			}
 			break;
 		}
 		case Foc_Set_eBrake_Throld:
 		{
-			
+			break;
 		}
 		default:
 		{

Applications/foc/commands.h

@@ -26,6 +26,8 @@ typedef enum {
 	Foc_Set_eBrake_Throld,
 	Foc_Lock_Motor,
 	Foc_Auto_Hold,
+	Foc_Set_Config,
+	Foc_Get_Config,
 	Foc_Hall_Phase_Cali_Result = 160,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,

Applications/foc/core/PMSM_FOC_Core.c

@@ -175,11 +175,13 @@ static void PMSM_FOC_UserInit(void) {
 	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_PhaseVoleBrkLim = gFoc_Ctrl.hwLim.s_PhaseVolMax;
+	
 	gFoc_Ctrl.protLim.s_iDCLim = gFoc_Ctrl.userLim.s_iDCLim;
 	gFoc_Ctrl.protLim.s_PhaseCurrLim = gFoc_Ctrl.userLim.s_PhaseCurrLim;
-	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);
+
+	eRamp_init_target(&gFoc_Ctrl.userLim.rpmLimRamp, gFoc_Ctrl.userLim.s_motRPMLim, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
+	eRamp_init_target(&gFoc_Ctrl.userLim.phaseCurrLimRamp, gFoc_Ctrl.userLim.s_PhaseCurrLim, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
+	eRamp_init_target(&gFoc_Ctrl.userLim.DCCurrLimRamp, gFoc_Ctrl.userLim.s_iDCLim, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
 }
 
 void PMSM_FOC_CoreInit(void) {
@@ -210,8 +212,8 @@ void PMSM_FOC_CoreInit(void) {
 		shark_task_create(PMSM_FOC_Debug_Task, NULL);
 		g_focinit = true;
 	}
-	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_modulation = CONFIG_SVM_MODULATION;//SVM_Modulation;
+	gFoc_Ctrl.params.n_PhaseFilterCeof = (CONFIG_CURR_LP_PARAM>1.0f?1.0f:CONFIG_CURR_LP_PARAM);
 	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_maxDCVol;//(CONFIG_RATED_DC_VOL);
@@ -226,7 +228,7 @@ void PMSM_FOC_CoreInit(void) {
 	FOC_DqRamp_init(&gFoc_Ctrl.vdq_ctl[1], (CONFIG_IDQ_CTRL_TS/CONFIG_FOC_VDQ_RAMP_TS));	
 	PMSM_FOC_Reset_PID();
 
-	gFoc_Ctrl.plot_type = Plot_Phase_curr;
+	gFoc_Ctrl.plot_type = Plot_None;
 }
 
 //#define PHASE_LFP_FIR
@@ -328,7 +330,7 @@ static u32 PMSM_FOC_Debug_Task(void *p) {
 		}else if (gFoc_Ctrl.plot_type == Plot_Q_flow) {
 			plot_2data16(FtoS16x10(gFoc_Ctrl.idq_ctl[1].s_Cp), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q));
 		}else if (gFoc_Ctrl.plot_type == Plot_DQ_Curr) {
-			plot_2data16(FtoS16x10(gFoc_Ctrl.out.s_RealIdq.d), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q));
+			plot_3data16(FtoS16x10(gFoc_Ctrl.out.s_RealIdq.d), FtoS16x10(gFoc_Ctrl.out.s_RealIdq.q), FtoS16x10(gFoc_Ctrl.out.s_FilteriDC));
 		}else if (gFoc_Ctrl.plot_type == Plot_Spd_flow) {
 			plot_2data16(gFoc_Ctrl.in.s_targetRPM, gFoc_Ctrl.in.s_motRPM);
 		}
@@ -535,10 +537,10 @@ void PMSM_FOC_RunTime_Limit(void) {
 	phaselim = min(phaselim, gFoc_Ctrl.userLim.s_PhaseCurrLim);
 
 	if (phaselim != gFoc_Ctrl.userLim.phaseCurrLimRamp.target) {
-		eRamp_set_step_target(&gFoc_Ctrl.userLim.phaseCurrLimRamp, phaselim, CONFIG_eCTRL_STEP_TS, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
+		eRamp_set_step_target(&gFoc_Ctrl.userLim.phaseCurrLimRamp, phaselim, CONFIG_eCTRL_STEP_TS);
 	}
 	if (dclim != gFoc_Ctrl.userLim.DCCurrLimRamp.target) {
-		eRamp_set_step_target(&gFoc_Ctrl.userLim.DCCurrLimRamp, dclim, CONFIG_eCTRL_STEP_TS, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
+		eRamp_set_step_target(&gFoc_Ctrl.userLim.DCCurrLimRamp, dclim, CONFIG_eCTRL_STEP_TS);
 	}
 	gFoc_Ctrl.protLim.s_iDCLim = dclim;
 	gFoc_Ctrl.protLim.s_PhaseCurrLim = phaselim;
@@ -583,7 +585,7 @@ void PMSM_FOC_DCCurrLimit(float ibusLimit) {
 	}
 	gFoc_Ctrl.userLim.s_iDCLim = (ibusLimit);
 	if (ibusLimit <= gFoc_Ctrl.protLim.s_iDCLim) {
-		eRamp_set_step_target(&gFoc_Ctrl.userLim.DCCurrLimRamp, ibusLimit, CONFIG_eCTRL_STEP_TS, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
+		eRamp_set_step_target(&gFoc_Ctrl.userLim.DCCurrLimRamp, ibusLimit, CONFIG_eCTRL_STEP_TS);
 	}
 }
 
@@ -596,7 +598,7 @@ void PMSM_FOC_SpeedLimit(float speedLimit) {
 		speedLimit = gFoc_Ctrl.hwLim.s_motRPMMax;
 	}
 	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);
+	eRamp_set_step_target(&gFoc_Ctrl.userLim.rpmLimRamp, speedLimit, CONFIG_eCTRL_STEP_TS);
 }
 
 float PMSM_FOC_GetSpeedLimit(void) {
@@ -654,7 +656,7 @@ void PMSM_FOC_PhaseCurrLim(float lim) {
 	gFoc_Ctrl.userLim.s_PhaseCurrLim = lim;
 	
 	if (lim <= gFoc_Ctrl.protLim.s_PhaseCurrLim) {
-		eRamp_set_step_target(&gFoc_Ctrl.userLim.phaseCurrLimRamp, lim, CONFIG_eCTRL_STEP_TS, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
+		eRamp_set_step_target(&gFoc_Ctrl.userLim.phaseCurrLimRamp, lim, CONFIG_eCTRL_STEP_TS);
 	}
 }
 

Applications/foc/core/PMSM_FOC_Core.h

@@ -22,14 +22,14 @@ typedef enum {
 
 typedef enum {
 	Plot_None,
-	Plot_D_Step,
-	Plot_Q_Step,
-	Plot_D_flow,
-	Plot_Q_flow,
-	Plot_Spd_flow,
 	Plot_Phase_curr,
-	Plot_Phase_vol,
 	Plot_DQ_Curr,
+	Plot_Phase_vol,
+	Plot_Spd_flow,
+	Plot_D_flow,
+	Plot_Q_flow,
+	Plot_D_Step,
+	Plot_Q_Step,
 	Plot_t_Max,
 }Plot_t;
 
@@ -42,6 +42,7 @@ typedef enum {
 	FOC_Speed_TooLow,
 	FOC_NotCruiseMode,
 	FOC_Param_Err,
+	FOC_MEM_Err,
 	FOC_Unknow_Cmd,
 }FOC_ErrCode_t;
 

Applications/foc/core/e_ctrl.c

@@ -25,9 +25,9 @@ void eCtrl_init(u16 accl_time, u16 dec_time){
 	g_eCtrl.ebrk_shadow = g_eCtrl.ebrk_time;
 	g_eCtrl.accl_shadow = g_eCtrl.accl_time;
 	g_eCtrl.dec_shadow = g_eCtrl.dec_time;
-	eRamp_init(&g_eCtrl.current);
-	eRamp_init(&g_eCtrl.speed);
-	eRamp_init(&g_eCtrl.torque);
+	eRamp_init(&g_eCtrl.current, g_eCtrl.accl_time, g_eCtrl.dec_time);
+	eRamp_init(&g_eCtrl.speed, g_eCtrl.accl_time, g_eCtrl.dec_time);
+	eRamp_init(&g_eCtrl.torque, g_eCtrl.accl_time, g_eCtrl.dec_time);
 }
 
 void eCtrl_set_ebrk_time(u16 ebrk_time) {
@@ -61,9 +61,9 @@ bool eCtrl_enable_eBrake(bool enable) {
 }
 
 void _eCtrl_clear_ramp(void) {
-	eRamp_init(&g_eCtrl.current);
-	eRamp_init(&g_eCtrl.speed);
-	eRamp_init(&g_eCtrl.torque);
+	eRamp_init(&g_eCtrl.current, 500, 500);
+	eRamp_init(&g_eCtrl.speed, g_eCtrl.accl_time, g_eCtrl.dec_time);
+	eRamp_init(&g_eCtrl.torque, g_eCtrl.accl_time, g_eCtrl.dec_time);
 	g_eCtrl.current_shadow = 0.0f;
 	g_eCtrl.torque_shadow = 0.0f;
 	g_eCtrl.speed_shadow = 0.0f;
@@ -88,6 +88,9 @@ void eCtrl_Running(void) {
 	if (g_eCtrl.accl_shadow != g_eCtrl.accl_time || g_eCtrl.dec_shadow != g_eCtrl.dec_time) {
 		g_eCtrl.dec_time = g_eCtrl.dec_shadow;
 		g_eCtrl.accl_time = g_eCtrl.accl_shadow;
+		eRamp_set_time(&g_eCtrl.current, g_eCtrl.accl_time, g_eCtrl.dec_time);
+		eRamp_set_time(&g_eCtrl.speed, g_eCtrl.accl_time, g_eCtrl.dec_time);
+		eRamp_set_time(&g_eCtrl.torque, g_eCtrl.accl_time, g_eCtrl.dec_time);
 		etime_changed = true;
 	}
 	if (g_eCtrl.current_shadow != g_eCtrl.current.target || etime_changed) {
@@ -104,31 +107,30 @@ void eCtrl_Running(void) {
 		_eCtrl_process_eBrake();
 	}
 	eRamp_running(&g_eCtrl.current);
-	eRamp_running(&g_eCtrl.speed);
-	eRamp_running(&g_eCtrl.torque);
+	eRamp_X2_running(&g_eCtrl.speed);
+	eRamp_X2_running(&g_eCtrl.torque);
 }
 
 static bool _eCtrl_isHwBrk_shutPower(void) {
 	return (g_eCtrl.hw_brake && nv_get_foc_params()->n_brkShutPower);
 }
 
-static void _eCtrl_set_target(e_Ramp *ramp, float c) {
-	eRamp_set_step_target(ramp, c, CONFIG_eCTRL_STEP_TS, (u32)g_eCtrl.accl_time, (u32)g_eCtrl.dec_time);
-}
-
 static void _eCtrl_set_TgtCurrent(float c) {
-	_eCtrl_set_target(&g_eCtrl.current, c);
+	eRamp_set_step_target(&g_eCtrl.current, c, CONFIG_eCTRL_STEP_TS);
 }
 
 static void _eCtrl_set_TgtSpeed(float s) {
-	_eCtrl_set_target(&g_eCtrl.speed, s);
+	if (g_eCtrl.hw_brake && nv_get_foc_params()->n_brkShutPower){
+		return;
+	}
+	eRamp_set_X2_target(&g_eCtrl.speed, s);
 }
 
 static void _eCtrl_set_TgtTorque(float t) {
 	if (g_eCtrl.hw_brake && nv_get_foc_params()->n_brkShutPower){
 		return;
 	}
-	_eCtrl_set_target(&g_eCtrl.torque, t);
+	eRamp_set_X2_target(&g_eCtrl.torque, t);
 }
 
 float eCtrl_get_RefSpeed(void) {

Applications/foc/core/e_ctrl.h

@@ -2,13 +2,18 @@
 #define EBRAKE_CTRL_H__
 #include "os/os_types.h"
 #include "foc/core/ramp_ctrl.h"
+#include "math/fast_math.h"
 #include "math/fix_math.h"
 
 typedef struct {
-	//float start;
+	float start;
 	float target;
 	float interpolation;
-	float step_val;	
+	float step_val;
+	float A;
+	u32 acct;
+	u32 dect;
+	u32 time;
 }e_Ramp;
 
 typedef struct {
@@ -30,18 +35,27 @@ typedef struct {
 	float speed_shadow;
 	bool is_ebrake_shadow;
 }e_Ctrl;
-static void eRamp_init(e_Ramp *r) {
-	//r->start = 0;
+static void eRamp_init(e_Ramp *r, u32 acc, u32 dec) {
+	r->start = 0;
 	r->target = 0;
 	r->interpolation = 0;
 	r->step_val = 0;
+	r->acct = acc;
+	r->dect = dec;
 }
 
-static void eRamp_init_target(e_Ramp *r, float target) {
-	//r->start = 0;
+static void eRamp_init_target(e_Ramp *r, float target, u32 acc, u32 dec) {
+	r->start = target;
 	r->target = target;
 	r->interpolation = target;
 	r->step_val = 0;
+	r->acct = acc;
+	r->dect = dec;
+}
+
+static void eRamp_set_time(e_Ramp *r, u32 acc, u32 dec) {
+	r->acct = acc;
+	r->dect = dec;
 }
 
 static void eRamp_set_target(e_Ramp *r, float target) {
@@ -73,7 +87,7 @@ static float eRamp_get_target(e_Ramp *r) {
 	return r->target;
 }
 
-static void eRamp_set_step_target(e_Ramp *ramp, float c, u32 intval, u32 acct, u32 dect) {
+static void eRamp_set_step_target(e_Ramp *ramp, float c, u32 intval) {
 	float c_now = eRamp_get_intepolation(ramp);
 	float step_val = 0;
 	int   sign = 1;
@@ -83,12 +97,12 @@ static void eRamp_set_step_target(e_Ramp *ramp, float c, u32 intval, u32 acct, u
 	}
 	u32 step_ms = intval;	
 	if (sign > 0) { //增加扭矩
-		step_val = (c - c_now)/(acct/step_ms);
+		step_val = (c - c_now)/(ramp->acct/step_ms);
 		if (step_val < MIN_FLOAT) {
 			step_val = MIN_FLOAT;
 		}
 	}else if (sign < 0) {
-		step_val = (c_now - c)/(dect/step_ms);
+		step_val = (c_now - c)/(ramp->dect/step_ms);
 		if (step_val < MIN_FLOAT) {
 			step_val = MIN_FLOAT;
 		}
@@ -98,7 +112,37 @@ static void eRamp_set_step_target(e_Ramp *ramp, float c, u32 intval, u32 acct, u
 	eRamp_set_step(ramp, step_val);
 }
 
+static void eRamp_X2_running(e_Ramp *r) {
+	if(r->target == r->interpolation || (r->A == 0)) {
+		return;
+	}
+	if(r->time < 0xFFFFu) {
+		r->time ++;
+	}
+	r->interpolation = r->start + r->A * (float)SQ(r->time);
+	if ((r->A > 0) && (r->interpolation > r->target)) {
+		r->interpolation = r->target;
+	}else if ((r->A < 0) && (r->interpolation < r->target)) {
+		r->interpolation = r->target;
+	}
+}
+
+static void eRamp_set_X2_target(e_Ramp *ramp, float c) {
+	float c_now = eRamp_get_intepolation(ramp);
+
+	float delta = c - c_now;
+	if (delta > 0) {
+		ramp->A = delta/SQ(ramp->acct);
+	}else {
+		ramp->A = delta/SQ(ramp->dect);
+	}
+	ramp->start = c_now;
+	ramp->time = 0;
+
+	eRamp_set_target(ramp, c);
+}
 
+//y=Ax^2;
 void eCtrl_init(u16 accl_time, u16 dec_time);
 void eCtrl_set_ebrk_time(u16 ebrk_time);
 void eCtrl_brake_signal(bool hw_brake);

Applications/foc/core/torque.c

@@ -4,6 +4,7 @@
 #include "foc/core/e_ctrl.h"
 #include "foc/core/PMSM_FOC_Core.h"
 #include "app/nv_storage.h"
+#include "libs/logger.h"
 /*
 通过查表获取对应扭矩和速度时的Id和IQ的分配
 */

Applications/foc/foc_config.h

@@ -1,17 +1,18 @@
 #ifndef _FOC_CONFIG_H__
 #define _FOC_CONFIG_H__
 
-#define CONFIG_DEFAULT_IDC_LIM 200
+#define CONFIG_DEFAULT_IDC_LIM 45
 #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
-#define CONFIG_DEFAULT_EBRK_PHASE_CURR 0.0F //0:means disable ebrake
+#define CONFIG_DEFAULT_EBRK_PHASE_CURR 10.0F //0:means disable ebrake
 #define CONFIG_DEFAULT_EBRK_IDC_LIM 15
 #define CONFIG_SVM_MODULATION       1.0F
 #define CONFIG_BRK_SHUT_POWER_ENABLE 1
+#define CONFIG_AUTOHOLD_ENABLE 1
 
 /* 转把 */
 #define CONFIG_THROTTLE_LOW_VALUE 1.2f /* 转把最小值 */
@@ -34,6 +35,10 @@
 #define CONFIG_DEFAULT_D_TIME 3000       /* 默认的斜率给定时间，越大，越慢到给定值*/
 #define CONFIG_eCTRL_STEP_TS CONFIG_SPD_CTRL_MS     /* 斜率给定的step的时间值，单位 ms */
 #define CONFIG_eCTRL_Brake_TIME 1500     /* 捏住刹车的时间，超过这个时间启动ebrake，单位 ms */
+#define CONFIG_ACC_TIME 3000
+#define CONFIG_DEC_TIME 2000
+
+#define CONFIG_AUTOHOLD_DETECT_TIME 3000
 
 #define CONFIG_LIMIT_RAMP_TIME (10 * 1000)
 

Applications/foc/limit.c

@@ -13,13 +13,12 @@ static limter_t mos_temp_lim[] = { //mos过温限流，限制相电流
 	//{.enter_pointer = 120, .exit_pointer = 110, .limit_value = 0},
 	//{.enter_pointer = 110, .exit_pointer = 90, .limit_value = 40},
 	//{.enter_pointer = 90, .exit_pointer = 80, .limit_value = 80},
-	{.enter_pointer = 85, .exit_pointer = 20, .limit_value = 0},
+	{.enter_pointer = 120, .exit_pointer = 20, .limit_value = 0},
 };  
 static limter_t vol_under_lim[] = { //欠压限流,限制母线
 	{.enter_pointer = 40, .exit_pointer = 44, .limit_value = 20},
 	{.enter_pointer = 44, .exit_pointer = 46, .limit_value = 40},
-};  
-
+};
 
 static u16 _temp_limiter(s16 temp, limter_t *lim) {
 	if (!lim->is_limit) {

Applications/foc/motor/motor.c

@@ -29,19 +29,6 @@ static motor_t motor = {
 	.s_direction = POSITIVE,
 };
 
-static void mc_gpio_init(void) {
-#ifdef GPIO_BRAKE_IN_GROUP
-	rcu_periph_clock_enable(GPIO_BRAKE_IN_RCU);
-	gpio_init(GPIO_BRAKE_IN_GROUP, GPIO_BRAKE_IN_MODE, GPIO_OSPEED_50MHZ, GPIO_BRAKE_IN_PIN);
-	
-	gpio_exti_source_select(GPIO_BRAKE_EXIT_SRC_GROUP, GPIO_BRAKE_EXIT_SRC_PIN);
-	exti_init(GPIO_BRAKE_EXTI, EXTI_INTERRUPT, EXTI_TRIG_BOTH);
-	nvic_irq_enable(GPIO_BRAKE_IRQ, EBREAK_IRQ_PRIORITY, 0U);
-	exti_interrupt_flag_clear(GPIO_BRAKE_EXTI);
-	exti_interrupt_enable(GPIO_BRAKE_EXTI);	
-#endif
-}
-
 static void MC_Check_MosVbusThrottle(void) {
 	int count = 1000;
 	gpio_phase_u_detect(true);
@@ -80,7 +67,8 @@ void mc_init(void) {
 	foc_command_init();
 	torque_init();
 	PMSM_FOC_CoreInit();
-	mc_gpio_init();
+	eCtrl_init(CONFIG_ACC_TIME, CONFIG_DEC_TIME);
+	mc_brk_gpio_init();
 	MC_Check_MosVbusThrottle();
 	sched_timer_enable(CONFIG_SPD_CTRL_US);
 	shark_task_create(_self_check_task, NULL);
@@ -126,7 +114,7 @@ bool mc_start(u8 mode) {
 	}
 	pwm_up_enable(false);
 	motor.mode = mode;
-	eCtrl_init(1000, 3000);
+	eCtrl_init(CONFIG_ACC_TIME, CONFIG_DEC_TIME);
 	motor_encoder_start(motor.s_direction);
 	PMSM_FOC_Start(mode);
 	pwm_turn_on_low_side();
@@ -143,13 +131,14 @@ bool mc_start(u8 mode) {
 	motor.b_epm = false;
 	motor.b_epm_cmd_move = false;
 	motor.epm_dir = EPM_Dir_None;
-
+	motor.n_autohold_time = 0;
 	if (phase_curr_offset_check()) {
 		PMSM_FOC_SetCriticalError(FOC_CRIT_CURR_OFF_Err);
 		mc_stop();
 		return false;
 	}
 	if (mc_is_hwbrake()) {
+		sys_debug("hw brake\n");
 		PMSM_FOC_Brake(true);
 	}
 	gpio_beep(200);
@@ -253,6 +242,10 @@ bool mc_is_epm(void) {
 	return motor.b_epm;
 }
 
+bool mc_is_start(void) {
+	return (motor.b_start || PMSM_FOC_Is_Start());
+}
+
 bool mc_start_epm_move(EPM_Dir_t dir, bool is_command) {
 	sys_debug("epm dir %d, %d\n", dir, motor.epm_dir);
 	if (!motor.b_epm || !motor.b_start) {
@@ -434,6 +427,10 @@ bool mc_auto_hold(bool hold) {
 	if (motor.b_auto_hold == hold) {
 		return true;
 	}
+	if (nv_get_foc_params()->n_autoHold == 0) {
+		PMSM_FOC_SetErrCode(FOC_NotAllowed);
+		return false;
+	}
 	if (!motor.b_start) {
 		PMSM_FOC_SetErrCode(FOC_NotAllowed);
 		return false;
@@ -461,11 +458,10 @@ bool mc_throttle_released(void) {
 }
 
 static bool mc_is_hwbrake(void) {
-#ifdef GPIO_BRAKE_IN_GROUP
 	int count = 50;
 	int settimes = 0;
 	while(count-- > 0) {
-		bool b1 = gpio_input_bit_get(GPIO_BRAKE_IN_GROUP, GPIO_BRAKE_IN_PIN) == SET;
+		bool b1 = mc_get_gpio_brake();
 		if (b1) {
 			settimes ++;
 		}
@@ -488,9 +484,6 @@ static bool mc_is_hwbrake(void) {
 		motor.n_brake_errors++;
 		return false;
 	}
-#else
-	return false;
-#endif
 }
 void MC_Brake_IRQHandler(void) {
 
@@ -504,6 +497,7 @@ void MC_Brake_IRQHandler(void) {
 		motor.b_break = false;
 		PMSM_FOC_Brake(false);
 	}
+	sys_debug("brake %d\n", motor.b_break);
 }
 
 static void _pwm_brake_timer_handler(shark_timer_t *t){
@@ -581,6 +575,23 @@ static bool mc_run_stall_process(u8 run_mode) {
 	return false;
 }
 
+static void mc_autohold_process(void) {
+	if (PMSM_FOC_AutoHoldding() && !mc_throttle_released()) {
+		mc_auto_hold(false);
+	}
+	if (!PMSM_FOC_AutoHoldding() && motor.b_break && (encoder_get_speed() == 0.0f)) {
+		if (motor.n_autohold_time == 0) {
+			motor.n_autohold_time = get_tick_ms();
+		}else {
+			if (get_delta_ms(motor.n_autohold_time) >= CONFIG_AUTOHOLD_DETECT_TIME) {
+				mc_auto_hold(true);
+			}
+		}
+	}else {
+		motor.n_autohold_time = 0;
+	}
+}
+
 /*FOC 的部分处理，比如速度环，状态机，转把采集等*/
 measure_time_t g_meas_MCTask;
 void Sched_MC_mTask(void) {
@@ -592,6 +603,17 @@ void Sched_MC_mTask(void) {
 	/* 母线电流计算 */
 	PMSM_FOC_Calc_iDC();
 
+#if 0
+	eCtrl_Running();
+	float f_throttle = get_throttle_float();
+	if ((f_throttle != motor.throttle) || motor.b_updated) {
+		motor.throttle = f_throttle;
+		float torque = torque_target_from_throttle(f_throttle);
+		PMSM_FOC_Set_Torque(torque);
+	}
+	return;
+#endif
+
 	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
 		return;
 	}
@@ -604,9 +626,8 @@ 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);
-		}
+		mc_autohold_process();
+
 		if (motor.mode != CTRL_MODE_OPEN) {
 			u32 mask;
 			if (mc_can_stop_foc()) {

Applications/foc/motor/motor.h

@@ -25,6 +25,7 @@ typedef struct {
 	s8     s_direction;
 	u32    n_brake_errors;
 	u8     mode;
+	u32    n_autohold_time;
 	bool   b_updated;
 }motor_t;
 
@@ -48,6 +49,7 @@ void mc_get_running_status(u8 *data);
 bool mc_command_epm_move(EPM_Dir_t dir);
 bool mc_throttle_epm_move(EPM_Dir_t dir);
 void mc_need_update(void);
+bool mc_is_start(void);
 
 static __INLINE float motor_encoder_get_angle(void) {
 #ifdef USE_ENCODER_HALL

Applications/os/heap_4.c

@@ -38,7 +38,7 @@
 
 #define portBYTE_ALIGNMENT			8
 #define portBYTE_ALIGNMENT_MASK 	0x0007
-#define configTOTAL_HEAP_SIZE    (12*1024)
+#define configTOTAL_HEAP_SIZE    (16*1024)
 #define configASSERT(x)
 /* Block sizes must not get too small. */
 #define heapMINIMUM_BLOCK_SIZE	( ( size_t ) ( xHeapStructSize << 1 ) )

Applications/prot/can_message.c

@@ -124,8 +124,10 @@ static void can_process_message(can_message_t *message){
 			command.can_src = message->src;
 			command.len = message->len;
 			command.data = os_alloc(message->len);
-			if (command.data) {
-				memcpy(command.data, message->data, message->len);
+			if (command.data || message->len == 0) {
+				if (message->len > 0) {
+					memcpy(command.data, message->data, message->len);
+				}
 				foc_send_command(&command);
 			}
 		}