Merge remote-tracking branch 'origin/running_test' into tcs_dev

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

Applications/app/app.c

@@ -147,7 +147,7 @@ static u32 _app_report_task(void *p) {
 		can_report_motparam(mot_params_get_est_flux(), FLUX_TYPE);
 	}
 
-	if (++loop % 10 == 0) {
+	if (++loop % 5 == 0) {
 		//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);
@@ -156,12 +156,12 @@ 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("dead time %d\n", get_deadtime());
-		//thro_torque_log();
+		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();
+		//sample_log();
+		//PMSM_FOC_LogDebug();
 		//F_debug();
 		//eCtrl_debug_log();
 		//sys_debug("enc err %d, %d\n", foc_observer_enc_errcount(), foc_observer_sensorless_stable());

Applications/foc/core/PMSM_FOC_Core.c

@@ -775,15 +775,16 @@ u8 PMSM_FOC_CtrlMode(void) {
 }
 
 #define RAMPE_1 CONFIG_RAMP_FIRST_TARGET
+#define NORMAL_STEP 3.0F
 static void crosszero_step_towards(float *value, float target) {
-	static float no_cro_step = 2.0f;
+	static float no_cro_step = NORMAL_STEP;
 	float v_now = *value;
 	bool cross_zero = false;
 
 	float high_ramp_torque = CONFIG_RAMP_SECOND_TARGET;
 	if (target > 0) {
 		if (v_now < -RAMPE_1) {
-			step_towards(value, -RAMPE_1, 2.0f);
+			step_towards(value, -RAMPE_1, NORMAL_STEP);
 			cross_zero = true;
 		}else if (v_now >= -RAMPE_1 && v_now <= high_ramp_torque) {
 			step_towards(value, target, 0.03f);
@@ -791,7 +792,7 @@ static void crosszero_step_towards(float *value, float target) {
 		}
 	}else if (target == 0) {
 		if (v_now > high_ramp_torque) {
-			step_towards(value, high_ramp_torque, 1.0f);
+			step_towards(value, high_ramp_torque, NORMAL_STEP);
 			cross_zero = true;
 		}else if (v_now >= RAMPE_1 && v_now <= high_ramp_torque) {
 			step_towards(value, target, 0.03f);
@@ -799,7 +800,7 @@ static void crosszero_step_towards(float *value, float target) {
 		}
 	}else {
 		if (v_now > high_ramp_torque) {
-			step_towards(value, high_ramp_torque, 20.0f);
+			step_towards(value, high_ramp_torque, NORMAL_STEP);
 			cross_zero = true;
 		}else if (v_now >= -RAMPE_1 && v_now <= high_ramp_torque) {
 			step_towards(value, target, 0.03f);
@@ -807,7 +808,7 @@ static void crosszero_step_towards(float *value, float target) {
 		}
 	}
 	if (!cross_zero) {
-		step_towards(&no_cro_step, 3.0f, 0.1f);
+		step_towards(&no_cro_step, NORMAL_STEP, 0.1f);
 		step_towards(value, target, no_cro_step);
 	}else {
 		no_cro_step = 0.5f;
@@ -956,8 +957,9 @@ static __INLINE void PMSM_FOC_idq_Assign(void) {
 /*called in media task */
 void PMSM_FOC_idqCalc(void) {
 	if (gFoc_Ctrl.in.b_AutoHold) {
-		gFoc_Ctrl.pi_lock.max = CONFIG_MAX_LOCK_TORQUE;
-		gFoc_Ctrl.pi_lock.min = -CONFIG_MAX_LOCK_TORQUE;
+		float hold_torque = min(gFoc_Ctrl.protLim.s_TorqueLim, CONFIG_MAX_LOCK_TORQUE);
+		gFoc_Ctrl.pi_lock.max = hold_torque;
+		gFoc_Ctrl.pi_lock.min = -hold_torque;
 		float vel_count = motor_encoder_get_vel_count();
 		float errRef = 0 - vel_count;
 		gFoc_Ctrl.in.s_targetTorque = PI_Controller_Run(&gFoc_Ctrl.pi_lock ,errRef);
@@ -1407,6 +1409,8 @@ static PI_Controller *_pid(u8 id) {
 #ifndef CONFIG_SPEED_LADRC
 		pi = &gFoc_Ctrl.pi_vel;
 #endif
+	}else if (id == PID_Lock_id) {
+		pi = &gFoc_Ctrl.pi_lock;
 	}
 	return pi;
 }

Applications/foc/core/PMSM_FOC_Core.h

@@ -73,6 +73,11 @@ typedef enum {
 	FOC_CRIT_Err_Max = 32,
 }FOC_CritiCal_Ebit_t;
 
+typedef enum {
+	FOC_EV_MOT_Limit_L=FOC_CRIT_Err_Max + 1,
+	FOC_EV_MOS_Limit_L,
+}FOC_EVENT_R_t;
+
 #define FOC_Cri_Err_Mask(err) (1<<(err))
 
 typedef struct {

Applications/foc/core/thro_torque.c

@@ -15,7 +15,7 @@ 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;
+	_torque.torque_req = _torque.torque_real = _torque.torque_acc_ = 0.0f;
 	_torque.gear = mc_get_internal_gear();
 }
 
@@ -81,6 +81,8 @@ float thro_get_ration(float f_thro) {
 
 static float _thro_torque_for_accelerate(float ration) {
 	float max_torque = thro_torque_gear_map((s16)_torque.spd_filted, _torque.gear);
+	float thro_torque = max_torque * ration;
+
 	float acc_r = 1.0f;
 	if (_torque.thro_ration_last < 1.0f) {
 		acc_r = (ration - _torque.thro_ration_last)/ (1.0f - _torque.thro_ration_last);
@@ -90,7 +92,20 @@ static float _thro_torque_for_accelerate(float ration) {
 	if (acc_torque < 0) {
 		acc_torque = 0;
 	}
-	return acc_torque;
+	/*
+	   直接获取油门开度对应的加速扭矩thro_torque 不小于间接计算得到的 acc_torque
+	   如果差值在正负5以上，需要step 补偿
+	*/
+	float torque_acc_ = thro_torque - acc_torque;
+	float step = 0.0f;
+	if (ABS(torque_acc_) < 5) {
+		torque_acc_ = 0;
+	}else {
+		float acc_t = mc_get_gear_config()->n_accl_time;
+		step = torque_acc_ / (acc_t + 0.00001f);
+	}
+	step_towards(&_torque.torque_acc_, torque_acc_, step);
+	return (acc_torque + _torque.torque_acc_);
 }
 
 
@@ -188,13 +203,14 @@ static void thro_torque_filter(float f_throttle) {
 		LowPass_Filter(_torque.spd_filted, curr_rpm, 0.01f);
 	}
 }
-
+static float acc_r, acc_r_last, acc_trq;
+static float dec_r, dec_r_last, dec_trq;
 void thro_torque_process(u8 run_mode, float f_throttle) {
 
 	thro_torque_filter(f_throttle);
 
 	float thro_r = thro_ration(_torque.thro_filted);
-	u8    n_gear = mc_get_internal_gear();
+	_torque.gear = mc_get_internal_gear();
 
 	if (thro_r > _torque.thro_ration) {
 		if (!_torque.accl) {
@@ -203,15 +219,23 @@ void thro_torque_process(u8 run_mode, float f_throttle) {
 			if (_torque.torque_real < 0) { //电子刹车的时候，扭矩可能为负
 				_torque.torque_real = 0;
 			}
+			_torque.torque_acc_ = 0;
+			acc_r = thro_r;
+			acc_r_last = _torque.thro_ration_last;
+			acc_trq = _torque.torque_real;
 		}
-		_torque.gear = n_gear;
 		_torque.accl = true;
 	}else 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;
+			if (_torque.torque_real < 0) { //电子刹车的时候，扭矩可能为负
+				_torque.torque_real = 0;
+			}
+			dec_r = thro_r;
+			dec_r_last = _torque.thro_ration_last;
+			dec_trq = _torque.torque_real;
 		}
-		_torque.gear = n_gear;
 		_torque.accl = false;
 	}
 
@@ -235,10 +259,12 @@ void thro_torque_process(u8 run_mode, float f_throttle) {
 		}
 
 		if (eCtrl_get_FinalTorque() < 0.0001f && vel < CONFIG_MIN_RPM_EXIT_EBRAKE) {
-			eCtrl_enable_eBrake(false);	
+			eCtrl_enable_eBrake(false);
+			thro_torque_reset();
 		}
 		if (!mc_throttle_released() || (mc_throttle_released() && (vel == 0.0f))) {
 			eCtrl_enable_eBrake(false);
+			thro_torque_reset();
 		}
 	}
 }
@@ -254,4 +280,6 @@ float get_user_request_torque(void) {
 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 - %d\n", _torque.thro_ration, _torque.thro_ration_last, thro_torque_for_accelerate(), _torque.gear);
+	sys_debug("acc:%f,%f,%f\n", acc_r, acc_r_last, acc_trq);
+	sys_debug("dec:%f,%f,%f\n", dec_r, dec_r_last, dec_trq);
 }

Applications/foc/core/thro_torque.h

@@ -6,6 +6,7 @@ typedef struct {
 	bool  accl;
 	float torque_req;
 	float torque_real;
+	float torque_acc_;
 	float thro_filted;
 	float spd_filted;
 	float thro_ration;

Applications/foc/limit.c

@@ -123,6 +123,8 @@ static u16 _motor_limit(void) {
 			lim_value = (u16)(((float)gear->n_max_trq * curr_value) / 100.0f);
 			mc_set_motor_lim_level(i + 1);
 			return lim_value;
+		}else {
+			mc_set_motor_lim_level(0);
 		}
 	}
 	
@@ -163,6 +165,8 @@ static u16 _mos_limit(void) {
 			lim_value = (u16)(((float)gear->n_max_trq * curr_value) / 100.0f);
 			mc_set_mos_lim_level(i + 1);
 			return lim_value;
+		}else {
+			mc_set_mos_lim_level(0);
 		}
 	}
 	
@@ -199,7 +203,9 @@ u16 vbus_under_vol_limit(void) {
 		u16 lim_value = _vol_limiter(vol, lim);
 		if (lim_value != HW_LIMIT_NONE) {
 			if (mc_set_critical_error(FOC_CRIT_UN_Vol_Err)) {
-				mc_crit_err_add_s16(FOC_CRIT_UN_Vol_Err, vol);
+				if (PMSM_FOC_GetSpeed() > CONFIG_ZERO_SPEED_RPM) {
+					mc_crit_err_add_s16(FOC_CRIT_UN_Vol_Err, vol);
+				}
 			}
 			return lim_value;
 		}

Applications/foc/motor/motor.c

@@ -406,10 +406,16 @@ bool mc_stop(void) {
 }
 
 void mc_set_mos_lim_level(u8 l) {
+	if (motor.mos_lim != l) {
+		mc_crit_err_add(FOC_EV_MOS_Limit_L, l, get_mos_temp());
+	}
 	motor.mos_lim = l;
 }
 
 void mc_set_motor_lim_level(u8 l) {
+	if (motor.motor_lim != l) {
+		mc_crit_err_add(FOC_EV_MOT_Limit_L, l, get_motor_temp());
+	}
 	motor.motor_lim = l;
 }
 
@@ -1101,7 +1107,9 @@ static void motor_vbus_crit_low(s16 curr_vbus) {
 				PMSM_FOC_Stop();
 			}
 			if (mc_set_critical_error(FOC_CRIT_Vol_HW_Err)) {
-				mc_crit_err_add_s16(FOC_CRIT_Vol_HW_Err, curr_vbus);
+				if (PMSM_FOC_GetSpeed() > CONFIG_ZERO_SPEED_RPM) {
+					mc_crit_err_add_s16(FOC_CRIT_Vol_HW_Err, curr_vbus);
+				}
 			}
 		}
 	}else {
@@ -1262,7 +1270,7 @@ static void mc_autohold_process(void) {
 			mc_auto_hold(false);
 		}
 	}
-	if (!PMSM_FOC_AutoHoldding() && motor.b_break && ((motor_encoder_get_speed() == 0))) {
+	if (!PMSM_FOC_AutoHoldding() && motor.b_break && (motor_encoder_get_speed() == 0)) {
 		if (motor.n_autohold_time == 0) {
 			motor.n_autohold_time = get_tick_ms();
 		}else {