1. mc_gear_mode_set增加参数，表示是否是从停止状态变为启动状态，这个状态下需要使用挡位的acc时间作为ramp，否则起步加速会很慢

2. 母线限流，不能先reset到当前电流，因为用了前馈，可能会导致扭矩突然下降
3. 挡位设置中，不能达到的速度的扭矩尽量不要设置为0，否则高速降档的时候会有突然减速的情况

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

Applications/foc/core/controller.c

@@ -78,7 +78,6 @@ bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
 	ctrl->target_idq.q = 0;
 	ctrl->target_torque = 0;
 	ctrl->target_torque_raw = 0;
-
 	foc_init(&ctrl->foc);
 	foc_observer_init();
 	ctrl->b_start = start;
@@ -321,15 +320,18 @@ static __INLINE float mot_contrl_dc_curr_limiter(mot_contrl_t *ctrl, float maxTr
 	/* 计算iq的前馈,(母线功率 - d轴功率)/vq
 	 * 加上0.0001f 避免除0
 	*/
+
 	float r = ctrl->dc_curr_filted / (ctrl->dc_curr_calc + 0.0001f) + 0.0001f;
-	r = fclamp(r, 0.5f, 1.5f);
+	r = fclamp(ABS(r), 0.5f, 1.5f);
 	float q_axis_power = dc_lim * get_vbus_float() / r - ctrl->out_idq_filterd.d * ctrl->out_vdq_filterd.d;
-	float iq_ff = q_axis_power / (ctrl->out_vdq_filterd.q + 0.0001f);
-	iq_ff = ABS(iq_ff);
+	float vq = ctrl->out_vdq_filterd.q;
+	if (vq < 1.0f) {
+		vq = 1.0f;
+	}
+	float iq_ff = q_axis_power / vq;
+	iq_ff = fabs(iq_ff);
 	if (iq_ff > maxTrq) {
 		ctrl->dc_lim_t_ff = maxTrq;
-	}else if (iq_ff < 0) {
-		ctrl->dc_lim_t_ff = 0;
 	}else {
 		ctrl->dc_lim_t_ff = iq_ff;
 	}
@@ -365,7 +367,6 @@ static __INLINE float mot_contrl_dc_curr_limiter(mot_contrl_t *ctrl, float maxTr
 	return PI_Controller_Parallel(&ctrl->pi_power, errRef, ctrl->dc_lim_t_ff);
 }
 #endif
-
 static __INLINE float mot_contrl_vel_limiter(mot_contrl_t *ctrl, float maxTrq) {
 	ctrl->pi_vel_lim.max = maxTrq;
 	ctrl->pi_vel_lim.min = 0;
@@ -636,7 +637,10 @@ void mot_contrl_set_dccurr_limit(mot_contrl_t *ctrl, float ibusLimit) {
 		ibusLimit = min(ibusLimit, ctrl->protlim.dc_curr);
 	}
 	ctrl->userlim.dc_curr = ibusLimit;
-	line_ramp_reset(&ctrl->ramp_dc_curr_lim, ctrl->dc_curr_filted);//从当前实际母线电流开始做ramp
+	/*
+	 * 这个地方不能像限制扭矩和速度那样从当前实际母线电流开始做ramp
+     * 因为用了功率环前馈，会导致扭矩突然下降
+	*/
 	line_ramp_set_target(&ctrl->ramp_dc_curr_lim, ctrl->userlim.dc_curr);
 }
 

Applications/foc/motor/motor.c

@@ -223,7 +223,7 @@ static void _led_off_timer_handler(shark_timer_t *t) {
 }
 
 
-static void mc_gear_mode_set(void) {
+static void mc_gear_mode_set(bool set_imd) {
 	gear_t *gears = mc_gear_conf();
 	float max_vel = (float)min(gears->max_speed, motor.u_set.rpm_lim);
 	float max_idc = (float)min(gears->max_idc, motor.u_set.idc_lim);
@@ -235,7 +235,15 @@ static void mc_gear_mode_set(void) {
 		max_idc = min(max_idc, (float)sensorless_gear.max_idc);
 		max_torque = min(max_torque, (float)sensorless_gear.max_torque);
 	}
-
+	if (set_imd) {
+		line_ramp_set_time(&motor.controller.ramp_vel_lim, gears->accl_time);
+		line_ramp_set_time(&motor.controller.ramp_dc_curr_lim, gears->accl_time);
+		line_ramp_set_time(&motor.controller.ramp_torque_lim, gears->accl_time);
+	}else {
+		line_ramp_set_time(&motor.controller.ramp_vel_lim, CONFIG_LIMIT_RAMP_TIME);
+		line_ramp_set_time(&motor.controller.ramp_dc_curr_lim, CONFIG_LIMIT_RAMP_TIME);
+		line_ramp_set_time(&motor.controller.ramp_torque_lim, CONFIG_LIMIT_RAMP_TIME);
+	}
 	mot_contrl_set_vel_limit(&motor.controller, max_vel);
 	mot_contrl_set_dccurr_limit(&motor.controller, max_idc);
 	mot_contrl_set_torque_limit(&motor.controller, max_torque);
@@ -392,7 +400,7 @@ bool mc_start(u8 mode) {
 	mot_contrl_start(&motor.controller, mode);
 	mot_contrl_set_torque_ramp_time(&motor.controller, mc_gear_conf()->zero_accl, mc_conf()->c.thro_dec_time);
 	mc_set_ebrk_level(motor.u_set.ebrk_lvl);
-	mc_gear_mode_set();
+	mc_gear_mode_set(false);
 	cpu_exit_critical(mask);
 
 	pwm_turn_on_low_side();
@@ -481,7 +489,7 @@ bool mc_set_gear(u8 gear) {
 #endif
 		u32 mask = cpu_enter_critical();
 		motor.n_gear = gear;
-		mc_gear_mode_set();
+		mc_gear_mode_set(false);
 		cpu_exit_critical(mask);
 	}
 	return true;
@@ -592,7 +600,7 @@ void mc_set_idc_limit(s16 limit) {
 		return;
 	}
 	motor.u_set.idc_lim = limit;
-	mc_gear_mode_set();
+	mc_gear_mode_set(false);
 }
 
 void mc_set_rpm_limit(s16 limit) {
@@ -600,7 +608,7 @@ void mc_set_rpm_limit(s16 limit) {
 		return;
 	}
 	motor.u_set.rpm_lim = limit;
-	mc_gear_mode_set();
+	mc_gear_mode_set(false);
 }
 
 
@@ -700,7 +708,7 @@ bool mc_start_epm(bool epm) {
 		mot_contrl_request_mode(&motor.controller, CTRL_MODE_TRQ);
 		mot_contrl_set_torque_limit_rttime(&motor.controller, CONFIG_LIMIT_RAMP_TIME);
 		mot_contrl_set_vel_rttime(&motor.controller, CONFIG_CRUISE_RAMP_TIME);
-		mc_gear_mode_set();
+		mc_gear_mode_set(false);
 	}
 	cpu_exit_critical(mask);
 	
@@ -734,7 +742,7 @@ bool mc_start_epm_move(epm_dir_t dir, bool is_command) {
 		
 		if (!mot_contrl_is_start(&motor.controller)) {
 			mot_contrl_start(&motor.controller, motor.mode);
-			mc_gear_mode_set();
+			mc_gear_mode_set(false);
 			pwm_enable_channel();
 		}else if (mot_contrl_is_auto_holdding(&motor.controller)) {
 			mc_auto_hold(false);
@@ -1573,7 +1581,7 @@ static void mc_motor_runstop(void) {
 	if (!mot_contrl_is_start(&motor.controller) && mc_can_restart_foc()) {
 		mask = cpu_enter_critical();
 		mot_contrl_start(&motor.controller, motor.mode);
-		mc_gear_mode_set();
+		mc_gear_mode_set(true);
 		throttle_torque_reset();
 		pwm_enable_channel();
 		g_meas_foc.first = true;
@@ -1661,7 +1669,7 @@ void Sched_MC_mTask(void) {
 	}
 	bool sensor_less = !foc_observer_is_encoder();
 	if (mc_detect_vbus_mode() || (limted == FOC_LIM_CHANGE_L) || (_sensorless_run != sensor_less)) {
-		mc_gear_mode_set();
+		mc_gear_mode_set(false);
 		if (sensor_less && foc_observer_sensorless_stable()) {//unstable 记录在ADC中断处理中
 			if (motor_encoder_may_error() == ENCODER_PWM_ERR) {
 				if (mc_set_critical_error(FOC_CRIT_Encoder_Err)) {
@@ -1682,7 +1690,7 @@ void Sched_MC_mTask(void) {
 	/* 如果取消高温，欠压等限流需要释放转把后才生效，确保不会突然加速 */
 	if (motor.b_limit_pending && mc_throttle_released()) {
 		motor.b_limit_pending = false;
-		mc_gear_mode_set();
+		mc_gear_mode_set(false);
 	}
 	/* 堵转处理 */
 	if (mc_run_stall_process(runMode) || (motor.mode == CTRL_MODE_CURRENT)) {

Applications/foc/motor/throttle.c

@@ -323,7 +323,7 @@ void throttle_set_torque(mot_contrl_t * ctrl, float torque) {
 			}else {
 				float f_now = (float)now_ramp_time;
 				float f_next = (float)next_ramp_time;
-				step_towards(&f_now, f_next, 0.5f);
+				step_towards(&f_now, f_next, 5.0f);
 				mot_contrl_set_torque_acc_time(ctrl ,(u16)f_now);
 			}
 		}

configs/mc124_R1_V2.yml

@@ -1,4 +1,4 @@
-##### 配置文件自动生成，不要手动修改!! 2024/1/3 18:56:20
+##### 配置文件自动生成，不要手动修改!! 2024/1/18 14:54:54
 Version: 1
 CheckCrc: 0
 Motor:
@@ -15,7 +15,7 @@ Motor:
   GearRatio: 6.25
   MaxFwDCurr: 100
   MaxTorque: 350
-  EncOffset: 0
+  EncOffset: -2
 Foc:
   MaxDCVol: 90
   MinDCVol: 40
@@ -65,7 +65,7 @@ Gear:
   MaxTorque: 250
   MaxIdc: 30
   ZeroAccl: 300
-  NormalAccl: 100
+  NormalAccl: 1000
   Torque:
   - 100
   - 100
@@ -81,15 +81,15 @@ Gear:
   MaxTorque: 250
   MaxIdc: 45
   ZeroAccl: 300
-  NormalAccl: 80
+  NormalAccl: 500
   Torque:
   - 100
   - 100
   - 100
   - 80
   - 60
-  - 40
-  - 0
+  - 50
+  - 50
   - 0
   - 0
   - 0
@@ -97,7 +97,7 @@ Gear:
   MaxTorque: 300
   MaxIdc: 60
   ZeroAccl: 500
-  NormalAccl: 50
+  NormalAccl: 250
   Torque:
   - 100
   - 100
@@ -113,7 +113,7 @@ Gear:
   MaxTorque: 300
   MaxIdc: 150
   ZeroAccl: 500
-  NormalAccl: 30
+  NormalAccl: 100
   Torque:
   - 100
   - 100