电流环饱和后，定速巡航的reset值取实际的电流矢量

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

Applications/app/app.c

@@ -96,6 +96,7 @@ static u32 _app_report_task(void *p) {
 	if (++loop % 10 == 0) {
 		can_report_pid_value(0x45, PID_TRQ_id);
 		sys_debug("max exec time %d\n", g_meas_foc.exec_max_time);
+		sys_debug("sat = %d, %d, %d\n", PMSM_FOC_Get()->pi_id->is_sat, PMSM_FOC_Get()->pi_iq->is_sat, PMSM_FOC_Get()->pi_speed->is_sat);
 	}
 	return 500;
 }

Applications/bsp/board_mc_v1.h

@@ -12,8 +12,8 @@
 #define CONFIG_MIN_DC_VOL   (25.0f)
 
 #define CONFIG_MAX_VBUS_CURRENT 150.0f
-#define CONFIG_MAX_MOT_RPM      8000.0f
-#define CONFIG_MAX_PHASE_CURR   180.0F
+#define CONFIG_MAX_MOT_RPM      9000.0f
+#define CONFIG_MAX_PHASE_CURR   400.0F
 #define CONFIG_MAX_PHASE_VOL    (CONFIG_MOS_MAX_VOL - 20.0F)
 #define CONFIG_MAX_TORQUE       50.0F
 
@@ -26,7 +26,7 @@
 #define CONFIG_UNDER_VOL_DC_CURR 15.0F
 #define CONFIG_MAX_FW_D_CURR     100.0F //d轴最大的退磁电流
 
-#define CONFIG_CURR_LP_CUT_FREQ (2000.0F)
+#define CONFIG_CURR_LP_CUT_FREQ (3000.0F)
 
 #define CONFIG_CURR_LP_PARAM (CONFIG_CURR_LP_CUT_FREQ*2*3.14F/(float)FOC_PWM_FS)
 

Applications/foc/core/PI_Controller.h

@@ -36,6 +36,7 @@ typedef struct {
 	float  Ui;
 	float  sat;
 	float  DT;
+	bool   is_sat;
 }PI_Controller;
 
 static __INLINE void PI_Controller_max(PI_Controller *pi, float max, float min) {
@@ -45,6 +46,7 @@ static __INLINE void PI_Controller_max(PI_Controller *pi, float max, float min)
 static __INLINE void PI_Controller_Reset(PI_Controller *pi, float init) {
 	pi->Ui = (init);
 	pi->sat = 0.0f;
+	pi->is_sat = false;
 }
 
 static __INLINE float PI_Controller_run(PI_Controller *pi, float err) {
@@ -53,7 +55,13 @@ static __INLINE float PI_Controller_run(PI_Controller *pi, float err) {
 	float integral = ki_err * pi->DT;
 	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
 	float out = pi->Ui + kp_err;
-	return (MATH_sat(out, pi->min, pi->max));
+	float sat_out =  (MATH_sat(out, pi->min, pi->max));
+	if (out != sat_out) {
+		pi->is_sat = true;
+	}else {
+		pi->is_sat = false;
+	}
+	return sat_out;
 }
 
 static __INLINE float PI_Controller_RunSat(PI_Controller *pi, float err) {
@@ -63,6 +71,11 @@ static __INLINE float PI_Controller_RunSat(PI_Controller *pi, float err) {
 	pi->Ui = pi->Ui + integral + pi->sat * pi->kb;
 	float out = pi->Ui + kp_err ;
 	float out_sat = MATH_sat(out, pi->min, pi->max);
+	if (out != out_sat) {
+		pi->is_sat = true;
+	}else {
+		pi->is_sat = false;
+	}
 	pi->sat = out_sat - out;
 	if (pi->sat >= 0.0f) {
 		pi->sat = 0.0f;
@@ -77,7 +90,13 @@ static __INLINE float PI_Controller_RunSerial(PI_Controller *pi, float err) {
 	float integral = ki_err * pi->DT;
 	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
 	float out = pi->Ui + kp_err;
-	return (MATH_sat(out, pi->min, pi->max));
+	float sat_out =  (MATH_sat(out, pi->min, pi->max));
+	if (out != sat_out) {
+		pi->is_sat = true;
+	}else {
+		pi->is_sat = false;
+	}
+	return sat_out;
 }
 
 static __INLINE float _fmod(float v, s32 m) {

Applications/foc/core/PMSM_FOC_Core.c

@@ -407,7 +407,11 @@ u8 PMSM_FOC_CtrlMode(void) {
 		if ((preMode == CTRL_MODE_SPD) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
 			PI_Controller_Reset(gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
 		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
-			PI_Controller_Reset(gFoc_Ctrl.pi_speed, gFoc_Ctrl.in.s_targetTorque);
+			float target_troque = gFoc_Ctrl.in.s_targetTorque;
+			if (gFoc_Ctrl.pi_id->is_sat || gFoc_Ctrl.pi_iq->is_sat) {
+				target_troque = sqrtf(SQ(gFoc_Ctrl.out.s_RealIdq.d) + SQ(gFoc_Ctrl.out.s_RealIdq.q)) * 1.05f;
+			}
+			PI_Controller_Reset(gFoc_Ctrl.pi_speed, target_troque);
 		}else if ((preMode == CTRL_MODE_CURRENT) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
 			PI_Controller_Reset(gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
 		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT)) {

Applications/foc/foc_config.h

@@ -1,15 +1,17 @@
 #ifndef _FOC_CONFIG_H__
 #define _FOC_CONFIG_H__
 
-#define CONFIG_DEFAULT_IDC_LIM 80
-#define CONFIG_DEFAULT_PHASE_CURR_LIM 200
+#define CONFIG_DEFAULT_IDC_LIM 100
+#define CONFIG_DEFAULT_PHASE_CURR_LIM 120
+#define CONFIG_DEFAULT_RPM_LIM       2000
+
 #define CONFIG_DEFAULT_EPM_PHASE_CURR 50
 #define CONFIG_DEFAULT_EPM_RPM        200
 #define CONFIG_DEFAULT_EBRK_PHASE_CURR 0.0F //0:means disable ebrake
 #define CONFIG_DEFAULT_EBRK_IDC_LIM 15
 #define CONFIG_SVM_MODULATION       1.0F
 #define CONFIG_BRK_SHUT_POWER_ENABLE 1
-#define CONFIG_DEFAULT_RPM_LIM       4000
+
 /* 转把 */
 #define CONFIG_THROTTLE_LOW_VALUE 1.2f /* 转把最小值 */
 #define CONFIG_THROTTLE_MAX_VALUE 3.8f /* 转把最大值 */