更新二段加减速

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

Applications/app/app.c

@@ -104,8 +104,8 @@ static u32 _app_report_task(void *p) {
 }
 
 static u32 _app_plot_task(void * args) {
-	//can_report_plot_values(0x45);
-	return 10;
+	can_report_plot_values(0x45);
+	return 50;
 }
 static u32 _app_low_task(void *args) {
 	wdog_reload();

Applications/foc/core/PMSM_FOC_Core.c

@@ -425,7 +425,8 @@ u8 PMSM_FOC_CtrlMode(void) {
 		}else if ((preMode == CTRL_MODE_CURRENT) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
 			PI_Controller_Reset(gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
 		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK)) {
-			eCtrl_reset_Current(min(PMSM_FOC_Get_Real_Torque(), gFoc_Ctrl.in.s_targetTorque));
+			float real_trq = PMSM_FOC_Get_Real_Torque();
+			eCtrl_reset_Current(min(real_trq, gFoc_Ctrl.in.s_targetTorque));
 			eCtrl_set_TgtCurrent(-PMSM_FOC_GeteBrkPhaseCurrent());
 		}	
 	}

Applications/foc/core/e_ctrl.c

@@ -10,6 +10,7 @@ static void _eCtrl_set_TgtCurrent(float c);
 static void _eCtrl_set_TgtSpeed(float s);
 static void _eCtrl_set_TgtTorque(float t);
 static bool _eCtrl_isHwBrk_shutPower(void);
+static void _eCtrl_process_eBrake(void);
 
 void eCtrl_init(u16 accl_time, u16 dec_time){
 	g_eCtrl.dec_time = dec_time;
@@ -26,7 +27,7 @@ void eCtrl_init(u16 accl_time, u16 dec_time){
 	g_eCtrl.accl_time_shadow = g_eCtrl.accl_time;
 	g_eCtrl.dec_time_shadow = g_eCtrl.dec_time;
 	g_eCtrl.hw_brake = false;
-	g_eCtrl.is_ebrake_shadow = g_eCtrl.is_ebrake = false;
+	g_eCtrl.is_ebrake = false;
 	eRamp_init(&g_eCtrl.current, g_eCtrl.ebrk_time, g_eCtrl.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);
@@ -60,16 +61,21 @@ void eCtrl_set_TgtSpeed(float s) {
 }
 
 bool eCtrl_enable_eBrake(bool enable) {
+	float ebrak = g_eCtrl.is_ebrake;
 	if (PMSM_FOC_GeteBrkPhaseCurrent() == 0.0f) {
-		g_eCtrl.is_ebrake_shadow = false;
-		return g_eCtrl.is_ebrake_shadow;
+		ebrak = false;
+	}else {
+		if (enable && ((PMSM_FOC_GetSpeed() > CONFIG_MIN_RPM_FOR_EBRAKE))) {
+			ebrak = true;
+		}else if (!enable && !_eCtrl_isHwBrk_shutPower()){
+			ebrak = false;
+		}
 	}
-	if (enable && ((PMSM_FOC_GetSpeed() > CONFIG_MIN_RPM_FOR_EBRAKE))) {
-		g_eCtrl.is_ebrake_shadow = true;
-	}else if (!enable && !_eCtrl_isHwBrk_shutPower()){
-		g_eCtrl.is_ebrake_shadow = false;
+	if (ebrak != g_eCtrl.is_ebrake) {
+		g_eCtrl.is_ebrake = ebrak;
+		_eCtrl_process_eBrake();
 	}
-	return g_eCtrl.is_ebrake_shadow;
+	return ebrak;
 }
 
 static void _eCtrl_clear_ramp(void) {
@@ -81,10 +87,10 @@ static void _eCtrl_clear_ramp(void) {
 	g_eCtrl.speed_shadow = 0.0f;
 }
 
-void _eCtrl_process_eBrake(void) {
+static void _eCtrl_process_eBrake(void) {
 	if (g_eCtrl.is_ebrake) {
 		PMSM_FOC_SetCtrlMode(CTRL_MODE_CURRENT_BRK);
-		eRamp_init(&g_eCtrl.current, nv_get_foc_params()->n_ebrk_time, nv_get_foc_params()->n_ebrk_time);
+		eRamp_reset_target(&g_eCtrl.current, 0);
 		g_eCtrl.current_shadow = 0.0f;
 	}else {
 		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
@@ -92,7 +98,7 @@ void _eCtrl_process_eBrake(void) {
 }
 
 bool eCtrl_is_eBrk_enabled(void) {
-	return (g_eCtrl.is_ebrake || g_eCtrl.is_ebrake_shadow);
+	return g_eCtrl.is_ebrake;
 }
 
 void eCtrl_Running(void) {
@@ -119,18 +125,14 @@ void eCtrl_Running(void) {
 	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) {
-		g_eCtrl.is_ebrake = g_eCtrl.is_ebrake_shadow;
-		_eCtrl_process_eBrake();
-	}
-	eRamp_running(&g_eCtrl.current);
+	eRamp_X2_running(&g_eCtrl.current);
 	eRamp_X2_running(&g_eCtrl.speed);
 	eRamp_X2_running(&g_eCtrl.torque);
 }
 
 void eCtrl_Reset(void) {
 	_eCtrl_clear_ramp();
-	g_eCtrl.is_ebrake_shadow = g_eCtrl.is_ebrake = false;
+	g_eCtrl.is_ebrake = false;
 }
 
 static bool _eCtrl_isHwBrk_shutPower(void) {
@@ -138,7 +140,8 @@ static bool _eCtrl_isHwBrk_shutPower(void) {
 }
 
 static void _eCtrl_set_TgtCurrent(float c) {
-	eRamp_set_step_target(&g_eCtrl.current, c, CONFIG_eCTRL_STEP_TS);
+	//eRamp_set_step_target(&g_eCtrl.current, c, CONFIG_eCTRL_STEP_TS);
+	eRamp_set_X2_target(&g_eCtrl.current, c);
 }
 
 static void _eCtrl_set_TgtSpeed(float s) {

Applications/foc/core/e_ctrl.h

@@ -9,13 +9,14 @@
 typedef struct {
 	float start;
 	float target;
-	float first_target;
 	float interpolation;
 	float step_val;
+	float first_target;
+	float first_step;
 	float A;
-	u32 acct;
-	u32 dect;
-	u32 time;
+	float acct;
+	float dect;
+	float time;
 }e_Ramp;
 
 typedef struct {
@@ -35,7 +36,6 @@ typedef struct {
 	float current_shadow;
 	float torque_shadow;
 	float speed_shadow;
-	bool is_ebrake_shadow;
 }e_Ctrl;
 
 static void eRamp_init(e_Ramp *r, u32 acc, u32 dec) {
@@ -44,6 +44,7 @@ static void eRamp_init(e_Ramp *r, u32 acc, u32 dec) {
 	r->first_target = 0;
 	r->interpolation = 0;
 	r->step_val = 0;
+	r->first_step = 0;
 	r->acct = acc;
 	r->dect = dec;
 }
@@ -51,9 +52,10 @@ 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->first_target = target;
 	r->interpolation = target;
 	r->step_val = 0;
+	r->first_step = 0;
 	r->acct = acc;
 	r->dect = dec;
 }
@@ -61,9 +63,10 @@ static void eRamp_init_target(e_Ramp *r, float target, u32 acc, u32 dec) {
 static void eRamp_reset_target(e_Ramp *r, float target) {
 	r->start = target;
 	r->target = target;
-	r->first_target = 0;
+	r->first_target = target;
 	r->interpolation = target;
 	r->step_val = 0;
+	r->first_step = 0;
 }
 static void eRamp_set_time(e_Ramp *r, u32 acc, u32 dec) {
 	r->acct = acc;
@@ -102,23 +105,13 @@ static float eRamp_get_target(e_Ramp *r) {
 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;
+	float delta = c - c_now;
+	float step_ms = intval;
 
-	if (c < c_now) {
-		sign = -1;
-	}
-	u32 step_ms = intval;	
-	if (sign > 0) { //增加扭矩
-		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)/(ramp->dect/step_ms);
-		if (step_val < MIN_FLOAT) {
-			step_val = MIN_FLOAT;
-		}
-		step_val = -step_val;
+	if (delta >= 0) {
+		step_val = (delta)/(ramp->acct/step_ms);
+	}else {
+		step_val = (delta)/(ramp->dect/step_ms);
 	}
 	eRamp_set_target(ramp, c);
 	eRamp_set_step(ramp, step_val);
@@ -153,6 +146,19 @@ static void eRamp_X2_running(e_Ramp *r) {
 		r->interpolation = r->target;
 	}
 #else
+	if (r->first_step != 0) {
+		float interpolation = r->interpolation + r->first_step;
+		if ((r->first_step > 0) && (interpolation >= r->first_target)) {
+			interpolation = r->first_target;
+			r->first_step = r->first_target = 0;
+		}else if ((r->first_step < 0) && (interpolation <= r->first_target)) {
+			interpolation = r->first_target;
+			r->first_step = r->first_target = 0;
+		}
+		r->interpolation = interpolation;
+		return;
+	}
+	
 	eRamp_running(r);
 #endif
 }
@@ -178,7 +184,30 @@ static void eRamp_set_X2_target(e_Ramp *ramp, float c) {
 	ramp->time = 0;
 	eRamp_set_target(ramp, c);
 #else
-	eRamp_set_step_target(ramp, c, CONFIG_eCTRL_STEP_TS);
+	float c_now = eRamp_get_intepolation(ramp);
+	float step_val = 0;
+	float delta = c - c_now;
+
+	float step_ms = CONFIG_eCTRL_STEP_TS;	
+	if (delta > 0) {
+		float first_delta = min(delta, 10.0f);
+		ramp->first_target = c_now + first_delta;
+		ramp->first_step = 0.05f;
+		delta -= first_delta;
+		step_val = (delta)/(ramp->acct/step_ms);
+	}else if (delta < 0){
+		float first_delta = MAX(delta, -5.0f);
+		ramp->first_target = c_now + first_delta;
+		ramp->first_step = -0.05f;
+		delta -= first_delta;
+		step_val = (delta)/(ramp->dect/step_ms);
+	}else {
+		step_val = 0;
+		ramp->first_step = ramp->first_target = 0;
+	}
+	eRamp_set_target(ramp, c);
+	eRamp_set_step(ramp, step_val);
+
 #endif
 }
 

Applications/foc/core/torque.c

@@ -1,207 +1,216 @@
-#include "foc/core/torque.h"
-#include "foc/foc_config.h"
-#include "foc/motor/motor.h"
-#include "foc/core/e_ctrl.h"
-#include "foc/core/PMSM_FOC_Core.h"
-#include "app/nv_storage.h"
-#include "libs/logger.h"
-/*
-通过查表获取对应扭矩和速度时的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();
-	torque_reset();
-}
-
-void torque_reset(void) {
-	memset(&g_trq_mn, 0, sizeof(g_trq_mn));
-}
-
-void torque_get_idq(float torque, float rpm, DQ_t *dq_out) {
-	if ((_trq_tbl == NULL) || (torque < 0 || rpm < 0)) {
-		dq_out->d = 0;
-		dq_out->q = torque;
-		return;
-	}
-	int trq_idx = (int)torque / TBL_TRQ_INTVAL;
-	int rpm_idx = (int)rpm / TBL_SPD_INTVAL;
-	if (trq_idx >= MAX_TRQ_POINTS) {
-		trq_idx = MAX_TRQ_POINTS -1;
-	}
-	if (rpm_idx >= MAX_SPD_POINTS) {
-		rpm_idx = MAX_SPD_POINTS -1;
-	}
-	s16 d = _trq_tbl->dq[trq_idx][rpm_idx].d;
-	s16 q = _trq_tbl->dq[trq_idx][rpm_idx].q;
-	if (trq_idx < MAX_TRQ_POINTS - 1) {
-		trq_idx += 1;
-	}
-	if (rpm_idx < MAX_SPD_POINTS - 1) {
-		rpm_idx += 1;
-	}
-	s16 d_delta = _trq_tbl->dq[trq_idx][rpm_idx].d - d;
-	s16 q_delta = _trq_tbl->dq[trq_idx][rpm_idx].q - q;
-	float comp_ceof = 0.5f * ((torque - torque/TBL_TRQ_INTVAL*TBL_TRQ_INTVAL)/(float)TBL_TRQ_INTVAL + (rpm - rpm/TBL_SPD_INTVAL*TBL_SPD_INTVAL)/(float)TBL_SPD_INTVAL);
-
-	dq_out->d = d + d_delta * comp_ceof;
-	dq_out->q = q + q_delta * comp_ceof;
-
-}
-
-/* 获取油门开度 */
-static float throttle_ration(float f_throttle) {
-	if (f_throttle <= nv_get_foc_params()->n_minThroVol) {
-		return 0;
-	}
-	float delta = f_throttle - (nv_get_foc_params()->n_minThroVol);
-
-	int ration = (delta * 1000.0f) / (nv_get_foc_params()->n_maxThroVol - nv_get_foc_params()->n_minThroVol);
-	
-	return ((float)ration)/1000.0f;
-}
-
-float throttle_to_speed(float f_throttle) {
-	return (PMSM_FOC_GetSpeedLimit() * throttle_ration(f_throttle));
-}
-
-float throttle_to_torque(float f_throttle) {
-	return PMSM_FOC_GetTorqueLimit() * throttle_ration(f_throttle);
-}
-
-#define REAL_DQ_CEOF 1.1f
-
-#if 1
-void torque_mode_process(void) {
-	float ref_trq = PMSM_FOC_GetTorqueLimit() * g_trq_mn.thro_value;
-
-	if ((mc_throttle_released()) && eCtrl_enable_eBrake(true)) {
-		g_trq_mn.thro_value = 0;
-		return;
-	}
-
-	if (!mc_throttle_released()) {
-		if (PMSM_FOC_GetSpeed() <= 1.0f) {
-			ref_trq  = MAX(eCtrl_get_FinalTorque() * REAL_DQ_CEOF, ref_trq );
-		}
-		PMSM_FOC_Set_Torque(ref_trq );
-	}else if (mc_throttle_released() && eCtrl_get_FinalTorque() != 0){
-		float real_trq = PMSM_FOC_Get_Real_Torque();
-		float ref_now = min(real_trq, eCtrl_get_RefTorque());
-		eCtrl_reset_Torque(ref_now);
-		PMSM_FOC_Set_Torque(0);
-		g_trq_mn.thro_value = 0;
-	}
-}
-#else
-void torque_mode_process(void) {
-	float thro_curr = g_trq_mn.thro_value;
-	float thro_prev = g_trq_mn.thro_prev;
-	float trq_ref = PMSM_FOC_GetTorqueLimit() * thro_curr;
-
-	if ((thro_curr == 0.0f) && eCtrl_enable_eBrake(true)) {
-		g_trq_mn.thro_value = 0;
-		g_trq_mn.accl = false;
-		g_trq_mn.thro_prev = 0.0f;
-		return;
-	}
-	float real_trq = eCtrl_get_FinalTorque() * REAL_DQ_CEOF;
-	if (trq_ref > 0) {
-		if (PMSM_FOC_GetSpeed() <= 10.0f) {
-			g_trq_mn.accl_ref = MAX(real_trq, trq_ref); //不能小于autohold产生的扭矩
-			trq_ref = g_trq_mn.accl_ref;
-		}else {
-			if (thro_curr > thro_prev) {
-				if (!g_trq_mn.accl){
-					g_trq_mn.accl_ref = eCtrl_get_RefTorque();
-				}
-				if (g_trq_mn.accl_ref > PMSM_FOC_GetTorqueLimit()) {
-					g_trq_mn.accl_ref = PMSM_FOC_GetTorqueLimit();
-				}
-				trq_ref = g_trq_mn.accl_ref + thro_curr * (PMSM_FOC_GetTorqueLimit() - g_trq_mn.accl_ref);
-				g_trq_mn.accl = true;
-			}else if (thro_curr < thro_prev){
-				if (g_trq_mn.accl) {
-					g_trq_mn.accl_ref = min(real_trq, eCtrl_get_RefTorque());
-					eCtrl_reset_Torque(g_trq_mn.accl_ref);
-				}
-				trq_ref = thro_curr * g_trq_mn.accl_ref;
-				g_trq_mn.accl = false;
-			}else {
-				if (g_trq_mn.accl) {
-					trq_ref = g_trq_mn.accl_ref + thro_curr * (PMSM_FOC_GetTorqueLimit() - g_trq_mn.accl_ref);
-				}else {
-					trq_ref = thro_curr * g_trq_mn.accl_ref;
-				}
-			}
-		}
-		PMSM_FOC_Set_Torque(trq_ref);
-	}
-	else if (eCtrl_get_FinalTorque() != 0){
-		float ref_now = min(real_trq, eCtrl_get_RefTorque());
-		eCtrl_reset_Torque(ref_now);
-		PMSM_FOC_Set_Torque(0);
-		g_trq_mn.thro_value = 0;
-	}
-	g_trq_mn.thro_prev = thro_curr;
-}
-
-#endif
-void speed_mode_process(void) {
-	float speed_Ref = g_trq_mn.spd_ref;
-	PMSM_FOC_Set_Speed(speed_Ref);
-}
-
-#define THRO_REF_LP_CEOF 0.2f
-
-void throttle_process(u8 run_mode, float f_throttle) {
-	if (run_mode == CTRL_MODE_TRQ) {
-		float thro_value = throttle_ration(f_throttle);
-		g_trq_mn.thro_value = LowPass_Filter(g_trq_mn.thro_value, thro_value, THRO_REF_LP_CEOF);
-		if ((g_trq_mn.count++ % 20) == 0) {
-			torque_mode_process();
-		}
-	
}else if (run_mode == CTRL_MODE_SPD) {
-		float spd_ref = throttle_to_speed(f_throttle);
-		g_trq_mn.spd_ref = LowPass_Filter(g_trq_mn.spd_ref, spd_ref, THRO_REF_LP_CEOF);
-		if ((g_trq_mn.count++ % 20) == 0) {
-			speed_mode_process();
-		}
-	}else if (run_mode == CTRL_MODE_CURRENT_BRK) {
-		eCtrl_reset_Torque(0);
-		if (eCtrl_get_FinalCurrent() < 0.0001f && PMSM_FOC_GetSpeed() < CONFIG_MIN_RPM_EXIT_EBRAKE) {
-			eCtrl_enable_eBrake(false);	
-		}
-		if (!mc_throttle_released() || (mc_throttle_released() && (PMSM_FOC_GetSpeed() == 0.0f))) {
-			eCtrl_enable_eBrake(false);
-		}
-	}
-}
-
-/*
-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 = 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 = 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))) {
-			eCtrl_enable_eBrake(false);
-		}
-	}
-} */
-
+#include "foc/core/torque.h"
+#include "foc/foc_config.h"
+#include "foc/motor/motor.h"
+#include "foc/core/e_ctrl.h"
+#include "foc/core/PMSM_FOC_Core.h"
+#include "app/nv_storage.h"
+#include "libs/logger.h"
+#include "prot/can_foc_msg.h"
+
+/*
+通过查表获取对应扭矩和速度时的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();
+	torque_reset();
+}
+
+void torque_reset(void) {
+	memset(&g_trq_mn, 0, sizeof(g_trq_mn));
+}
+
+void torque_get_idq(float torque, float rpm, DQ_t *dq_out) {
+	if ((_trq_tbl == NULL) || (torque < 0 || rpm < 0)) {
+		dq_out->d = 0;
+		dq_out->q = torque;
+		return;
+	}
+	int trq_idx = (int)torque / TBL_TRQ_INTVAL;
+	int rpm_idx = (int)rpm / TBL_SPD_INTVAL;
+	if (trq_idx >= MAX_TRQ_POINTS) {
+		trq_idx = MAX_TRQ_POINTS -1;
+	}
+	if (rpm_idx >= MAX_SPD_POINTS) {
+		rpm_idx = MAX_SPD_POINTS -1;
+	}
+	s16 d = _trq_tbl->dq[trq_idx][rpm_idx].d;
+	s16 q = _trq_tbl->dq[trq_idx][rpm_idx].q;
+	if (trq_idx < MAX_TRQ_POINTS - 1) {
+		trq_idx += 1;
+	}
+	if (rpm_idx < MAX_SPD_POINTS - 1) {
+		rpm_idx += 1;
+	}
+	s16 d_delta = _trq_tbl->dq[trq_idx][rpm_idx].d - d;
+	s16 q_delta = _trq_tbl->dq[trq_idx][rpm_idx].q - q;
+	float comp_ceof = 0.5f * ((torque - torque/TBL_TRQ_INTVAL*TBL_TRQ_INTVAL)/(float)TBL_TRQ_INTVAL + (rpm - rpm/TBL_SPD_INTVAL*TBL_SPD_INTVAL)/(float)TBL_SPD_INTVAL);
+
+	dq_out->d = d + d_delta * comp_ceof;
+	dq_out->q = q + q_delta * comp_ceof;
+
+}
+
+/* 获取油门开度 */
+static float throttle_ration(float f_throttle) {
+	if (f_throttle <= nv_get_foc_params()->n_minThroVol) {
+		return 0;
+	}
+	float delta = f_throttle - (nv_get_foc_params()->n_minThroVol);
+
+	int ration = (delta * 1000.0f) / (nv_get_foc_params()->n_maxThroVol - nv_get_foc_params()->n_minThroVol);
+	
+	return ((float)ration)/1000.0f;
+}
+
+float throttle_to_speed(float f_throttle) {
+	return (PMSM_FOC_GetSpeedLimit() * throttle_ration(f_throttle));
+}
+
+float throttle_to_torque(float f_throttle) {
+	return PMSM_FOC_GetTorqueLimit() * throttle_ration(f_throttle);
+}
+
+#define REAL_DQ_CEOF 1.1f
+
+#if 1
+void torque_mode_process(void) {
+	float ref_trq = PMSM_FOC_GetTorqueLimit() * g_trq_mn.thro_value;
+//	float pre_trq = g_trq_mn.torque_prev;
+	if ((mc_throttle_released()) && eCtrl_enable_eBrake(true)) {
+		g_trq_mn.thro_value = 0;
+		g_trq_mn.torque_prev = 0;
+		g_trq_mn.torque_base = 0;
+		return;
+	}
+	
+	if (!mc_throttle_released()) {
+		if (PMSM_FOC_GetSpeed() <= 10.0f) {
+			ref_trq  = MAX(eCtrl_get_FinalTorque() * REAL_DQ_CEOF, ref_trq );
+			g_trq_mn.torque_base = ref_trq;
+		}
+		PMSM_FOC_Set_Torque(ref_trq );
+		g_trq_mn.torque_prev = ref_trq;
+	}else if (mc_throttle_released() && eCtrl_get_FinalTorque() != 0){
+		float real_trq = PMSM_FOC_Get_Real_Torque() * 0.9f;
+		float ref_now = min(real_trq, eCtrl_get_RefTorque());
+		eCtrl_reset_Torque(ref_now);
+		PMSM_FOC_Set_Torque(0);
+		g_trq_mn.thro_value = 0;
+		g_trq_mn.torque_prev = 0;
+		g_trq_mn.torque_base = 0;
+	}
+}
+#else
+void torque_mode_process(void) {
+	float thro_curr = g_trq_mn.thro_value;
+	float thro_prev = g_trq_mn.thro_prev;
+	float trq_ref = PMSM_FOC_GetTorqueLimit() * thro_curr;
+
+	if ((thro_curr == 0.0f) && eCtrl_enable_eBrake(true)) {
+		g_trq_mn.thro_value = 0;
+		g_trq_mn.accl = false;
+		g_trq_mn.thro_prev = 0.0f;
+		return;
+	}
+	float real_trq = eCtrl_get_FinalTorque() * REAL_DQ_CEOF;
+	if (trq_ref > 0) {
+		if (PMSM_FOC_GetSpeed() <= 10.0f) {
+			g_trq_mn.accl_ref = MAX(real_trq, trq_ref); //不能小于autohold产生的扭矩
+			trq_ref = g_trq_mn.accl_ref;
+		}else {
+			if (thro_curr > thro_prev) {
+				if (!g_trq_mn.accl){
+					g_trq_mn.accl_ref = eCtrl_get_RefTorque();
+				}
+				if (g_trq_mn.accl_ref > PMSM_FOC_GetTorqueLimit()) {
+					g_trq_mn.accl_ref = PMSM_FOC_GetTorqueLimit();
+				}
+				trq_ref = g_trq_mn.accl_ref + thro_curr * (PMSM_FOC_GetTorqueLimit() - g_trq_mn.accl_ref);
+				g_trq_mn.accl = true;
+			}else if (thro_curr < thro_prev){
+				if (g_trq_mn.accl) {
+					g_trq_mn.accl_ref = min(real_trq, eCtrl_get_RefTorque());
+					eCtrl_reset_Torque(g_trq_mn.accl_ref);
+				}
+				trq_ref = thro_curr * g_trq_mn.accl_ref;
+				g_trq_mn.accl = false;
+			}else {
+				if (g_trq_mn.accl) {
+					trq_ref = g_trq_mn.accl_ref + thro_curr * (PMSM_FOC_GetTorqueLimit() - g_trq_mn.accl_ref);
+				}else {
+					trq_ref = thro_curr * g_trq_mn.accl_ref;
+				}
+			}
+		}
+		PMSM_FOC_Set_Torque(trq_ref);
+	}
+	else if (eCtrl_get_FinalTorque() != 0){
+		float ref_now = min(real_trq, eCtrl_get_RefTorque());
+		eCtrl_reset_Torque(ref_now);
+		PMSM_FOC_Set_Torque(0);
+		g_trq_mn.thro_value = 0;
+	}
+	g_trq_mn.thro_prev = thro_curr;
+}
+
+#endif
+void speed_mode_process(void) {
+	float speed_Ref = g_trq_mn.spd_ref;
+	PMSM_FOC_Set_Speed(speed_Ref);
+}
+
+#define THRO_REF_LP_CEOF 0.2f
+
+void throttle_process(u8 run_mode, float f_throttle) {
+	if (run_mode == CTRL_MODE_TRQ) {
+		float thro_value = throttle_ration(f_throttle);
+		g_trq_mn.thro_value = LowPass_Filter(g_trq_mn.thro_value, thro_value, THRO_REF_LP_CEOF);
+		if ((g_trq_mn.count++ % 20) == 0) {
+			torque_mode_process();
+		}
+	
+}else if (run_mode == CTRL_MODE_SPD) {
+		float spd_ref = throttle_to_speed(f_throttle);
+		g_trq_mn.spd_ref = LowPass_Filter(g_trq_mn.spd_ref, spd_ref, THRO_REF_LP_CEOF);
+		if ((g_trq_mn.count++ % 20) == 0) {
+			speed_mode_process();
+		}
+	}else if (run_mode == CTRL_MODE_CURRENT_BRK) {
+		eCtrl_reset_Torque(0);
+		if (eCtrl_get_FinalCurrent() < 0.0001f && PMSM_FOC_GetSpeed() < CONFIG_MIN_RPM_EXIT_EBRAKE) {
+			eCtrl_enable_eBrake(false);	
+		}
+		if (!mc_throttle_released() || (mc_throttle_released() && (PMSM_FOC_GetSpeed() == 0.0f))) {
+			eCtrl_enable_eBrake(false);
+		}
+	}
+}
+
+/*
+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 = 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 = 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))) {
+			eCtrl_enable_eBrake(false);
+		}
+	}
+} */
+

Applications/foc/core/torque.h

@@ -6,7 +6,7 @@ typedef struct {
 	float thro_prev;
 	float speed_prev;
 	float torque_prev;
-	float accl_ref;
+	float torque_base;
 	bool  accl;
 	float thro_value; //油门开度百分比
 	float spd_ref;

Applications/foc/motor/motor.c

@@ -558,34 +558,7 @@ static bool mc_can_stop_foc(void) {
 	return false;
 }
 #endif
-#if 0
-static bool mc_run_stall_process(u8 run_mode) {
-	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !PMSM_FOC_AutoHoldding()) {
-		//堵转判断
-		if (motor.b_runStall) {
-			if (!mc_throttle_released()) {
-				return true;
-			}
-			motor.runStall_time = 0;
-			motor.b_runStall = false; //转把释放，清除堵转标志
-		}else if ((ABS(PMSM_FOC_GetSpeed()) < 1.0f) && (PMSM_FOC_Get()->out.s_RealIdq.q >= CONFIG_STALL_MAX_CURRENT)){
-			if (motor.runStall_time == 0) {
-				motor.runStall_time = get_tick_ms();
-			}else {
-				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);
-					return true;
-				}
-			}
-		}else {
-			motor.runStall_time = 0;
-		}
-	}
-	return false;
-}
-#else
+
 static bool mc_run_stall_process(u8 run_mode) {
 	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !PMSM_FOC_AutoHoldding()) {
 		//堵转判断
@@ -619,7 +592,6 @@ static bool mc_run_stall_process(u8 run_mode) {
 	return false;
 }
 
-#endif
 static void _autohold_beep_timer_handler(shark_timer_t *t) {
 	gpio_beep(60);
 }
@@ -667,17 +639,6 @@ 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;
 	}

Applications/prot/can_foc_msg.c

@@ -120,10 +120,24 @@ void can_report_ext_status(u8 can) {
 }
 
 void can_report_plot_values(u8 can) {
-	u8 data[10];
+    s16 trq_ref = eCtrl_get_RefTorque() * 10;
+	s16 real_ref = PMSM_FOC_Get_Real_Torque() * 10;
+	can_plot2(trq_ref, real_ref);
+}
+
+void can_plot1(s16 v1) {
+	u8 data[4];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Plot));
-	encode_float(data + 2, eCtrl_get_RefTorque());
-	encode_float(data + 6, 0);//PMSM_FOC_Get()->in.s_targetTorque);
-	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
+	encode_s16(data + 2, v1);
+	can_send_message(get_indicator_can_id(0x45), data, sizeof(data), 0);
 }
 
+void can_plot2(s16 v1, s16 v2) {
+	u8 data[6];
+	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Plot));
+	encode_s16(data + 2, v1);
+	encode_s16(data + 4, v2);
+	can_send_message(get_indicator_can_id(0x45), data, sizeof(data), 0);
+}
+
+

Applications/prot/can_foc_msg.h

@@ -13,6 +13,7 @@ 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);
-
+void can_plot1(s16 v1);
+void can_plot2(s16 v1, s16 v2);
 #endif	/*_Can_Foc_Msg_H__ */