1.加入弱磁控制，通过当前电压矢量模长和最大模长（母线的0.866666）比控制id的大小

2.电机高温，先限制相电流，比如不能满足（限制后还是大于当前相电流），再降低母线电流

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

Applications/app/app.c

@@ -148,6 +148,10 @@ static u32 app_plot_task(void * args) {
 		s16 thro2_v = get_throttle2_float() * 100.0f;
 		s16 sig = throttle_get_signal();
 		can_plot3(sig, thro_v, thro2_v);
+	}else if (plot_type == 10) {
+		s16 duty = (s16)(motor.controller.duty_raw * 100);
+		s16 duty_filterd = (s16)(motor.controller.duty_filterd * 100);
+		can_plot2(duty, duty_filterd);
 	}
 	return 20;
 }

Applications/foc/core/controller.c

@@ -252,6 +252,8 @@ bool mot_contrl_update(mot_contrl_t *ctrl) {
 
 	foc_update(foc);
 
+	ctrl->duty_raw = NORM2_f(foc->out.vol_dq.d, foc->out.vol_dq.q)/(foc->in.dc_vol * CONFIG_SVM_MODULATION * SQRT3_BY_2);
+	LowPass_Filter(ctrl->duty_filterd, ctrl->duty_raw, 0.01f);
 	float lowpass = foc->mot_vel_radusPers * FOC_CTRL_US * 2;
 	lowpass = fclamp(lowpass, 0.001f, 1.0f);
 	LowPass_Filter(ctrl->out_idq_filterd.d, foc->out.curr_dq.d ,lowpass);
@@ -467,8 +469,15 @@ void mot_contrl_slow_task(mot_contrl_t *ctrl) {
 u8 mot_contrl_protect(mot_contrl_t *ctrl) {
 	u8 changed = FOC_LIM_NO_CHANGE;
 	float dc_lim = (float)vbus_under_vol_limit();
-	float torque_lim = (float)min(mos_temp_high_limit(), motor_temp_high_limit());
-	
+	u16 mot_trq_lim = motor_temp_high_limit(type_phase);
+	s16 mos_trq_lim = mos_temp_high_limit();
+	float torque_lim = (float)min(mos_trq_lim, mot_trq_lim);
+
+	/*如果电机高温扭矩限制后，还是比当前的扭矩大，需要通过限制母线电流来实现降温*/
+	if ((torque_lim != HW_LIMIT_NONE) && (torque_lim >= mc_get_max_torque_now())) {
+		float mot_idc_lim = motor_temp_high_limit(type_idc);
+		dc_lim = min(dc_lim, mot_idc_lim);
+	}
 	if (ctrl->protlim.dc_curr != dc_lim || ctrl->protlim.torque != torque_lim) {
 		if ((dc_lim > ctrl->protlim.dc_curr) || (torque_lim > ctrl->protlim.torque)) {
 			changed = FOC_LIM_CHANGE_H;
@@ -815,7 +824,7 @@ void mot_contrl_calc_current(mot_contrl_t *ctrl) {
 	}else {
 		ctrl->dc_curr_filted = ctrl->dc_curr_calc;
 	}
-	ctrl->out_current_vec = sqrtf(SQ(id) + SQ(iq));
+	ctrl->out_current_vec = NORM2_f(id, iq);
 }
 
 

Applications/foc/core/controller.h

@@ -137,6 +137,8 @@ typedef struct{
 	float			dc_curr_calc;
 	float   		phase_curr_filted[3];
 	float           autohold_torque;
+	float			duty_raw;
+	float           duty_filterd;
 	u8 				mode_req;
 	u8 				mode_running;
 	u8      		error;

Applications/foc/core/smo_observer.c

@@ -124,7 +124,7 @@ static void smo_arctan(void) {
 }
 #else
 static void smo_pll(void) {
-	float eab_sqr = sqrtf(SQ(smo.est_eAlpha_Filted) + SQ(smo.est_eBeta_Filted) + (1e-10f));
+	float eab_sqr = NORM2_f(smo.est_eAlpha_Filted, smo.est_eBeta_Filted) + (1e-10f));
 	float ealpha_in = -smo.est_eAlpha_Filted/eab_sqr;
 	float ebeta_in  =  smo.est_eBeta_Filted/eab_sqr;
 	float sin, cos;

Applications/foc/foc_config.h

@@ -61,6 +61,9 @@
 #define CONFIG_Volvec_Delay_Comp /* 电压矢量角度补偿 */
 #define CONFIG_Volvec_Delay_Comp_Start_Vel 500 // rpm
 
+#define CONFIG_CONTRL_FW_ENABLE 1
+#define CONFIG_CONTRL_FW_START_DUTY 0.9F
+
 #define CONFIG_ENABLE_IAB_REC 0   // for phase current debug
 
 #ifdef CONFIG_SPEED_LADRC

Applications/foc/limit.c

@@ -93,7 +93,7 @@ static u16 _vol_limiter(s16 vol, limter_t *lim) {
 	}
 }
 
-static u16 _motor_limit(void) {
+static u16 _motor_limit(limit_type type) {
 	static int temp_sensor_err = 0;
 	static u16 lim_value  = HW_LIMIT_NONE;
 	s16 temp = get_motor_temp_raw();
@@ -139,7 +139,11 @@ static u16 _motor_limit(void) {
 			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->max_torque * curr_value) / 100.0f);
+			float max_value = (float)gear->max_torque;
+			if (type == type_idc) {
+				max_value = (float)gear->max_idc;
+			}
+			lim_value = (u16)((max_value * curr_value) / 100.0f);
 			mc_set_motor_lim_level(i + 1);
 			return lim_value;
 		}else {
@@ -208,12 +212,12 @@ static u16 _mos_limit(void) {
 }
 
 /* this maybe limit power or torque, based on the current power */
-u16 motor_temp_high_limit(void) {
+u16 motor_temp_high_limit(limit_type type) {
 	if (!_inited) {
 		_inited = true;
 		limiter_init();
 	}
-	return _motor_limit();
+	return _motor_limit(type);
 }
 
 /* limit the max torque(max phase current) */

Applications/foc/limit.h

@@ -10,8 +10,12 @@ typedef struct {
 	u32 ticks;
 	bool is_limit;
 }limter_t;
+typedef enum {
+	type_phase,
+	type_idc,
+}limit_type;
 
-u16 motor_temp_high_limit(void);
+u16 motor_temp_high_limit(limit_type type);
 u16 mos_temp_high_limit(void);
 u16 vbus_under_vol_limit(void);
 

Applications/foc/motor/mot_params_ind.c

@@ -240,7 +240,7 @@ void goertzel_dft(float *x, float *real, float *image, float *mag) {
 	}
 	*real = d1 - (d2 * 0.5f * hj_real);
 	*image = -d2 * hj_image;
-	*mag = sqrtf(SQ(*real) + SQ(*image));
+	*mag = NORM2_f(*real, *image);
 }
 
 void mot_params_calc_inductance(void) {

Applications/foc/motor/motor.c

@@ -302,7 +302,9 @@ float mc_gear_max_torque(s16 vel, u8 gear_n) {
 	return f_map((float)vel, min_rpm, max_rpm, torque_1, torque_2);
 }
 
-
+float mc_get_max_torque_now(void) {
+	return mc_gear_max_torque(motor.controller.foc.mot_velocity_filterd, motor.n_gear);
+}
 /* 必须立即停机 */
 bool mc_critical_need_stop(void) {
 	u32 mask = FOC_Cri_Err_Mask(FOC_CRIT_IDC_OV) | FOC_Cri_Err_Mask(FOC_CRIT_Angle_Err) | FOC_Cri_Err_Mask(FOC_CRIT_Phase_Err) |

Applications/foc/motor/motor.h

@@ -136,6 +136,7 @@ bool mc_set_target_vel(s16 vel);
 float mc_gear_max_torque(s16 vel, u8 gear);
 bool mc_set_force_torque(s16 torque);
 bool mc_tcs_is_enabled(void);
+float mc_get_max_torque_now(void);
 
 static __INLINE mot_contrl_t *mot_contrl(void) {
 	return &motor.controller;

Applications/foc/motor/motor_param.c

@@ -346,10 +346,15 @@ void motor_mpta_fw_lookup(float rpm, float torque, dq_t *dq_out) {
 		}
 	}
 #else
+#if CONFIG_CONTRL_FW_ENABLE
+	if (mot_contrl()->duty_filterd >= 0.9f) {
+		d = -f_map(mot_contrl()->duty_filterd, 0.9f, 1.0f, 0, mc_conf()->m.max_fw_id);
+	}
+#endif
 	q = torque;
 #endif
-	step_towards(&dq_out->d, d, 1.0f);
-	step_towards(&dq_out->q, q, 0.7f);
+	step_towards(&dq_out->d, d, 10.0f);
+	step_towards(&dq_out->q, q, 5.0f);
 }
 
 #endif

Applications/math/fast_math.h

@@ -19,6 +19,8 @@
 #ifndef SQ
 #define SQ(x) ((x)*(x))
 #endif
+#define NORM2_f(x,y)		(sqrtf(SQ(x) + SQ(y)))
+
 // nan and infinity check for floats
 #define UTILS_IS_INF(x)		((x) == (1.0F / 0.0F) || (x) == (-1.0F / 0.0F))
 #define UTILS_IS_NAN(x)		((x) != (x))

configs/mc124_R1.yml

@@ -1,4 +1,4 @@
-##### 配置文件自动生成，不要手动修改!! 2023/11/2 16:25:58
+##### 配置文件自动生成，不要手动修改!! 2023/11/15 16:04:00
 Version: 1
 CheckCrc: 0
 Motor:
@@ -59,27 +59,27 @@ Foc:
 Settings:
   AutoHold: 1
   BrkShutPower: 1
-  TcsEnable: 1
+  TcsEnable: 0
 Gear:
-- MaxSpeed: 2000
-  MaxTorque: 100
-  MaxIdc: 30
-  ZeroAccl: 500
+- MaxSpeed: 3500
+  MaxTorque: 250
+  MaxIdc: 45
+  ZeroAccl: 300
   NormalAccl: 100
   Torque:
   - 100
   - 100
   - 100
+  - 80
+  - 60
   - 0
   - 0
   - 0
   - 0
   - 0
-  - 0
-  - 0
-- MaxSpeed: 2800
-  MaxTorque: 200
-  MaxIdc: 35
+- MaxSpeed: 4500
+  MaxTorque: 250
+  MaxIdc: 50
   ZeroAccl: 300
   NormalAccl: 80
   Torque:
@@ -87,42 +87,42 @@ Gear:
   - 100
   - 100
   - 80
+  - 75
+  - 60
   - 0
   - 0
   - 0
   - 0
-  - 0
-  - 0
-- MaxSpeed: 1000
-  MaxTorque: 100
-  MaxIdc: 30
+- MaxSpeed: 5500
+  MaxTorque: 300
+  MaxIdc: 65
   ZeroAccl: 500
-  NormalAccl: 100
+  NormalAccl: 50
   Torque:
   - 100
   - 100
   - 100
-  - 100
-  - 100
-  - 100
-  - 100
-  - 100
+  - 90
+  - 80
+  - 70
+  - 50
   - 0
   - 0
-- MaxSpeed: 1000
-  MaxTorque: 100
-  MaxIdc: 30
+  - 0
+- MaxSpeed: 6500
+  MaxTorque: 300
+  MaxIdc: 100
   ZeroAccl: 500
-  NormalAccl: 100
+  NormalAccl: 30
   Torque:
   - 100
   - 100
   - 100
-  - 100
-  - 100
-  - 100
-  - 100
-  - 100
+  - 90
+  - 80
+  - 70
+  - 70
+  - 50
   - 0
   - 0
 GearLow:
@@ -216,7 +216,7 @@ Protect:
     Exit: 42
     Value: 0
 EnergyRecovery:
-- Torque: 10
+- Torque: 0
   Time: 600
 - Torque: 10
   Time: 500