1. 能量回收负扭矩根据车速，线性减弱

2. 温度保护，线性下降，不做突然下降

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

Applications/app/app.c

@@ -16,6 +16,7 @@
 #include "foc/core/thro_torque.h"
 #include "foc/core/F_Calc.h"
 #include "foc/motor/motor_param.h"
+#include "foc/limit.h"
 
 
 #ifdef CONFIG_DQ_STEP_RESPONSE
@@ -147,7 +148,7 @@ static u32 _app_report_task(void *p) {
 	}
 	return 200;
 }
-int plot_type = 10;
+int plot_type = 1;
 static void plot_smo_angle(void) {
 	float smo_angle = foc_observer_sensorless_angle();
 	float delta = smo_angle - PMSM_FOC_Get()->in.s_motAngle;

Applications/app/nv_storage.c

@@ -63,20 +63,20 @@ static void nv_default_limter(void) {
 	limiter.motor[0].limit_value = 0;
 	limiter.motor[1].enter_pointer = 110;
 	limiter.motor[1].exit_pointer = 97;
-	limiter.motor[1].limit_value = CONFIG_MAX_MOTOR_TORQUE/3;
+	limiter.motor[1].limit_value = 50;
 	limiter.motor[2].enter_pointer = 97;
 	limiter.motor[2].exit_pointer = 85;
-	limiter.motor[2].limit_value = CONFIG_MAX_MOTOR_TORQUE*2/3;
+	limiter.motor[2].limit_value = 80;
 
 	limiter.mos[0].enter_pointer = 110;
 	limiter.mos[0].exit_pointer = 100;
 	limiter.mos[0].limit_value = 0;
 	limiter.mos[1].enter_pointer = 100;
 	limiter.mos[1].exit_pointer = 90;
-	limiter.mos[1].limit_value = CONFIG_MAX_MOTOR_TORQUE/3;
+	limiter.mos[1].limit_value = 60;
 	limiter.mos[2].enter_pointer = 90;
 	limiter.mos[2].exit_pointer = 80;
-	limiter.mos[2].limit_value = CONFIG_MAX_MOTOR_TORQUE*2/3;
+	limiter.mos[2].limit_value = 80;
 
 	limiter.vbus.enter_pointer = 76;
 	limiter.vbus.exit_pointer = 80;

Applications/foc/core/PMSM_FOC_Core.c

@@ -574,8 +574,10 @@ u8 PMSM_FOC_CtrlMode(void) {
 
 #define RAMPE_1 CONFIG_RAMP_FIRST_TARGET
 static void crosszero_step_towards(float *value, float target) {
+	static float no_cro_step = 2.0f;
 	float v_now = *value;
 	bool cross_zero = false;
+
 	float high_ramp_torque = CONFIG_RAMP_SECOND_TARGET;
 	if (target > 0) {
 		if (v_now < -RAMPE_1) {
@@ -603,7 +605,10 @@ static void crosszero_step_towards(float *value, float target) {
 		}
 	}
 	if (!cross_zero) {
-		step_towards(value, target, 2.0f);
+		step_towards(&no_cro_step, 3.0f, 0.1f);
+		step_towards(value, target, no_cro_step);
+	}else {
+		no_cro_step = 0.5f;
 	}
 }
 
@@ -812,8 +817,8 @@ void PMSM_FOC_idqCalc(void) {
 
 u8 PMSM_FOC_RunTime_Limit(void) {
 	u8 changed = FOC_LIM_NO_CHANGE;
-	float dc_lim = (float)vbus_current_vol_lower_limit();
-	float torque_lim = (float)torque_temp_high_limit();
+	float dc_lim = (float)vbus_under_vol_limit();
+	float torque_lim = (float)min(mos_temp_high_limit(), motor_temp_high_limit());
 
 	if (gFoc_Ctrl.protLim.s_iDCLim != dc_lim || gFoc_Ctrl.protLim.s_TorqueLim != torque_lim) {
 		if ((dc_lim > gFoc_Ctrl.protLim.s_iDCLim) || (torque_lim > gFoc_Ctrl.protLim.s_TorqueLim)) {

Applications/foc/core/ladrc_observer.c

@@ -1,6 +1,7 @@
 #include "ladrc_observer.h"
 #include "app/nv_storage.h"
 #include "math/fast_math.h"
+#include "libs/logger.h"
 
 static ladrc_observer observer;
 
@@ -48,6 +49,7 @@ void ladrc_observer_init(float Wo, float vel_min, float lpf_cut_off) {
 		observer.angle_array[i] = 0;
 	}
 	ladrc_observer_band(0);
+	sys_debug("ld:%f, lq:%f, vmin:%f, wo:%f\n", observer.ld, observer.lq, observer.vel_min, observer.Wo);
 }
 
 float ladrc_observer_update(float va, float vb, float ia, float ib) {

Applications/foc/limit.c

@@ -100,6 +100,25 @@ static u16 _motor_limit(void) {
 			}else if (_can_recovery){
 				mc_clr_critical_error(FOC_CRIT_MOTOR_TEMP_Err);
 			}
+			mc_gear_t *gear = mc_get_gear_config();
+
+			float prv_lim_value;
+			float next_lim_tmp;
+			if (i < (ARRAY_SIZE(motor_temp_lim)-1)) {
+				prv_lim_value = (float)motor_temp_lim[i + 1].limit_value;
+			}else {
+				prv_lim_value = 100.0f; //最低一级限流
+			}
+			if (i != 0) {
+				next_lim_tmp = (float)motor_temp_lim[i - 1].enter_pointer;
+			}else {
+				next_lim_tmp = (float)lim->enter_pointer + 10.0f; //最大一级限流
+			}
+			float delta_tmp = (next_lim_tmp - (float)lim->enter_pointer);
+			float delta_value = (prv_lim_value - (float)lim->limit_value);
+			float curr_value = prv_lim_value - (float)(temp - lim->enter_pointer)/delta_tmp * delta_value;
+			curr_value = fclamp(curr_value, 0, prv_lim_value);
+			lim_value = (u16)(((float)gear->n_max_trq * curr_value) / 100.0f);
 			mc_set_motor_lim_level(i + 1);
 			err_add_record(FOC_CRIT_MOTOR_TEMP_Err, temp);
 			return lim_value;
@@ -120,6 +139,25 @@ static u16 _mos_limit(void) {
 			}else if (_can_recovery){
 				mc_clr_critical_error(FOC_CRIT_MOS_TEMP_Err);
 			}
+			mc_gear_t *gear = mc_get_gear_config();
+
+			float prv_lim_value;
+			float next_lim_tmp;
+			if (i < (ARRAY_SIZE(mos_temp_lim)-1)) {
+				prv_lim_value = (float)mos_temp_lim[i + 1].limit_value;
+			}else {
+				prv_lim_value = 100.0f; //最低一级限流
+			}
+			if (i != 0) {
+				next_lim_tmp = (float)mos_temp_lim[i - 1].enter_pointer;
+			}else {
+				next_lim_tmp = (float)lim->enter_pointer + 10.0f; //最大一级限流
+			}
+			float delta_tmp = (next_lim_tmp - (float)lim->enter_pointer);
+			float delta_value = (prv_lim_value - (float)lim->limit_value);
+			float curr_value = prv_lim_value - (float)(temp - lim->enter_pointer)/delta_tmp * delta_value;
+			curr_value = fclamp(curr_value, 0, prv_lim_value);
+			lim_value = (u16)(((float)gear->n_max_trq * curr_value) / 100.0f);
 			err_add_record(FOC_CRIT_MOS_TEMP_Err, temp);
 			mc_set_mos_lim_level(i + 1);
 			return lim_value;
@@ -129,17 +167,26 @@ static u16 _mos_limit(void) {
 	return HW_LIMIT_NONE;
 }
 
-u16 torque_temp_high_limit(void) {
+/* this maybe limit power or torque, based on the current power */
+u16 motor_temp_high_limit(void) {
+	if (!_inited) {
+		_inited = true;
+		limiter_init();
+	}
+	return _motor_limit();
+}
+
+/* limit the max torque(max phase current) */
+u16 mos_temp_high_limit(void) {
 	if (!_inited) {
 		_inited = true;
 		limiter_init();
 	}
-	u16 motor_lim = _motor_limit();
-	u16 mos_lim   = _mos_limit();
-	return min(motor_lim, mos_lim);
+	return _mos_limit();
 }
 
-u16 vbus_current_vol_lower_limit(void) {
+/* limit the DC bus current */
+u16 vbus_under_vol_limit(void) {
 	if (!_inited) {
 		_inited = true;
 		limiter_init();

Applications/foc/limit.h

@@ -2,7 +2,7 @@
 #define _Limit_H__
 #include "os/os_types.h"
 
-#define HW_LIMIT_NONE ((u16)0xFFFF)
+#define HW_LIMIT_NONE ((u16)0x7FFF)
 typedef struct {
 	u16 limit_value;
 	s16 enter_pointer;
@@ -11,8 +11,9 @@ typedef struct {
 	bool is_limit;
 }limter_t;
 
-u16 torque_temp_high_limit(void);
-u16 vbus_current_vol_lower_limit(void);
+u16 motor_temp_high_limit(void);
+u16 mos_temp_high_limit(void);
+u16 vbus_under_vol_limit(void);
 
 #endif /* _Limit_H__ */
 

Applications/foc/motor/motor_param.c

@@ -214,18 +214,17 @@ 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) {
+	if (rpm >= 2000) {
 		return -max_e_trq;
 	}else if (rpm >= 1000) {
-		return -max_e_trq * 0.75f;
+		return -max_e_trq * ((rpm - 1000.0f) / 1000.0f * 0.25f + 0.75f);
 	}else if (rpm > CONFIG_MIN_RPM_EXIT_EBRAKE) {
-		return -max_e_trq * 0.25f;
+		return -max_e_trq * 0.75f * (rpm - CONFIG_MIN_RPM_EXIT_EBRAKE)/((float)(1000 - CONFIG_MIN_RPM_EXIT_EBRAKE));
 	}
 
-	return 0;
+	return 0.0f;
 }
 

Applications/foc/motor/motor_param.h

@@ -103,7 +103,7 @@ float motor_get_ebreak_toruqe(float rpm);
 
 #define MOTOR_NR 0x13
 
-#define CONFIG_CURRENT_BANDWITH  1000.0f /* 电流环带宽 */
+#define CONFIG_CURRENT_BANDWITH  (200 * 2 * PI) /* 电流环带宽 */
 #define CONFIG_DEFAULT_PHASE_CURR_LIM 200
 #define CONFIG_MAX_FW_D_CURR     100.0F //d轴最大的退磁电流