转把指令处理

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

Applications/app/app.c

@@ -15,6 +15,7 @@
 
 static u32 _app_low_task(void *args);
 static u32 _app_report_task(void *args);
+static u32 _app_plot_task(void *args);
 extern void PMSM_FOC_LogDebug(void);
 extern void err_code_log(void);
 extern void encoder_log(void);
@@ -80,6 +81,7 @@ void app_start(void){
 	gpio_led1_enable(true);
 	shark_task_create(_app_low_task, NULL);
 	shark_task_create(_app_report_task, NULL);
+	shark_task_create(_app_plot_task, NULL);
 	sys_debug("mc start\n");
 	shark_task_run();
 }
@@ -94,16 +96,17 @@ static u32 _app_report_task(void *p) {
 	can_report_ext_status(0x43);
 	
 	if (++loop % 10 == 0) {
-		can_report_pid_value(0x45, PID_TRQ_id);
-		//sys_debug("max exec time %d, %d\n", g_meas_foc.exec_max_error_time, g_meas_foc.exec_time);
-		sys_debug("run stall %d\n", mc_params()->b_runStall);
+		sys_debug("modulation %f\n", PMSM_FOC_Get()->out.f_vdqRation);
 		encoder_log();
 		err_code_log();
 	}
 	return 200;
 }
 
-
+static u32 _app_plot_task(void * args) {
+	can_report_plot_values(0x45);
+	return 10;
+}
 static u32 _app_low_task(void *args) {
 	wdog_reload();
 	fetch_jtag_cmd();

Applications/app/nv_storage.c

@@ -66,8 +66,8 @@ static void nv_default_foc_params(void) {
 	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.15f;
-	foc_params.pid_conf[PID_TRQ_id].ki = 0.14f;
+	foc_params.pid_conf[PID_TRQ_id].kp = 0.07f;
+	foc_params.pid_conf[PID_TRQ_id].ki = 0.2f;
 	foc_params.pid_conf[PID_TRQ_id].kb = 1.0f;
 
 	foc_params.pid_conf[PID_Spd_id].kp = 0.13f;

Applications/bsp/can.c

@@ -13,7 +13,7 @@
 #define CAN_SEND_WAIT_TIMEOUT -2
 
 
-#define TX_NUM 32
+#define TX_NUM 64
 #define RX_NUM 64
 static c_buffer_t g_tx_circle;
 static c_buffer_t g_rx_circle;

Applications/foc/commands.h

@@ -41,6 +41,7 @@ typedef enum {
 	Foc_Report_Pid,
 	Foc_Report_Status,
 	Foc_Report_MTPA_DQ_Angle,
+	Foc_Report_Plot,
 	Foc_Cmd_Max = 0xE0
 }foc_cmd_t;
 #define CMD_2_CAN_KEY(cmd) ((u16)(((u16)cmd) | (CAN_MY_ADDRESS<<8)))

Applications/foc/core/PMSM_FOC_Core.c

@@ -421,7 +421,7 @@ u8 PMSM_FOC_CtrlMode(void) {
 		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
 			float target_troque = gFoc_Ctrl.in.s_targetTorque;
 			if (gFoc_Ctrl.pi_id->is_sat || gFoc_Ctrl.pi_iq->is_sat) {
-				target_troque = sqrtf(SQ(gFoc_Ctrl.out.s_RealIdq.d) + SQ(gFoc_Ctrl.out.s_RealIdq.q)) * 1.05f;
+				target_troque = PMSM_FOC_Get_Real_Torque();
 			}
 			PI_Controller_Reset(gFoc_Ctrl.pi_speed, target_troque);
 		}else if ((preMode == CTRL_MODE_CURRENT) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
@@ -488,7 +488,7 @@ void PMSM_FOC_idqCalc(void) {
 			}
 		}
 	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
-		float refTorque = min(eCtrl_get_FinalTorque(), eRamp_get_intepolation(&gFoc_Ctrl.rtLim.phaseCurrLimRamp));
+		float refTorque = min(eCtrl_get_RefTorque(), eRamp_get_intepolation(&gFoc_Ctrl.rtLim.phaseCurrLimRamp));
 		if (refTorque >= 0) {
 			gFoc_Ctrl.pi_torque->max = refTorque;
 			gFoc_Ctrl.pi_torque->min = -gFoc_Ctrl.userLim.s_PhaseCurreBrkLim;
@@ -556,6 +556,10 @@ void PMSM_FOC_Slow_Task(void) {
 	PMSM_FOC_idqCalc();
 }
 
+float PMSM_FOC_Get_Real_Torque(void) {
+	return sqrtf(SQ(gFoc_Ctrl.out.s_RealIdq.d) + SQ(gFoc_Ctrl.out.s_RealIdq.q)) * 1.1f;
+}
+
 PMSM_FOC_Ctrl *PMSM_FOC_Get(void) {
 	return &gFoc_Ctrl;
 }
@@ -772,6 +776,11 @@ bool PMSM_FOC_Set_Torque(float trq) {
 	return true;
 }
 
+void PMSM_FOC_Reset_Torque(void) {
+	float real_trq = PMSM_FOC_Get_Real_Torque();
+	eCtrl_reset_Torque(real_trq);
+}
+
 bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
 	if (PMSM_FOC_Is_CruiseEnabled()) {
 		if (rpm < CONFIG_MIN_CRUISE_RPM) {

Applications/foc/core/PMSM_FOC_Core.h

@@ -282,6 +282,8 @@ void PMSM_FOC_Set_PlotType(Plot_t t);
 void PMSM_FOC_RunTime_Limit(void);
 void PMSM_FOC_RT_PhaseCurrLim(float lim);
 void PMSM_FOC_RT_LimInit(void);
+float PMSM_FOC_Get_Real_Torque(void);
+void PMSM_FOC_Reset_Torque(void);
 
 #endif /* _PMSM_FOC_Core_H__ */
 

Applications/foc/core/e_ctrl.c

@@ -45,6 +45,10 @@ void eCtrl_set_TgtTorque(float t) {
 	g_eCtrl.torque_shadow = t;
 }
 
+void eCtrl_reset_Torque(float init_trq) {
+	eRamp_reset_target(&g_eCtrl.torque, init_trq);
+}
+
 void eCtrl_set_TgtSpeed(float s) {
 	g_eCtrl.speed_shadow = s;
 }
@@ -54,7 +58,7 @@ bool eCtrl_enable_eBrake(bool enable) {
 		g_eCtrl.is_ebrake_shadow = false;
 		return g_eCtrl.is_ebrake_shadow;
 	}
-	if (enable && PMSM_FOC_GetSpeed() > CONFIG_MIN_RPM_FOR_EBRAKE) {
+	if (enable && ((PMSM_FOC_GetSpeed() > CONFIG_MIN_RPM_FOR_EBRAKE) && (PMSM_FOC_Get_Real_Torque() > CONFIG_MIN_CURRENT_FOR_EBRK))) {
 		g_eCtrl.is_ebrake_shadow = true;
 	}else if (!enable && !_eCtrl_isHwBrk_shutPower()){
 		g_eCtrl.is_ebrake_shadow = false;
@@ -62,7 +66,7 @@ bool eCtrl_enable_eBrake(bool enable) {
 	return g_eCtrl.is_ebrake_shadow;
 }
 
-void _eCtrl_clear_ramp(void) {
+static void _eCtrl_clear_ramp(void) {
 	eRamp_init(&g_eCtrl.current, nv_get_foc_params()->n_ebrk_time, nv_get_foc_params()->n_ebrk_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);
@@ -75,6 +79,7 @@ void _eCtrl_process_eBrake(void) {
 	_eCtrl_clear_ramp();
 	if (g_eCtrl.is_ebrake) {
 		PMSM_FOC_SetCtrlMode(CTRL_MODE_CURRENT_BRK);
+		eRamp_reset_target(&g_eCtrl.current, PMSM_FOC_Get()->in.s_targetTorque);
 		eCtrl_set_TgtCurrent(-PMSM_FOC_GeteBrkPhaseCurrent());
 	}else {
 		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
@@ -86,25 +91,27 @@ bool eCtrl_is_eBrk_enabled(void) {
 }
 
 void eCtrl_Running(void) {
-	bool etime_changed = false;
 	if (g_eCtrl.accl_time_shadow != g_eCtrl.accl_time || g_eCtrl.dec_time_shadow != g_eCtrl.dec_time) {
 		g_eCtrl.dec_time = g_eCtrl.dec_time_shadow;
 		g_eCtrl.accl_time = g_eCtrl.accl_time_shadow;
+		/* 重新计算ramp */
 		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;
+		_eCtrl_set_TgtSpeed(g_eCtrl.speed_shadow);
+		_eCtrl_set_TgtTorque(g_eCtrl.torque_shadow);
 	}
 	if (g_eCtrl.ebrk_time_shadow != g_eCtrl.ebrk_time) {
 		g_eCtrl.ebrk_time = g_eCtrl.ebrk_time_shadow;
 		eRamp_set_time(&g_eCtrl.current, g_eCtrl.ebrk_time, g_eCtrl.ebrk_time);
+		_eCtrl_set_TgtCurrent(g_eCtrl.current_shadow);
 	}
-	if (g_eCtrl.current_shadow != g_eCtrl.current.target || etime_changed) {
+	if (g_eCtrl.current_shadow != g_eCtrl.current.target) {
 		_eCtrl_set_TgtCurrent(g_eCtrl.current_shadow);
 	}
-	if (g_eCtrl.speed_shadow != g_eCtrl.speed.target || etime_changed) {
+	if (g_eCtrl.speed_shadow != g_eCtrl.speed.target) {
 		_eCtrl_set_TgtSpeed(g_eCtrl.speed_shadow);
 	}
-	if (g_eCtrl.torque_shadow != g_eCtrl.torque.target || etime_changed) {
+	if (g_eCtrl.torque_shadow != g_eCtrl.torque.target) {
 		_eCtrl_set_TgtTorque(g_eCtrl.torque_shadow);
 	}
 	if (g_eCtrl.is_ebrake_shadow != g_eCtrl.is_ebrake) {

Applications/foc/core/e_ctrl.h

@@ -2,12 +2,14 @@
 #define EBRAKE_CTRL_H__
 #include "os/os_types.h"
 #include "foc/core/ramp_ctrl.h"
+#include "foc/foc_config.h"
 #include "math/fast_math.h"
 #include "math/fix_math.h"
 
 typedef struct {
 	float start;
 	float target;
+	float first_target;
 	float interpolation;
 	float step_val;
 	float A;
@@ -38,6 +40,7 @@ typedef struct {
 static void eRamp_init(e_Ramp *r, u32 acc, u32 dec) {
 	r->start = 0;
 	r->target = 0;
+	r->first_target = 0;
 	r->interpolation = 0;
 	r->step_val = 0;
 	r->acct = acc;
@@ -47,12 +50,20 @@ static void eRamp_init(e_Ramp *r, u32 acc, u32 dec) {
 static void eRamp_init_target(e_Ramp *r, float target, u32 acc, u32 dec) {
 	r->start = target;
 	r->target = target;
+	r->first_target = 0;
 	r->interpolation = target;
 	r->step_val = 0;
 	r->acct = acc;
 	r->dect = dec;
 }
 
+static void eRamp_reset_target(e_Ramp *r, float target) {
+	r->start = target;
+	r->target = target;
+	r->first_target = 0;
+	r->interpolation = target;
+	r->step_val = 0;
+}
 static void eRamp_set_time(e_Ramp *r, u32 acc, u32 dec) {
 	r->acct = acc;
 	r->dect = dec;
@@ -113,7 +124,23 @@ static void eRamp_set_step_target(e_Ramp *ramp, float c, u32 intval) {
 }
 
 static void eRamp_X2_running(e_Ramp *r) {
-	if(r->target == r->interpolation || (r->A == 0)) {
+#if 0
+	if(r->target == r->interpolation) {
+		return;
+	}
+	if ((r->first_target != 0) && (r->interpolation != r->first_target)) {
+		r->interpolation += r->step_val;
+		if (r->step_val > 0) {
+			if (r->interpolation >= r->first_target) {
+				r->interpolation = r->first_target;
+				r->first_target = 0.0f;
+			}
+		}else {
+			if (r->interpolation <= r->first_target) {
+				r->interpolation = r->first_target;
+				r->first_target = 0.0f;
+			}
+		}
 		return;
 	}
 	if(r->time < 0xFFFFu) {
@@ -125,21 +152,32 @@ static void eRamp_X2_running(e_Ramp *r) {
 	}else if ((r->A < 0) && (r->interpolation < r->target)) {
 		r->interpolation = r->target;
 	}
+#else
+	eRamp_running(r);
+#endif
 }
 
 static void eRamp_set_X2_target(e_Ramp *ramp, float c) {
+#if 0
 	float c_now = eRamp_get_intepolation(ramp);
 
 	float delta = c - c_now;
 	if (delta > 0) {
-		ramp->A = delta/SQ(ramp->acct);
+		ramp->first_target = 0;//min(delta, 10.0f);
+		ramp->step_val = 0.01f;
+		ramp->A = (delta - ramp->first_target)/SQ(ramp->acct);
 	}else {
-		ramp->A = delta/SQ(ramp->dect);
+		ramp->first_target = 0;//MAX(delta, -10.0f);
+		ramp->step_val = -0.01f;
+		ramp->A = (delta - ramp->first_target)/SQ(ramp->dect);
 	}
-	ramp->start = c_now;
+	ramp->start = c_now + ramp->first_target;
 	ramp->time = 0;
 
 	eRamp_set_target(ramp, c);
+#else
+	eRamp_set_step_target(ramp, c, CONFIG_eCTRL_STEP_TS);
+#endif
 }
 
 //y=Ax^2;
@@ -159,6 +197,7 @@ float eCtrl_get_FinalCurrent(void);
 float eCtrl_get_FinalTorque(void);
 void eCtrl_Running(void);
 void eCtrl_Reset(void);
+void eCtrl_reset_Torque(float init_trq);
 
 #endif /* EBRAKE_CTRL_H__ */
 

Applications/foc/core/torque.c

@@ -9,9 +9,10 @@
 通过查表获取对应扭矩和速度时的Id和IQ的分配
 */
 static torque_lut_t *_trq_tbl = NULL;
-
+static torque_manager_t g_trq_mn;
 void torque_init(void) {
 	_trq_tbl = nv_get_trq_tlb();
+	memset(&g_trq_mn, 0, sizeof(g_trq_mn));
 }
 
 void torque_get_idq(float torque, float rpm, DQ_t *dq_out) {
@@ -45,7 +46,7 @@ void torque_get_idq(float torque, float rpm, DQ_t *dq_out) {
 
 }
 
-float speed_target_from_throttle(float f_throttle) {
+float throttle_to_speed(float f_throttle) {
 
 	if (f_throttle <= nv_get_foc_params()->n_minThroVol) {
 		return 0;
@@ -57,38 +58,34 @@ float speed_target_from_throttle(float f_throttle) {
 	return (PMSM_FOC_GetSpeedLimit() * ration);
 }
 
-float torque_target_from_throttle(float f_throttle) {
+float throttle_to_torque(float f_throttle) {
 	if (f_throttle <= (nv_get_foc_params()->n_minThroVol)) {
 		return 0;
 	}
 	float delta = f_throttle - (nv_get_foc_params()->n_minThroVol);
 
 	float ration = delta / (nv_get_foc_params()->n_maxThroVol - nv_get_foc_params()->n_minThroVol);
-
-	return (PMSM_FOC_GetTorqueLimit() * ration);
+	int torque = PMSM_FOC_GetTorqueLimit() * ration * 10.0f;
+	return ((float)torque / 10.0f);
 }
 
-
-void torque_speed_target(u8 run_mode, float f_throttle) {
+void torque_manager(u8 run_mode, float f_throttle) {
+	if ((g_trq_mn.count++ % 20) != 0) {
+		return;
+	}
 	if (run_mode == CTRL_MODE_SPD) {
-		float speed_Ref = speed_target_from_throttle(f_throttle);
-		if (mc_is_epm()) {
-			if (speed_Ref == 0.0f) {
-				mc_throttle_epm_move(EPM_Dir_None);
-			}else {
-				mc_throttle_epm_move(EPM_Dir_Forward);
-			}
-		}else {
-			PMSM_FOC_Set_Speed(speed_Ref);
-		}
+		float speed_Ref = throttle_to_speed(f_throttle);
+		PMSM_FOC_Set_Speed(speed_Ref);
 	}else if (run_mode == CTRL_MODE_TRQ) {
 		if (mc_throttle_released()) {
 			eCtrl_enable_eBrake(true);
 			PMSM_FOC_Set_Torque(0);
+			g_trq_mn.torque_prev = 0;
 		}else {
-			float torque = torque_target_from_throttle(f_throttle);
+			float torque = throttle_to_torque(f_throttle);
 			eCtrl_enable_eBrake(false);
 			PMSM_FOC_Set_Torque(torque);
+			g_trq_mn.torque_prev = torque;
 		}
 	}else if (run_mode == CTRL_MODE_CURRENT_BRK) {
 		if (!mc_throttle_released() || (mc_throttle_released() && (PMSM_FOC_GetSpeed() == 0.0f))) {

Applications/foc/core/torque.h

@@ -2,12 +2,18 @@
 #define _TORQUE_LUT_H__
 #include "os/os_types.h"
 #include "foc/core/PMSM_FOC_Core.h"
-
+typedef struct {
+	float throttle_prev;
+	float speed_prev;
+	float torque_prev;
+	u32   timestamp; //ms
+	float toruqe_curr_max; //当前转把对应的最大扭矩
+	float speed_curr_max;   //当前转把对应的最大速度
+	u32   count;
+}torque_manager_t;
 void torque_init(void);
 void torque_get_idq(float torque, float rpm, DQ_t *dq_out);
-float speed_target_from_throttle(float f_throttle);
-float torque_target_from_throttle(float f_throttle);
-void torque_speed_target(u8 run_mode, float f_throttle);
+void torque_manager(u8 run_mode, float f_throttle);
 
 #endif /*_TORQUE_LUT_H__ */
 

Applications/foc/foc_config.h

@@ -21,7 +21,8 @@
 #define CONFIG_THROTTLE_MIN_IDQ   20   /* 转把对应最小的扭矩电流 Q轴 */
 #define CONFIG_MIN_CRUISE_RPM 	  1000   /* 能启动定速巡航的最小速度 */
 
-#define CONFIG_MIN_RPM_FOR_EBRAKE 1000 //进入电流回收的最小转速
+#define CONFIG_MIN_RPM_FOR_EBRAKE 800 //进入电流回收的最小转速
+#define CONFIG_MIN_CURRENT_FOR_EBRK 10.0F
 #define CONFIG_MIN_RPM_EXIT_EBRAKE 100 //推出电流回收的最小转速
 
 #define CONFIG_IDQ_CTRL_TS FOC_PWM_FS

Applications/foc/motor/motor.c

@@ -591,7 +591,7 @@ static bool mc_run_stall_process(u8 run_mode) {
 				if (get_delta_ms(motor.runStall_time) >= CONFIG_STALL_MAX_TIME) {
 					motor.b_runStall = true;
 					motor.runStall_time = 0;
-					torque_speed_target(run_mode, 0.0f);
+					torque_manager(run_mode, 0.0f);
 					return true;
 				}
 				if (ABS(motor.runStall_pos - motor_encoder_get_position()) >= 0.2f) {
@@ -632,6 +632,16 @@ static void mc_autohold_process(void) {
 	}
 }
 
+static void mc_process_throttle_epm(void) {
+	if (!motor.b_epm_cmd_move) {//通过命令前进后退，不处理转把
+		if (mc_throttle_released()) {
+			mc_throttle_epm_move(EPM_Dir_None);
+		}else {
+			mc_throttle_epm_move(EPM_Dir_Forward);
+		}
+	}
+}
+
 /*FOC 的部分处理，比如速度环，状态机，转把采集等*/
 measure_time_t g_meas_MCTask;
 void Sched_MC_mTask(void) {
@@ -687,13 +697,10 @@ void Sched_MC_mTask(void) {
 		}
 		if (runMode != CTRL_MODE_OPEN) {
 			eCtrl_Running();
-			float f_throttle = get_throttle_float();
-			if ((f_throttle != motor.throttle) || motor.b_updated) {
-				motor.throttle = f_throttle;
-				motor.b_updated = false;
-				if (!motor.b_epm_cmd_move) {//通过命令前进后退，不处理转把
-					torque_speed_target(runMode, f_throttle);
-				}
+			if ((runMode == CTRL_MODE_SPD) && mc_is_epm()) {
+				mc_process_throttle_epm();
+			}else {
+				torque_manager(runMode, get_throttle_float());
 			}
 			PMSM_FOC_Slow_Task();
 		}

Applications/prot/can_foc_msg.c

@@ -110,10 +110,19 @@ void can_report_ext_status(u8 can) {
 	data[1] = mc_is_start()?0:1;
 	data[1] |= (PMSM_FOC_Is_CruiseEnabled()?1:0) << 3;
 	data[1] |= ext_gear << 6;
-	encode_s16(data + 2, ABS((s16)motor_encoder_get_speed()));
+	encode_s16(data + 2, ABS((s16)(motor_encoder_get_speed()/4.0f)));
 	float vDC = get_vbus_float();
 	encode_s16(data + 4, (s16)(vDC*10));
 	float iDC = PMSM_FOC_GetVbusCurrent();
 	encode_s16(data + 6, (s16)(iDC*10));
 	shark_can0_send_ext_message(0x1A014D43, data, sizeof(data));
 }
+
+void can_report_plot_values(u8 can) {
+	u8 data[10];
+	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Plot));
+	encode_float(data + 2, eCtrl_get_RefTorque());
+	encode_float(data + 6, PMSM_FOC_Get()->in.s_targetTorque);
+	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
+}
+

Applications/prot/can_foc_msg.h

@@ -12,6 +12,7 @@ void can_report_pid_value(u8 can, u8 id);
 void can_report_foc_status(u8 can);
 void can_report_mpta_values(u8 can);
 void can_report_ext_status(u8 can);
+void can_report_plot_values(u8 can);
 
 #endif	/*_Can_Foc_Msg_H__ */
 

Applications/prot/can_pc_message.c

@@ -35,7 +35,7 @@ bool can_process_iap_message(can_message_t *can_message) {
 	rsplen = 3;
 	switch(can_message->key) {
 		case BUILD_CMD_KEY(0xF0):
-			if (mc_is_start()) {
+			if (PMSM_FOC_GetSpeed() != 0.0f) {
 				response[2] = 1;
 				iap_send_data(can_message->src, response, rsplen);
 				return true;