1. 解决定速巡航飞车问题

2. 解决转把归零，切定速巡航模式打齿问题
3. 解决定速巡航加速过程无法取消定速巡航的问题

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

Applications/app/app.c

@@ -128,26 +128,25 @@ static u32 _app_report_task(void *p) {
 		//sys_debug("rst 0x%x\n", get_mcu_reset_source());
 		//sys_debug("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
 		//sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
-		sys_debug("FOC time err %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error);
-		sys_debug("acc vol %d, bid %d\n", get_acc_vol(), gpio_board_id());
+		//sys_debug("FOC time err %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error);
+		//sys_debug("acc vol %d, bid %d\n", get_acc_vol(), gpio_board_id());
 		//sys_debug("throttle %f\n", get_throttle_float());
 		//sys_debug("ADC Vref %f, %f\n", get_adc_vref(), adc_5vref_compesion());
 		//sys_debug("dead time %d\n", get_deadtime());
 		//sys_debug("Sensorless err %d\n", foc_observer_enc_errcount());
 		//thro_torque_log();
 		//sys_debug("_>%f, %f, %f\n", ladrc_observer_get()->ld, ladrc_observer_get()->lq, ladrc_observer_get()->poles);
-		encoder_log();
+		//encoder_log();
 		motor_debug();
 		//sample_log();
 		PMSM_FOC_LogDebug();
 		//F_debug();
 		//eCtrl_debug_log();
 		//err_code_log();
-		sys_debug("limit %f %f\n", PMSM_FOC_Get()->rtLim.torqueLimRamp.target, PMSM_FOC_Get()->rtLim.torqueLimRamp.interpolation);
 	}
 	return 200;
 }
-int plot_type = 0;
+int plot_type = 10;
 static void plot_smo_angle(void) {
 	float smo_angle = foc_observer_sensorless_angle();
 	float delta = smo_angle - PMSM_FOC_Get()->in.s_motAngle;

Applications/foc/commands.c

@@ -72,9 +72,11 @@ static void process_ext_command(foc_cmd_body_t *command) {
 		u8 b1 = decode_u8((u8 *)command->data + 1);
 		u8 cruise = decode_8bits(b1, 0, 1);
 		if (cruise == 2) {
-			PMSM_FOC_EnableCruise(true);
+			mc_enable_cruise(true);
+			sys_debug("cruise enable: %d\n", mc_is_cruise_enabled());
 		}else if (cruise == 1) {
-			PMSM_FOC_EnableCruise(false);
+			mc_enable_cruise(false);
+			sys_debug("cruise disable: %d\n", mc_is_cruise_enabled());
 		}
 
 		u8 epm = decode_8bits(b0, 2, 3);
@@ -97,7 +99,7 @@ static void process_ext_command(foc_cmd_body_t *command) {
 
 		u16 cruise_spd = decode_u16((u8 *)command->data + 3);
 		if ((cruise_spd > 0) && (cruise_spd != 0xFFFF)) {
-			PMSM_FOC_Set_CruiseSpeed((float)cruise_spd);
+			mc_set_cruise_speed(true, (float)cruise_spd);
 		}
 		u8 response[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 		response[0] &= 0xFC;
@@ -105,7 +107,7 @@ static void process_ext_command(foc_cmd_body_t *command) {
 		response[0] |= (mc_get_gear() << 5);
 		
 		response[1] &= 0xC0;
-		response[1] |= (PMSM_FOC_Is_CruiseEnabled()?2:1);
+		response[1] |= (mc_is_cruise_enabled()?2:1);
 		response[1] |= (mc_is_epm()?1:2) << 2;
 		response[1] |= m_4896<<4;
 

Applications/foc/core/PMSM_FOC_Core.c

@@ -511,6 +511,7 @@ void PMSM_FOC_Schedule(void) {
 void PMSM_FOC_LogDebug(void) {
 	sys_debug("DC curr %f --- %f, - %f\n", gFoc_Ctrl.out.s_CalciDC, gFoc_Ctrl.out.s_CalciDC2, gFoc_Ctrl.hwLim.s_iDCMax);
 	sys_debug("%s\n", gFoc_Ctrl.out.empty_load?"NoLoad Running":"Load Runing");
+	sys_debug("cruise vel: %d\n", (s16)gFoc_Ctrl.in.s_cruiseRPM);
 }
 
 /*called in media task */
@@ -542,7 +543,6 @@ u8 PMSM_FOC_CtrlMode(void) {
 #endif
 		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
 #ifdef CONFIG_SPEED_LADRC
-			//ladrc_reset(&gFoc_Ctrl.vel_adrc, gFoc_Ctrl.in.s_motVelocity, gFoc_Ctrl.in.s_targetTorque);
 			ladrc_copy(&gFoc_Ctrl.vel_adrc, &gFoc_Ctrl.vel_lim_adrc);
 #else
 			float target_troque = gFoc_Ctrl.in.s_targetTorque;
@@ -557,16 +557,16 @@ u8 PMSM_FOC_CtrlMode(void) {
 #else
 			PI_Controller_Reset(&gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
 #endif
-		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE)) {
-#if 0
-			float real_trq = PMSM_FOC_Get_Real_dqVector() * 0.9f;
-			eCtrl_reset_Current(min(real_trq, gFoc_Ctrl.in.s_targetTorque));
-			eCtrl_set_TgtCurrent(-PMSM_FOC_GetEbrkTorque());
-#else
+		}else if ((preMode != gFoc_Ctrl.out.n_RunMode) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE)) {
 			eCtrl_reset_Torque(gFoc_Ctrl.in.s_targetTorque);
-			eCtrl_set_TgtTorque(-PMSM_FOC_GetEbrkTorque());
+			eCtrl_set_TgtTorque(motor_get_ebreak_toruqe(gFoc_Ctrl.in.s_motVelocity));
+		}else if ((preMode == CTRL_MODE_EBRAKE) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
+#ifdef CONFIG_SPEED_LADRC
+			ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, F_get_air());
+#else
+			PI_Controller_Reset(&gFoc_Ctrl.pi_torque, F_get_air());
 #endif
-		}	
+		}
 	}
 	
 	return gFoc_Ctrl.out.n_RunMode;
@@ -603,7 +603,6 @@ static void crosszero_step_towards(float *value, float target) {
 		}
 	}
 	if (!cross_zero) {
-		//*value = target;
 		step_towards(value, target, 2.0f);
 	}
 }
@@ -628,7 +627,7 @@ static __INLINE void PMSM_FOC_VelCtrl_Decide(void) {
 	if (f_te <= 0.0f) {
 		change_cnt = 0;
 		change_time = 0xFFFFFFFF;
-		gFoc_Ctrl.out.empty_load = false;
+		//gFoc_Ctrl.out.empty_load = false;
 		return;
 	}
 
@@ -636,12 +635,12 @@ static __INLINE void PMSM_FOC_VelCtrl_Decide(void) {
 		/* 误判空转，发现电机给定的N大于空气阻力，说明不是空转 */
 		if (gFoc_Ctrl.out.empty_load) {
 			float f_air = F_get_air();
-			if ((f_accl > 0) && (f_te > (f_air + f_accl))) {
+			if ((f_accl > 1.0f) && (f_te >= (f_air + f_accl))) {
 				change_cnt ++;
 			}else {
 				change_cnt = 0;
 			}
-			if (change_cnt >= 50) {
+			if (change_cnt >= 500) {
 				gFoc_Ctrl.out.empty_load = false;
 #ifdef CONFIG_SPEED_LADRC
 				ladrc_change_b0(&gFoc_Ctrl.vel_lim_adrc, nv_get_foc_params()->f_adrc_vel_lim_B0);
@@ -661,7 +660,7 @@ static __INLINE void PMSM_FOC_VelCtrl_Decide(void) {
 		}
 	}
 
-	if ((f_accl > 200.0f) && (f_accl/f_te > 1.5f )) {
+	if ((f_accl > 200.0f) && (f_accl/f_te > 3.0f )) {
 		change_cnt++;
 	}else if ((F_get_MotAccl() >= 10.0f) && (f_accl/f_te > 1.2f )) {
 		change_cnt = CHANGE_MAX_CNT;
@@ -683,7 +682,7 @@ static __INLINE void PMSM_FOC_VelCtrl_Decide(void) {
 		ladrc_change_b0(&gFoc_Ctrl.vel_adrc, 2500.0f);
 		ladrc_change_K(&gFoc_Ctrl.vel_adrc, 3);
 #endif
-	}else if (!change_done && (change_cnt <= -20)) {
+	}else if (!change_done && (change_cnt <= -200)) {
 		change_done = true;
 		change_cnt = 0;
 		gFoc_Ctrl.out.empty_load = false;
@@ -808,7 +807,8 @@ void PMSM_FOC_idqCalc(void) {
 		float errRef = refSpeed - gFoc_Ctrl.in.s_motVelocity;
 		float maxTrq = PI_Controller_Run(&gFoc_Ctrl.pi_speed, errRef);
 #endif
-		gFoc_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_iDC(maxTrq);
+		maxTrq = PMSM_FOC_Limit_iDC(maxTrq);
+		crosszero_step_towards(&gFoc_Ctrl.in.s_targetTorque, maxTrq);
 	}
 
 	PMSM_FOC_idq_Assign();
@@ -1014,7 +1014,7 @@ bool PMSM_FOC_EnableCruise(bool enable) {
 			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
 			return false;
 		}
-		eRamp_init_target2(&gFoc_Ctrl.in.cruiseRpmRamp, motSpd, CONFIG_CRUISE_RAMP_TIME);
+		eRamp_reset_target(&gFoc_Ctrl.in.cruiseRpmRamp, motSpd);
 		gFoc_Ctrl.in.s_cruiseRPM = motSpd;
 		gFoc_Ctrl.in.b_cruiseEna = enable;
 	}

Applications/foc/core/thro_torque.c

@@ -219,10 +219,15 @@ void thro_torque_process(u8 run_mode, float f_throttle) {
 			PMSM_FOC_Set_Speed(speed_Ref);
 		}
 	}else if (run_mode == CTRL_MODE_EBRAKE) {
-		if (eCtrl_get_FinalTorque() < 0.0001f && PMSM_FOC_GetSpeed() < CONFIG_MIN_RPM_EXIT_EBRAKE) {
+		float vel = PMSM_FOC_GetSpeed();
+		float ebrk_trq = motor_get_ebreak_toruqe(vel);
+		if (ebrk_trq != 0) {
+			eCtrl_set_TgtTorque(ebrk_trq);
+		}
+		if (eCtrl_get_FinalTorque() < 0.0001f && vel < CONFIG_MIN_RPM_EXIT_EBRAKE) {
 			eCtrl_enable_eBrake(false);	
 		}
-		if (!mc_throttle_released() || (mc_throttle_released() && (PMSM_FOC_GetSpeed() == 0.0f))) {
+		if (!mc_throttle_released() || (mc_throttle_released() && (vel == 0.0f))) {
 			eCtrl_enable_eBrake(false);
 		}
 	}

Applications/foc/foc_config.h

@@ -23,7 +23,7 @@
 #define CONFIG_MIN_CRUISE_RPM 	  1000   /* 能启动定速巡航的最小速度 */
 
 #define CONFIG_MIN_RPM_FOR_EBRAKE 800 //进入电流回收的最小转速
-#define CONFIG_MIN_RPM_EXIT_EBRAKE 100 //推出电流回收的最小转速
+#define CONFIG_MIN_RPM_EXIT_EBRAKE 250 //推出电流回收的最小转速
 
 #define CONFIG_IDQ_CTRL_TS FOC_PWM_FS
 #define CONFIG_SPD_CTRL_TS 1000
@@ -38,7 +38,7 @@
 #define CONFIG_eCTRL_Brake_TIME 1500     /* 捏住刹车的时间，超过这个时间启动ebrake，单位 ms */
 #define CONFIG_ACC_TIME 200
 #define CONFIG_DEC_TIME 10
-#define CONFIG_EBRK_RAMP_TIME 500
+#define CONFIG_EBRK_RAMP_TIME 50
 #define CONFIG_AUTOHOLD_DETECT_TIME 3000
 
 #define CONFIG_CRUISE_RAMP_TIME 2000

Applications/foc/motor/motor.c

@@ -562,7 +562,7 @@ void mc_get_running_status(u8 *data) {
 	data[0] = motor.mode;
 	data[0] |= (PMSM_FOC_AutoHoldding()?1:0) << 2;
 	data[0] |= (motor.b_break?1:0) << 3;
-	data[0] |= (PMSM_FOC_Get()->in.b_cruiseEna?1:0) << 4;
+	data[0] |= (motor.b_cruise?1:0) << 4;
 	data[0] |= (motor.b_start?1:0) << 5;
 	data[0] |= (mc_is_epm()?1:0) << 6;
 	data[0] |= (motor.b_lock_motor) << 7; //motor locked
@@ -573,7 +573,7 @@ u16 mc_get_running_status2(void) {
 	data = motor.n_gear;
 	data |= (PMSM_FOC_AutoHoldding()?1:0) << 2;
 	data |= (motor.b_break?1:0) << 3;
-	data |= (PMSM_FOC_Get()->in.b_cruiseEna?1:0) << 4;
+	data |= (motor.b_cruise?1:0) << 4;
 	data |= (motor.b_start?1:0) << 5;
 	data |= (mc_is_epm()?1:0) << 6;
 	data |= (motor.b_lock_motor) << 7; //motor locked
@@ -924,6 +924,7 @@ void MC_Brake_IRQHandler(void) {
 		return;
 	}
 	if (motor.b_break) {
+		mc_enable_cruise(false);
 		PMSM_FOC_Brake(true);
 	}else {
 		PMSM_FOC_Brake(false);
@@ -967,6 +968,9 @@ void MC_Protect_IRQHandler(void){
 }
 
 void motor_debug(void) {
+	if (!mc_unsafe_critical_error()) {
+		return;
+	}
 	sys_debug("err1: %f, %f, %f, %d\n", (float)mc_error.vbus_x10/10.0f, (float)mc_error.id_ref_x10/10.0f, (float)mc_error.iq_ref_x10/10.0f, mc_error.run_mode);
 	sys_debug("err2: %f, %f, %f, %f\n", (float)mc_error.id_x10/10.0f, (float)mc_error.iq_x10/10.0f, (float)mc_error.vd_x10/10.0f, (float)mc_error.vq_x10/10.0f);
 	sys_debug("err3: %f, %d, %d, %d, %d\n", (float)mc_error.ibus_x10/10.0f, mc_error.b_smo_running, mc_error.mos_temp, mc_error.mot_temp, mc_error.enc_error);
@@ -1182,10 +1186,11 @@ static void mc_process_curise(void) {
 			float trq_req = get_user_request_torque();
 			if (trq_req <= motor.cruise_torque * 1.1f) {
 				PMSM_FOC_ResumeCruise(); //重新开始定速巡航，巡航速度还是前一次定速巡航给的速度
-				motor.cruise_time = shark_get_seconds();
+				//motor.cruise_time = shark_get_seconds();
 			}
 		}
 	}else {
+		PMSM_FOC_EnableCruise(false);
 		can_pause_resume = false;
 	}
 }

Applications/foc/motor/motor_param.c

@@ -1,5 +1,6 @@
 #include "bsp/bsp.h"
 #include "foc/motor/motor_param.h"
+#include "foc/core/PMSM_FOC_Core.h"
 #include "math/fast_math.h"
 #include "libs/logger.h"
 
@@ -213,3 +214,18 @@ void motor_mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out) {
 
 #endif
 
+float motor_get_ebreak_toruqe(float rpm) {
+	float max_e_trq = PMSM_FOC_GetEbrkTorque();
+	if (rpm >= 3000) {
+		return -max_e_trq;
+	}else if (rpm >= 2000) {
+		return -max_e_trq;
+	}else if (rpm >= 1000) {
+		return -max_e_trq * 0.75f;
+	}else if (rpm > CONFIG_MIN_RPM_EXIT_EBRAKE) {
+		return -max_e_trq * 0.25f;
+	}
+
+	return 0;
+}
+

Applications/foc/motor/motor_param.h

@@ -24,6 +24,7 @@ typedef torque2dq_t torque_map_t;
 s16 motor_max_torque_for_rpm(s16 rpm);
 void motor_mpta_fw_lookup(float rpm, float torque, DQ_t *dq_out);
 float motor_get_lq_from_iq(s16 iq);
+float motor_get_ebreak_toruqe(float rpm);
 
 #if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_NEW1
 #define MOTOR_R   0.010f
@@ -92,7 +93,8 @@ float motor_get_lq_from_iq(s16 iq);
 #define MOTOR_Lq (0.000125f*0.5f)
 #define MOTOR_Ld (0.000091f*0.5f)
 #define MOTOR_POLES  5
-#define MOTOR_ENC_OFFSET 145.0F
+#define MOTOR_Flux (0.023f)
+#define MOTOR_ENC_OFFSET 0.0F
 #define CONFIG_MAX_MOTOR_TORQUE 200.0F
 
 #define TRQ_PI_KP 0.6F //0.13f

Applications/prot/can_foc_msg.c

@@ -122,7 +122,7 @@ void can_report_ext_status(u8 can) {
 	data[0] |= (mc_is_epm()?1:0) << 6;
 	data[0] |= (mc_is_epm()?1:0) << 7;
 	data[1] = mc_is_start()?0:1;
-	data[1] |= (PMSM_FOC_Is_CruiseEnabled()?1:0) << 3;
+	data[1] |= (mc_is_cruise_enabled()?1:0) << 3;
 	data[1] |= mc_get_gear() << 6;
 	encode_s16(data + 2, ABS((s16)(motor_encoder_get_speed()/4.0f)));
 	float vDC = get_vbus_float();