1. 转把扭矩解析按照加减速的逻辑处理，结合当前的实际扭矩

2. 定速巡航，必须转把归位一次才能通过转把加速

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

Applications/app/app.c

@@ -24,6 +24,7 @@ static u32 _app_plot_task(void *args);
 extern void PMSM_FOC_LogDebug(void);
 extern void err_code_log(void);
 extern void encoder_log(void);
+extern void thro_torque_log(void);
 extern measure_time_t g_meas_hall;
 extern measure_time_t g_meas_foc;
 extern measure_time_t g_meas_MCTask;
@@ -109,6 +110,7 @@ static u32 _app_report_task(void *p) {
 		sys_debug("throttle %f\n", get_throttle_float());
 		sys_debug("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());
 		sys_debug("target current %f\n", PMSM_FOC_Get()->in.s_targetCurrent);
+		thro_torque_log();
 		//sys_debug("fan rpm %d, %d\n", mc_params()->fan[0].rpm, mc_params()->fan[1].rpm);
 		encoder_log();
 		PMSM_FOC_LogDebug();
@@ -116,7 +118,7 @@ static u32 _app_report_task(void *p) {
 	}
 	return 200;
 }
-static int plot_type = 1;
+static int plot_type = 2;
 static void plot_smo_angle(void) {
 	float smo_angle = foc_observer_smo_angle();
 	float delta = smo_angle - PMSM_FOC_Get()->in.s_hallAngle;
@@ -133,7 +135,7 @@ static u32 _app_plot_task(void * args) {
 		can_plot2((s16)(PMSM_FOC_Get()->in.s_targetTorque*100.0f), (s16)PMSM_FOC_GetSpeed());
 		//can_plot3((s16)PMSM_FOC_Get()->vel_lim_adrc.z1, (s16)PMSM_FOC_Get()->vel_lim_adrc.z2, (s16)PMSM_FOC_GetSpeed());
 	}else if (plot_type == 2) {
-		can_plot2(eCtrl_get_RefTorque(), PMSM_FOC_Get_Real_dqVector());
+		can_plot2(eCtrl_get_RefTorque(), eCtrl_get_FinalTorque());
 	}else if (plot_type == 3) {
 		//can_plot2(PMSM_FOC_GetSpeed(), foc_observer_smo_speed());
 		plot_smo_angle();

Applications/foc/core/PMSM_FOC_Core.c

@@ -8,7 +8,7 @@
 #include "foc/motor/current.h"
 #include "foc/motor/motor.h"
 #include "foc/core/svpwm.h"
-#include "foc/core/torque.h"
+#include "foc/core/thro_torque.h"
 #include "foc/core/foc_observer.h"
 #include "foc/samples.h"
 #include "foc/limit.h"
@@ -988,15 +988,15 @@ void PMSM_FOC_AutoHold(bool lock) {
 			//解锁后为了防止倒溜，需要把当前
 			if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
 #ifdef CONFIG_SPEED_LADRC
-				ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, 0, gFoc_Ctrl.in.s_targetTorque);
+				ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, 0, gFoc_Ctrl.in.s_targetTorque * 1.1f);
 #else
-				PI_Controller_Reset(&gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
+				PI_Controller_Reset(&gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque * 1.1f);
 #endif
 			}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD) {
 #ifdef CONFIG_SPEED_LADRC
-				ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, gFoc_Ctrl.in.s_targetTorque);
+				ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, gFoc_Ctrl.in.s_targetTorque * 1.1f);
 #else
-				PI_Controller_Reset(&gFoc_Ctrl.pi_speed, gFoc_Ctrl.in.s_targetTorque);
+				PI_Controller_Reset(&gFoc_Ctrl.pi_speed, gFoc_Ctrl.in.s_targetTorque * 1.1f);
 #endif
 			}
 			eCtrl_reset_Torque(gFoc_Ctrl.in.s_targetTorque);

Applications/foc/core/thro_torque.c

@@ -0,0 +1,188 @@
+#include "foc/core/thro_torque.h"
+#include "foc/foc_config.h"
+#include "foc/motor/motor.h"
+#include "foc/motor/motor_param.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"
+
+static thro_torque_t _torque;
+static motor_map_t gear_torques[5][5] = {
+	[0] = {{500,  100}, {1200, 100}, {1700, 80}, {2200, 75}, {2800, 50},},
+	[1] = {{800,  130}, {1600, 120}, {2400, 110}, {3200, 80}, {4300, 60},},
+	[2] = {{1200, 150}, {2200, 140}, {3200, 120}, {4200, 80}, {5300, 70},},
+	[3] = {{3000, 200}, {4740, 150}, {5050, 110}, {5200, 85}, {5300, 70},},
+	[4] = {{4500, 200}, {4740, 150}, {5050, 110}, {5200, 85}, {5300, 70},},
+};
+
+void thro_torque_reset(void) {
+	_torque.accl = false;
+	_torque.thro_filted = 0.0f;
+	_torque.thro_ration = _torque.thro_ration_last = 0.0f;
+	_torque.torque_req = _torque.torque_real = 0.0f;
+}
+
+void thro_torque_init(void) {
+	thro_torque_reset();
+	trq2dq_lookup_init();
+	_torque.spd_filted = 0.0f;
+}
+
+static float thro_torque_gear_map(void) {
+	u8 gear = mc_get_gear();
+	if (gear > 4) {
+		gear = 0;
+	}
+	motor_map_t *map = &gear_torques[gear][0];
+	s16 rpm = (s16)_torque.spd_filted;
+	if (rpm <= map[0].rpm) {
+		return (float)map[0].torque;
+	}
+	int map_size = 5;
+	for (int i = 1; i < map_size; i++) {
+		if (rpm <= map[i].rpm) { //线性插值
+			float trq1 = map[i-1].torque;
+			float min_rpm = map[i-1].rpm;
+			float trq2 = map[i].torque;
+			float max_rpm = map[i].rpm;
+			if (trq1 > trq2) {
+				return f_map_inv((float)rpm, min_rpm, max_rpm, trq2, trq1);
+			}else {
+				return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
+			}
+		}
+	}
+	return (float)map[map_size-1].torque;	
+}
+
+/* 获取油门开度 */
+static float thro_ration(float f_throttle) {
+	if (f_throttle <= nv_get_foc_params()->n_startThroVol) {
+		return 0;
+	}
+	float delta = f_throttle - (nv_get_foc_params()->n_startThroVol);
+	int ration = (delta * 100.0f) / (nv_get_foc_params()->n_endThroVol - nv_get_foc_params()->n_startThroVol);	
+	return ((float)ration)/100.0f;
+}
+
+float thro_ration_to_voltage(float r) {
+	if (r == 0) {
+		return 0;
+	}
+	float vol = nv_get_foc_params()->n_startThroVol + r * (nv_get_foc_params()->n_endThroVol - nv_get_foc_params()->n_startThroVol);
+	return fclamp(vol, 0, nv_get_foc_params()->n_endThroVol);
+}
+
+static float thro_torque_for_accelerate(void) {
+	float max_torque = thro_torque_gear_map();
+	float acc_r = 1.0f;
+	if (_torque.thro_ration_last < 1.0f) {
+		acc_r = (_torque.thro_ration - _torque.thro_ration_last)/ (1.0f - _torque.thro_ration_last);
+	}
+	acc_r = fclamp(acc_r, 0, 1.0f);
+	float acc_torque = _torque.torque_real + acc_r * (max_torque - _torque.torque_real);
+
+	return acc_torque;
+}
+
+static float thro_torque_for_decelerate(void) {
+	float dec_r = _torque.thro_ration / _torque.thro_ration_last;
+	dec_r = fclamp(dec_r, 0.0f, 1.0f);
+	return dec_r * _torque.torque_real;
+}
+
+static void thro_torque_request(void) {
+#ifdef CONFIG_CRUISE_ENABLE_ACCL
+	if (mc_is_cruise_enabled() && mc_throttle_released()) {
+		return;
+	}
+#endif
+	if (mc_throttle_released() && eCtrl_enable_eBrake(true)) {
+		return;
+	}
+	if (!mc_throttle_released()) {
+		if (_torque.accl) { //加速需求
+			float ref_torque = thro_torque_for_accelerate();
+			float curr_rpm = PMSM_FOC_GetSpeed();
+			if (curr_rpm <= CONFIG_ZERO_SPEED_RPM) {//从静止开始加速
+				float hold_torque = eCtrl_get_FinalTorque();
+				ref_torque	= MAX(hold_torque, ref_torque);
+			}
+			_torque.torque_req = ref_torque;
+		}else {
+			float ref_torque = thro_torque_for_decelerate();
+			_torque.torque_req = ref_torque;
+		}
+		PMSM_FOC_Set_Torque(_torque.torque_req);
+	}else if (mc_throttle_released() && eCtrl_get_FinalTorque() != 0){
+		_torque.torque_req = 0.0f;
+		PMSM_FOC_Set_Torque(_torque.torque_req);
+	}
+}
+
+#define THRO_REF_LP_CEOF 0.2f
+static void thro_torque_filter(float f_throttle) {
+	if (mc_throttle_released()){
+		thro_torque_reset();
+	}else {
+		LowPass_Filter(_torque.thro_filted, f_throttle, THRO_REF_LP_CEOF);
+	}
+	float curr_rpm = PMSM_FOC_GetSpeed();
+	if (curr_rpm == 0) {
+		_torque.spd_filted = 0;
+	}else {
+		LowPass_Filter(_torque.spd_filted, curr_rpm, 0.01f);
+	}
+}
+
+void thro_torque_process(u8 run_mode, float f_throttle) {
+
+	thro_torque_filter(f_throttle);
+
+	float thro_r = thro_ration(_torque.thro_filted);
+	if (thro_r > _torque.thro_ration) {
+		if (!_torque.accl) {
+			_torque.thro_ration_last = _torque.thro_ration;
+			_torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
+		}
+		_torque.accl = true;
+	}else if (thro_r < _torque.thro_ration) {
+		if (_torque.accl) {
+			_torque.thro_ration_last = _torque.thro_ration + 1e-10f; // add 1e-10f avoid divide zero
+			_torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
+		}
+		_torque.accl = false;
+	}
+	_torque.thro_ration = thro_r;
+	if (run_mode == CTRL_MODE_TRQ) {
+		thro_torque_request();
+	}else if (run_mode == CTRL_MODE_SPD) {
+		if (!mc_is_cruise_enabled()) {
+			float speed_Ref = PMSM_FOC_GetSpeedLimit() * _torque.thro_ration;
+			PMSM_FOC_Set_Speed(speed_Ref);
+		}
+	}else if (run_mode == CTRL_MODE_EBRAKE) {
+		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);
+		}
+	}
+}
+
+/* 定速巡航需要判断是否需要加速 */
+float get_user_request_torque(void) {
+	if (_torque.accl) {
+		return thro_torque_for_accelerate();
+	}
+	return thro_torque_for_decelerate();
+}
+
+void thro_torque_log(void) {
+	sys_debug("accl %d, real %f, req %f\n", _torque.accl, _torque.torque_real, _torque.torque_req);
+	sys_debug("ration %f - %f - %f\n", _torque.thro_ration, _torque.thro_ration_last, thro_torque_for_accelerate());
+}

Applications/foc/core/thro_torque.h

@@ -0,0 +1,24 @@
+#ifndef _THRO_TORQUE_H__
+#define _THRO_TORQUE_H__
+#include "os/os_types.h"
+#include "foc/core/PMSM_FOC_Core.h"
+typedef struct {
+	bool  accl;
+	float torque_req;
+	float torque_real;
+	float thro_filted;
+	float spd_filted;
+	float thro_ration;
+	float thro_ration_last;
+}thro_torque_t;
+
+void trq2dq_lookup(int rpm, float torque, DQ_t *dq_out);
+void trq2dq_lookup_init(void);
+void thro_torque_reset(void);
+void thro_torque_init(void);
+float thro_ration_to_voltage(float r);
+void thro_torque_process(u8 run_mode, float f_throttle);
+float get_user_request_torque(void);
+
+#endif /* _THRO_TORQUE_H__ */
+

Applications/foc/motor/motor.c

@@ -18,9 +18,9 @@
 #include "foc/samples.h"
 #include "foc/motor/motor_param.h"
 #include "foc/core/foc_observer.h"
-#include "foc/core/torque.h"
+#include "foc/core/thro_torque.h"
 #include "app/nv_storage.h"
-#include "foc/core/torque.h"
+//#include "foc/core/torque.h"
 #include "foc/limit.h"
 
 #ifdef CONFIG_DQ_STEP_RESPONSE
@@ -186,7 +186,7 @@ void mc_init(void) {
 	samples_init();
 	motor_encoder_init();
 	foc_command_init();
-	torque_init();
+	thro_torque_init();
 	mc_detect_vbus_mode();
 	PMSM_FOC_CoreInit();
 	eCtrl_init(nv_get_foc_params()->n_acc_time, nv_get_foc_params()->n_dec_time);
@@ -214,6 +214,9 @@ bool mc_unsafe_critical_error(void) {
 	err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_Encoder_Err)));
 	sys_debug("err=0x%x\n", err);
 #endif
+	if (motor.b_calibrate || motor.b_force_run) {
+		err = err & (~(FOC_Cri_Err_Mask(FOC_CRIT_THRO_Err)));
+	}
 	return (err != 0);
 }
 
@@ -254,8 +257,11 @@ bool mc_start(u8 mode) {
 	pwm_up_enable(false);
 
 	_mc_internal_init(mode, true);
-
+#if 0
 	torque_reset();
+#else
+	thro_torque_reset();
+#endif
 	eCtrl_init(nv_get_foc_params()->n_acc_time, nv_get_foc_params()->n_dec_time);
 	motor_encoder_start(true);
 	PMSM_FOC_Start(mode);
@@ -337,6 +343,9 @@ u8 mc_get_gear(void) {
 }
 
 bool mc_enable_cruise(bool enable) {
+	if (enable == motor.b_cruise) {
+		return true;
+	}
 	if (PMSM_FOC_EnableCruise(enable)) {
 		motor.b_cruise = enable;
 		motor.cruise_time = enable?shark_get_seconds():0;
@@ -480,7 +489,7 @@ bool mc_command_epm_move(EPM_Dir_t dir) {
 bool mc_throttle_epm_move(EPM_Dir_t dir) {
 	return mc_start_epm_move(dir, false);
 }
-extern void sample_test_start(void);
+
 void mc_set_throttle_r(u8 r) {
 	motor.u_throttle_ration = r;
 
@@ -942,7 +951,11 @@ static bool mc_run_stall_process(u8 run_mode) {
 					motor.b_runStall = true;
 					motor.runStall_time = 0;
 					PMSM_FOC_Set_Torque(0);
+#if 0
 					torque_reset();
+#else
+					thro_torque_reset();
+#endif
 					return true;
 				}
 				if (ABS(motor.runStall_pos - motor_encoder_get_position()) >= 0.2f) {
@@ -1020,28 +1033,46 @@ static bool mc_process_force_running(void) {
 }
 #ifdef CONFIG_CRUISE_ENABLE_ACCL
 static void mc_process_curise(void) {
+	static bool can_pause_resume = false;
 	if (motor.b_cruise) {
 		if (PMSM_FOC_GetSpeed() < CONFIG_MIN_CRUISE_RPM) {
 			mc_enable_cruise(false);
 			return;
 		}
+		/* 定速巡航模式下，必须转把归位后才能开始通过拧动转把加速 */
+		if (mc_throttle_released() && !can_pause_resume) {
+			can_pause_resume = true;
+		}
+		if (!can_pause_resume) {
+			return;
+		}
 		if (PMSM_FOC_Is_CruiseEnabled()) {
 			u32 cruise_time = shark_get_seconds() - motor.cruise_time;
 			if ((cruise_time >= 3) && motor.cruise_torque == 0.0f) {
 				motor.cruise_torque = PMSM_FOC_Get()->in.s_targetTorque;
 			}else if ((cruise_time >= 3) && (motor.cruise_torque > 0.0f)){
+#if 0
 				float trq_req = get_thro_request_torque();
+#else
+				float trq_req = get_user_request_torque();
+#endif
 				if (trq_req > motor.cruise_torque * 1.2f) {
 					PMSM_FOC_PauseCruise(); //需要加速,暂停定速巡航
 				}
 			}
 		}else {
+#if 0
 			float trq_req = get_thro_request_torque();
+#else
+			float trq_req = get_user_request_torque();
+#endif
 			if (trq_req <= motor.cruise_torque * 1.1f) {
 				PMSM_FOC_ResumeCruise(); //重新开始定速巡航，巡航速度还是前一次定速巡航给的速度
 				motor.cruise_time = shark_get_seconds();
 			}
 		}
+	}else {
+		can_pause_resume = false;
 	}
 }
 #endif
@@ -1099,7 +1130,11 @@ void Sched_MC_mTask(void) {
 				mask = cpu_enter_critical();
 				PMSM_FOC_Start(motor.mode);
 				mc_gear_vmode_changed();
+				#if 0
 				torque_reset();
+				#else
+				thro_torque_reset();
+				#endif
 				pwm_enable_channel();
 				cpu_exit_critical(mask);
 			}
@@ -1109,12 +1144,21 @@ void Sched_MC_mTask(void) {
 			if ((runMode == CTRL_MODE_SPD) && mc_is_epm()) {
 				mc_process_throttle_epm();
 			}else {
+				#if 0
 				if (motor.b_ignor_throttle) { //使用软件设置的油门开度
 					float r = (float)motor.u_throttle_ration/100.0f;
 					request_torque(r);
 				}else {
 					throttle_process(runMode, get_throttle_float());
 				}
+				#else
+				float thro = get_throttle_float();
+				if (motor.b_ignor_throttle) {
+					float r = (float)motor.u_throttle_ration/100.0f;
+					thro = thro_ration_to_voltage(r);
+				}
+				thro_torque_process(runMode, thro);
+				#endif
 			}
 			PMSM_FOC_Slow_Task();
 		}

Project/MC100.uvoptx

@@ -345,8 +345,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\torque.c</PathWithFileName>
-      <FilenameWithoutPath>torque.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\mc_error.c</PathWithFileName>
+      <FilenameWithoutPath>mc_error.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -357,8 +357,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\mc_error.c</PathWithFileName>
-      <FilenameWithoutPath>mc_error.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\limit.c</PathWithFileName>
+      <FilenameWithoutPath>limit.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -369,8 +369,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\limit.c</PathWithFileName>
-      <FilenameWithoutPath>limit.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\smo_observer.c</PathWithFileName>
+      <FilenameWithoutPath>smo_observer.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -381,8 +381,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\smo_observer.c</PathWithFileName>
-      <FilenameWithoutPath>smo_observer.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\foc_observer.c</PathWithFileName>
+      <FilenameWithoutPath>foc_observer.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -393,8 +393,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\foc_observer.c</PathWithFileName>
-      <FilenameWithoutPath>foc_observer.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\trq2dq_table.c</PathWithFileName>
+      <FilenameWithoutPath>trq2dq_table.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -405,8 +405,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\trq2dq_table.c</PathWithFileName>
-      <FilenameWithoutPath>trq2dq_table.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\adrc.c</PathWithFileName>
+      <FilenameWithoutPath>adrc.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -417,8 +417,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\Applications\foc\core\adrc.c</PathWithFileName>
-      <FilenameWithoutPath>adrc.c</FilenameWithoutPath>
+      <PathWithFileName>..\Applications\foc\core\thro_torque.c</PathWithFileName>
+      <FilenameWithoutPath>thro_torque.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>

Project/MC100.uvprojx

@@ -438,11 +438,6 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\foc\core\PMSM_FOC_Core.c</FilePath>
             </File>
-            <File>
-              <FileName>torque.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\Applications\foc\core\torque.c</FilePath>
-            </File>
             <File>
               <FileName>mc_error.c</FileName>
               <FileType>1</FileType>
@@ -473,6 +468,11 @@
               <FileType>1</FileType>
               <FilePath>..\Applications\foc\core\adrc.c</FilePath>
             </File>
+            <File>
+              <FileName>thro_torque.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\Applications\foc\core\thro_torque.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>