加入软件设置油门开度的功能，模拟转把功能

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

Applications/foc/commands.c

@@ -292,6 +292,12 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			}
 			break;
 		}
+		case Foc_Set_Thro_Ration:
+		{
+			u8 r = decode_u8(command->data);
+			mc_set_throttle_r(r);
+			break;
+		}
 		case Foc_Lock_Motor:
 		{
 			u8 lock = decode_u8((u8 *)command->data);

Applications/foc/commands.h

@@ -31,6 +31,7 @@ typedef enum {
 	Foc_Get_Pid,
 	Foc_Fan_Duty, //57
 	Foc_Get_Gear_Limit,
+	Foc_Set_Thro_Ration,
 	Foc_Hall_Phase_Cali_Result = 160,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,

Applications/foc/core/torque.c

@@ -72,10 +72,14 @@ float throttle_to_torque(float f_throttle) {
 	return PMSM_FOC_GetTorqueLimit() * throttle_ration(f_throttle);
 }
 
+/* 软件设置油门开度，调试使用 */
+void throttle_set_ration(float r) {
+	g_trq_mn.thro_value = r;
+}
+
 #define REAL_DQ_CEOF 0.9f
 #define FINAL_DQ_CEFO 1.1F
-#define START_RPM_FOR_AUDOHOLD 10.0F
-#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)) {
@@ -86,7 +90,7 @@ void torque_mode_process(void) {
 	}
 	
 	if (!mc_throttle_released()) {
-		if (PMSM_FOC_GetSpeed() <= START_RPM_FOR_AUDOHOLD) {
+		if (PMSM_FOC_GetSpeed() <= CONFIG_ZERO_SPEED_RPM) {
 			ref_trq  = MAX(eCtrl_get_FinalTorque() * FINAL_DQ_CEFO, ref_trq );
 			g_trq_mn.torque_base = ref_trq;
 		}
@@ -102,60 +106,7 @@ void torque_mode_process(void) {
 		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);
@@ -188,29 +139,3 @@ void throttle_process(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 = 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

@@ -16,6 +16,8 @@ void torque_init(void);
 void torque_reset(void);
 void torque_get_idq(float torque, float rpm, DQ_t *dq_out);
 void throttle_process(u8 run_mode, float f_throttle);
+void throttle_set_ration(float r);
+void torque_mode_process(void);
 
 #endif /*_TORQUE_LUT_H__ */
 

Applications/foc/motor/motor.c

@@ -405,9 +405,14 @@ bool mc_throttle_epm_move(EPM_Dir_t dir) {
 	return mc_start_epm_move(dir, false);
 }
 
-void mc_set_spd_torque(s32 target) {
-	motor.b_ignor_throttle = true;
-	motor.s_targetFix = target;
+void mc_set_throttle_r(u8 r) {
+	motor.u_throttle_ration = r;
+
+	if (r > 0) {
+		motor.b_ignor_throttle = true;
+	}else {
+		motor.b_ignor_throttle = false;		
+	}
 }
 
 void mc_use_throttle(void) {
@@ -597,6 +602,9 @@ u32 mc_get_critical_error(void) {
 }
 
 bool mc_throttle_released(void) {
+	if (motor.b_ignor_throttle) {
+		return motor.u_throttle_ration == 0;
+	}
 	return get_throttle_float() <= nv_get_foc_params()->n_minThroVol;
 }
 
@@ -884,7 +892,13 @@ void Sched_MC_mTask(void) {
 			if ((runMode == CTRL_MODE_SPD) && mc_is_epm()) {
 				mc_process_throttle_epm();
 			}else {
-				throttle_process(runMode, get_throttle_float());
+				if (motor.b_ignor_throttle) { //使用软件设置的油门开度
+					float r = (float)motor.u_throttle_ration/100.0f;
+					throttle_set_ration(r);
+					torque_mode_process();
+				}else {
+					throttle_process(runMode, get_throttle_float());
+				}
 			}
 			PMSM_FOC_Slow_Task();
 		}

Applications/foc/motor/motor.h

@@ -28,8 +28,7 @@ typedef struct {
 	bool   b_epm_cmd_move;
 	u32    runStall_time;
 	float  runStall_pos;
-	s16    s_testAngle;
-	s32    s_targetFix;
+	u8     u_throttle_ration;
 	s8     s_direction;
 	u32    n_brake_errors;
 	u32    n_CritiCalErrMask;
@@ -51,7 +50,7 @@ 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_set_throttle_r(u8 r);
 void mc_use_throttle(void);
 bool mc_current_sensor_calibrate(float current);
 bool mc_encoder_zero_calibrate(s16 vd);