定速巡航模式下，可以加速

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

Applications/foc/commands.c

@@ -192,7 +192,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Set_Cruise_Mode:
 		{
 			u8 enable = decode_u8(command->data);
-			if (!PMSM_FOC_EnableCruise(enable)) {
+			if (!mc_enable_cruise(enable)) {
 				erroCode = PMSM_FOC_GetErrCode();
 			}
 			

Applications/foc/core/PMSM_FOC_Core.c

@@ -441,9 +441,7 @@ void PMSM_FOC_LogDebug(void) {
 /*called in media task */
 u8 PMSM_FOC_CtrlMode(void) {
 	u8 preMode = gFoc_Ctrl.out.n_RunMode;
-	if (gFoc_Ctrl.in.b_cruiseEna && gFoc_Ctrl.in.s_motRPM < CONFIG_MIN_CRUISE_RPM) {
-		gFoc_Ctrl.in.b_cruiseEna = false;
-	}
+
 	if (!gFoc_Ctrl.in.b_motEnable) {
 		gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
 	}else if (gFoc_Ctrl.in.n_ctlMode == CTRL_MODE_OPEN) {
@@ -849,6 +847,18 @@ bool PMSM_FOC_EnableCruise(bool enable) {
 	return true;
 }
 
+bool PMSM_FOC_PauseCruise(void) {
+	gFoc_Ctrl.in.b_cruiseEna = false;
+	return true;
+}
+
+bool PMSM_FOC_ResumeCruise(void) {
+	gFoc_Ctrl.in.b_cruiseEna = true;
+	eRamp_init_target(&gFoc_Ctrl.in.cruiseRpmRamp, PMSM_FOC_GetSpeed(), CONFIG_ACC_TIME, CONFIG_DEC_TIME);
+	eRamp_set_step_target(&gFoc_Ctrl.in.cruiseRpmRamp, gFoc_Ctrl.in.s_cruiseRPM, CONFIG_eCTRL_STEP_TS);
+	return true;
+}
+
 bool PMSM_FOC_Is_CruiseEnabled(void) {
 	return (gFoc_Ctrl.in.b_cruiseEna && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD));
 }

Applications/foc/core/PMSM_FOC_Core.h

@@ -301,6 +301,8 @@ void PMSM_FOC_Reset_Torque(void);
 void PMSM_FOC_SpeedDirectLimit(float limit);
 bool PMSM_FOC_iDC_is_Limited(void);
 bool PMSM_FOC_Torque_is_Limited(void);
+bool PMSM_FOC_PauseCruise(void);
+bool PMSM_FOC_ResumeCruise(void);
 
 #endif /* _PMSM_FOC_Core_H__ */
 

Applications/foc/core/torque.c

@@ -97,7 +97,7 @@ static float __INLINE unline_func(float x) {
 #define REAL_DQ_CEOF 0.9f
 #define FINAL_DQ_CEFO 1.1F
 
-void request_torque(float thro_ration) {
+static float get_throttle_torque(float trq_ration) {
 	float curr_rpm = PMSM_FOC_GetSpeed();
 	if (curr_rpm == 0) {
 		torque_ctrl.spd_filted = 0;
@@ -107,7 +107,22 @@ void request_torque(float thro_ration) {
 	float torque_map = torque_max_from_gear_rpm();// (float)get_max_torque_for_rpm((s16)torque_ctrl.spd_filted);
 	float torque_lim = PMSM_FOC_GetTorqueLimit();
 	float max_torque = min(torque_map, torque_lim);
-	float ref_torque = max_torque * unline_func(thro_ration);
+
+	return max_torque * unline_func(trq_ration);
+}
+
+float get_thro_request_torque(void) {
+	return get_throttle_torque(torque_ctrl.thro_ration);
+}
+
+void request_torque(float thro_ration) {
+	float curr_rpm = PMSM_FOC_GetSpeed();
+	float ref_torque = get_throttle_torque(thro_ration);
+#ifdef CONFIG_CRUISE_ENABLE_ACCL
+	if (mc_is_cruise_enabled() && mc_throttle_released()) {
+		return;
+	}
+#endif
 	if ((mc_throttle_released()) && eCtrl_enable_eBrake(true)) {
 		return;
 	}
@@ -132,12 +147,13 @@ void request_torque(float thro_ration) {
 }
 
 void request_speed(float thro_ration) {
-	float speed_Ref = PMSM_FOC_GetSpeedLimit() * thro_ration;
-	PMSM_FOC_Set_Speed(speed_Ref);
+	if (!mc_is_cruise_enabled()) {
+		float speed_Ref = PMSM_FOC_GetSpeedLimit() * thro_ration;
+		PMSM_FOC_Set_Speed(speed_Ref);
+	}
 }
 
 #define THRO_REF_LP_CEOF 0.2f
-
 void throttle_process(u8 run_mode, float f_throttle) {
 	if (mc_throttle_released()){
 		torque_ctrl.throttle_value = 0;
@@ -146,7 +162,6 @@ void throttle_process(u8 run_mode, float f_throttle) {
 		LowPass_Filter(torque_ctrl.throttle_value, f_throttle, THRO_REF_LP_CEOF);
 	}
 	torque_ctrl.thro_ration = throttle_ration(f_throttle);
-
 	if (run_mode == CTRL_MODE_TRQ) {
 		request_torque(torque_ctrl.thro_ration);
 	}else if (run_mode == CTRL_MODE_SPD) {

Applications/foc/core/torque.h

@@ -16,6 +16,7 @@ void trq2dq_lookup(int rpm, float torque, DQ_t *dq_out);
 void throttle_process(u8 run_mode, float f_throttle);
 void request_torque(float thro_ration);
 void trq2dq_lookup_init(void);
+float get_thro_request_torque(void);
 
 #endif /*_TORQUE_LUT_H__ */
 

Applications/foc/foc_config.h

@@ -57,6 +57,8 @@
 
 #define CONFIG_TORQUE_MODE_MIN_RPM 600
 
+#define CONFIG_CRUISE_ENABLE_ACCL 1 //定速巡航可以加速，油门回退，继续定速巡航
+
 #define CONFIG_MAX_NEG_TORQUE 0.0F
 #ifdef CONFIG_SPEED_LADRC
 	#define CONFIG_LADRC_Wo  200.0F

Applications/foc/motor/motor.c

@@ -137,11 +137,14 @@ static u32 _self_check_task(void *p) {
 	return 5;
 }
 
-static void mc_detect_vbus_mode(void) {
+static bool mc_detect_vbus_mode(void) {
 #ifdef CONFIG_FORCE_96V_MODE
 	motor.b_is96Mode = true;
+	return false;
 #else
+	bool is_96mode = motor.b_is96Mode;
 	motor.b_is96Mode = get_vbus_int() >= CONFIG_96V_MODE_VOL;
+	return (is_96mode != motor.b_is96Mode);
 #endif
 }
 
@@ -159,6 +162,7 @@ static void _mc_internal_init(u8 mode, bool start) {
 	motor.b_break = false;
 	motor.b_wait_brk_release = false;
 	motor.b_force_run = false;
+	motor.b_cruise = false;
 }
 
 static void mc_gear_vmode_changed(void) {
@@ -332,6 +336,20 @@ u8 mc_get_gear(void) {
 	return motor.n_gear + 1;
 }
 
+bool mc_enable_cruise(bool enable) {
+	if (PMSM_FOC_EnableCruise(enable)) {
+		motor.b_cruise = enable;
+		motor.cruise_time = enable?shark_get_seconds():0;
+		motor.cruise_torque = 0.0f;
+		return true;
+	}
+	return false;
+}
+
+bool mc_is_cruise_enabled(void) {
+	return motor.b_cruise;
+}
+
 bool mc_set_foc_mode(u8 mode) {
 	if (mode == motor.mode) {
 		return true;
@@ -971,6 +989,47 @@ static void mc_process_throttle_epm(void) {
 	}
 }
 
+static bool mc_process_force_running(void) {
+	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
+		if (motor.b_force_run) {
+			if (_force_wait > 0) {
+				--_force_wait;
+			}else {
+				_force_angle += 1.0f;
+				rand_angle(_force_angle);
+				PMSM_FOC_Set_Angle(_force_angle);
+			}
+		}
+		return true;
+	}
+	return false;
+}
+#ifdef CONFIG_CRUISE_ENABLE_ACCL
+static void mc_process_curise(void) {
+	if (motor.b_cruise) {
+		if (PMSM_FOC_GetSpeed() < CONFIG_MIN_CRUISE_RPM) {
+			mc_enable_cruise(false);
+			return;
+		}
+		if (PMSM_FOC_Is_CruiseEnabled()) {
+			if ((shark_get_seconds() - motor.cruise_time >= 3) && motor.cruise_torque == 0.0f) {
+				motor.cruise_torque = PMSM_FOC_Get()->in.s_targetTorque;
+			}else if (motor.cruise_torque > 0.0f){
+				float trq_req = get_thro_request_torque();
+				if (trq_req > motor.cruise_torque * 1.2f) {
+					PMSM_FOC_PauseCruise(); //需要加速,暂停定速巡航
+				}
+			}
+		}else {
+			float trq_req = get_thro_request_torque();
+			if (trq_req <= motor.cruise_torque * 1.1f) {
+				PMSM_FOC_ResumeCruise(); //重新开始定速巡航，巡航速度还是前一次定速巡航给的速度
+				motor.cruise_time = shark_get_seconds();
+			}
+		}
+	}
+}
+#endif
 /*FOC 的部分处理，比如速度环，状态机，转把采集等*/
 measure_time_t g_meas_MCTask;
 #define mc_TaskStart time_measure_start(&g_meas_MCTask)
@@ -980,9 +1039,9 @@ void Sched_MC_mTask(void) {
 
 	adc_vref_filter();
 
-	bool is96v_prev = motor.b_is96Mode;
-	mc_detect_vbus_mode();
-
+#ifdef CONFIG_CRUISE_ENABLE_ACCL
+	mc_process_curise();
+#endif
 	u8 runMode = PMSM_FOC_CtrlMode();
 
 	/*保护功能*/
@@ -990,21 +1049,12 @@ void Sched_MC_mTask(void) {
 	/* 母线电流，实际采集的相电流矢量大小的计算 */
 	PMSM_FOC_Calc_Current();
 
-	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
-		if (motor.b_force_run) {
-			if (_force_wait > 0) {
-				--_force_wait;
-			}else {
-				_force_angle += 1.0f;
-				rand_angle(_force_angle);
-				PMSM_FOC_Set_Angle(_force_angle);
-			}
-		}
+	if (mc_process_force_running()) {
 		mc_TaskEnd;
 		return;
 	}
 
-	if ((is96v_prev != motor.b_is96Mode) || limted) {
+	if (mc_detect_vbus_mode() || limted) {
 		mc_gear_vmode_changed();
 	}
 

Applications/foc/motor/motor.h

@@ -25,6 +25,9 @@ typedef struct {
 	bool   b_auto_hold;
 	bool   b_break;
 	bool   b_epm;
+	bool   b_cruise;
+	u32    cruise_time;
+	float  cruise_torque;
 	EPM_Dir_t epm_dir;
 	bool   b_epm_cmd_move;
 	u32    runStall_time;
@@ -72,6 +75,8 @@ u32 mc_get_critical_error(void);
 void mc_set_fan_duty(u8 duty);
 void mc_force_run_open(s16 vd, s16 vq);
 u16 mc_get_running_status2(void);
+bool mc_enable_cruise(bool enable);
+bool mc_is_cruise_enabled(void);
 
 static __INLINE float motor_encoder_get_angle(void) {
 #ifdef USE_ENCODER_HALL