1. 电流环带宽2000hz，否则高速电流环不能稳定，而且计算的母线电流偏小

2. 限速，限母线电流
3. FOC限制值通过foc_config.h中的宏来定义
4. 电流低通滤波补偿幅值（相位补偿未实现）

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

Applications/app/app.c

@@ -95,6 +95,7 @@ static u32 _app_report_task(void *p) {
 	can_report_phase_voltage(0x45);
 	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);
 	}
 	return 500;
 }

Applications/app/nv_storage.c

@@ -42,21 +42,18 @@ static void nv_default_motor_params(void) {
 static void nv_default_foc_params(void) {
 	foc_params.s_maxDCVol = CONFIG_RATED_DC_VOL;
 	foc_params.s_minDCVol = CONFIG_MIN_DC_VOL;
-	foc_params.s_maxiDC = 30;
-	//foc_params.s_maxIdq = 200;
-	//foc_params.s_minIdq = -200;
-	foc_params.s_PhaseCurrLim = 180;
-	foc_params.s_maxRPM = 8000;
-	foc_params.s_maxEpmRPM = 200;
-	foc_params.s_maxEpmPhaseCurrLim = 10;
+	foc_params.s_PhaseCurrLim = CONFIG_DEFAULT_PHASE_CURR_LIM;
+	foc_params.s_maxRPM = CONFIG_DEFAULT_RPM_LIM;
+	foc_params.s_maxEpmRPM = CONFIG_DEFAULT_EPM_RPM;
+	foc_params.s_maxEpmPhaseCurrLim = CONFIG_DEFAULT_EPM_PHASE_CURR;
 	foc_params.s_maxTorque = foc_params.s_PhaseCurrLim;
-	foc_params.s_PhaseCurreBrkLim = 0.0f;
+	foc_params.s_PhaseCurreBrkLim = CONFIG_DEFAULT_EBRK_PHASE_CURR;
 	foc_params.n_currentBand = CONFIG_CURRENT_BANDWITH;
-	foc_params.n_modulation = 1.0f;
-	foc_params.n_PhaseFilterCeof = 0.2f;
-	foc_params.n_brkShutPower = 1;
-	foc_params.s_LimitiDC = 200.0f;
-	foc_params.s_iDCeBrkLim = 15.0f;
+	foc_params.n_modulation = CONFIG_SVM_MODULATION;
+	foc_params.n_PhaseFilterCeof = CONFIG_CURR_LP_PARAM;
+	foc_params.n_brkShutPower = CONFIG_BRK_SHUT_POWER_ENABLE;
+	foc_params.s_LimitiDC = CONFIG_DEFAULT_IDC_LIM;
+	foc_params.s_iDCeBrkLim = CONFIG_DEFAULT_EBRK_IDC_LIM;
 	
 	foc_params.pid_conf[PID_D_id].kp = (CONFIG_CURRENT_BANDWITH * MOTOR_Ld);
 	foc_params.pid_conf[PID_D_id].ki = (MOTOR_R/MOTOR_Ld);
@@ -66,16 +63,16 @@ static void nv_default_foc_params(void) {
 	foc_params.pid_conf[PID_Q_id].ki = (MOTOR_R/MOTOR_Lq);
 	foc_params.pid_conf[PID_Q_id].kb = 0;
 
-	foc_params.pid_conf[PID_TRQ_id].kp = 0.13f;
-	foc_params.pid_conf[PID_TRQ_id].ki = 0.3f;
+	foc_params.pid_conf[PID_TRQ_id].kp = 0.15f;
+	foc_params.pid_conf[PID_TRQ_id].ki = 0.14f;
 	foc_params.pid_conf[PID_TRQ_id].kb = 1.0f;
 
 	foc_params.pid_conf[PID_Spd_id].kp = 0.13f;
 	foc_params.pid_conf[PID_Spd_id].ki = 0.08f;
 	foc_params.pid_conf[PID_Spd_id].kb = 1.0f;
 
-	foc_params.pid_conf[PID_Pow_id].kp = 0.5f;
-	foc_params.pid_conf[PID_Pow_id].ki = 100.0f;
+	foc_params.pid_conf[PID_Pow_id].kp = 0.6f;
+	foc_params.pid_conf[PID_Pow_id].ki = 4.0f;
 	foc_params.pid_conf[PID_Pow_id].kb = 0;
 
 	foc_params.pid_conf[PID_Lock_id].kp = (0.1f);

Applications/app/nv_storage.h

@@ -7,7 +7,6 @@
 
 typedef struct {
 	float s_PhaseCurrLim;
-	float s_maxiDC;
 	float s_maxDCVol;
 	float s_minDCVol;
 	float s_maxRPM;

Applications/bsp/board_mc_v1.h

@@ -11,11 +11,12 @@
 #define CONFIG_RATED_DC_VOL (96.0f)   /* 母线最大电压 V*/
 #define CONFIG_MIN_DC_VOL   (25.0f)
 
-#define CONFIG_MAX_VBUS_CURRENT 45.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_PHASE_VOL    (CONFIG_MOS_MAX_VOL - 20.0F)
 #define CONFIG_MAX_TORQUE       50.0F
+
 #define CONFIG_CURRENT_BANDWITH 1500.0f /* 电流环带宽 */
 #define CONFIG_BEEP 
 #define CONFIG_STALL_MAX_CURRENT 100.0f //最大堵转相电流电流
@@ -25,6 +26,10 @@
 #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_PARAM (CONFIG_CURR_LP_CUT_FREQ*2*3.14F/(float)FOC_PWM_FS)
+
 #define SCHED_TIMER TIMER5
 #define SCHED_TIMER_RCU RCU_TIMER5
 #define SCHED_TIMER_IRQ TIMER5_IRQn

Applications/foc/core/PMSM_FOC_Core.c

@@ -165,10 +165,10 @@ static void PMSM_FOC_Conf_PID(void) {
 
 static void PMSM_FOC_UserInit(void) {
 	memset(&gFoc_Ctrl.userLim, 0, sizeof(gFoc_Ctrl.userLim));
-	gFoc_Ctrl.userLim.s_iDCLim = nv_get_foc_params()->s_maxiDC;
-	gFoc_Ctrl.userLim.s_motRPMLim = nv_get_foc_params()->s_maxRPM;//(MAX_SPEED);
+	gFoc_Ctrl.userLim.s_iDCLim = min(nv_get_foc_params()->s_LimitiDC, gFoc_Ctrl.hwLim.s_iDCMax);
+	gFoc_Ctrl.userLim.s_motRPMLim = min(nv_get_foc_params()->s_maxRPM, gFoc_Ctrl.hwLim.s_motRPMMax);
 	gFoc_Ctrl.userLim.s_torqueLim = nv_get_foc_params()->s_maxTorque;//MAX_TORQUE;
-	gFoc_Ctrl.userLim.s_PhaseCurrLim = nv_get_foc_params()->s_PhaseCurrLim;
+	gFoc_Ctrl.userLim.s_PhaseCurrLim = min(nv_get_foc_params()->s_PhaseCurrLim, gFoc_Ctrl.hwLim.s_PhaseCurrMax);
 	gFoc_Ctrl.userLim.s_vDCMaxLim = nv_get_foc_params()->s_maxDCVol;
 	gFoc_Ctrl.userLim.s_vDCMinLim = nv_get_foc_params()->s_minDCVol;
 	gFoc_Ctrl.userLim.s_iDCeBrkLim = nv_get_foc_params()->s_iDCeBrkLim;
@@ -231,7 +231,7 @@ void PMSM_FOC_CoreInit(void) {
 static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	AB_t vAB;
 #ifdef PHASE_LFP	
-	float *iabc = gFoc_Ctrl.in.s_iABCFilter;
+	float *iabc = gFoc_Ctrl.in.s_iABCComp;
 #elif defined PHASE_LFP_FIR
 	float *iabc = gFoc_Ctrl.in.s_iABCFilter;
 #else
@@ -268,6 +268,10 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[0], gFoc_Ctrl.in.s_iABC[0], gFoc_Ctrl.params.n_PhaseFilterCeof);
 	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[1], gFoc_Ctrl.in.s_iABC[1], gFoc_Ctrl.params.n_PhaseFilterCeof);
 	LowPass_Filter(gFoc_Ctrl.in.s_iABCFilter[2], gFoc_Ctrl.in.s_iABC[2], gFoc_Ctrl.params.n_PhaseFilterCeof);
+	float comp = lp_compestion(gFoc_Ctrl.in.s_motRPM/60.0f*4.0f, CONFIG_CURR_LP_CUT_FREQ);
+	gFoc_Ctrl.in.s_iABCComp[0] = gFoc_Ctrl.in.s_iABCFilter[0] * comp;
+	gFoc_Ctrl.in.s_iABCComp[1] = gFoc_Ctrl.in.s_iABCFilter[1] * comp;
+	gFoc_Ctrl.in.s_iABCComp[2] = gFoc_Ctrl.in.s_iABCFilter[2] * comp;
 #elif defined PHASE_LFP_FIR
 	gFoc_Ctrl.in.s_iABCFilter[1] = Fir_Filter(&phase1, gFoc_Ctrl.in.s_iABC[1]);
 	gFoc_Ctrl.in.s_iABCFilter[2] = Fir_Filter(&phase2, gFoc_Ctrl.in.s_iABC[2]);
@@ -369,7 +373,7 @@ void PMSM_FOC_Schedule(void) {
 	if (gFoc_Ctrl.plot_type != Plot_None) {
 		if (gFoc_Ctrl.ctrl_count % 5 == 0) {
 			if (gFoc_Ctrl.plot_type == Plot_Phase_curr) {
-				plot_3data16(FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[0]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[1]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[2]));
+				plot_3data16(FtoS16x10(gFoc_Ctrl.in.s_iABC[0]), FtoS16x10(gFoc_Ctrl.in.s_iABCFilter[0]), FtoS16x10(gFoc_Ctrl.in.s_iABCComp[0]));
 			}else if (gFoc_Ctrl.plot_type == Plot_Phase_vol) {
 				plot_3data16(FtoS16x10(gFoc_Ctrl.in.s_vABC[0]), FtoS16x10(gFoc_Ctrl.in.s_vABC[1]), FtoS16x10(gFoc_Ctrl.in.s_vABC[2]));
 			}
@@ -415,7 +419,7 @@ u8 PMSM_FOC_CtrlMode(void) {
 
 /* MPTA, 弱磁, 功率限制，主要是分配DQ轴电流 */
 static __INLINE float PMSM_FOC_Limit_Power(float maxTrq) {
-#if 0
+#if 1
 	PI_Ctrl_Power.max = maxTrq;
 	float errRef = eRamp_get_intepolation(&gFoc_Ctrl.userLim.DCCurrLimRamp) - gFoc_Ctrl.out.s_FilteriDC;
 	return PI_Controller_run(gFoc_Ctrl.pi_power, errRef);

Applications/foc/core/PMSM_FOC_Core.h

@@ -74,6 +74,7 @@ typedef struct {
 typedef struct {
 	float 	s_iABC[3];
 	float 	s_iABCFilter[3];
+	float   s_iABCComp[3];
 	float   s_vABC[3];
 	float   s_motRPM;   //from hall or encoder
 	float 	s_motAngle; //from hall or encoder
@@ -141,6 +142,9 @@ typedef struct {
 	s16   test_sample;
 	float sin;
 	float cos;
+	float pre_sin;
+	float pre_cos;
+	float s_preAngle;
 	u8    n_Error;
 	u32   n_CritiCalErrMask;
 	u32   n_CritiCalErrPrev;

Applications/foc/foc_config.h

@@ -1,7 +1,15 @@
 #ifndef _FOC_CONFIG_H__
 #define _FOC_CONFIG_H__
 
-
+#define CONFIG_DEFAULT_IDC_LIM 80
+#define CONFIG_DEFAULT_PHASE_CURR_LIM 200
+#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 /* 转把最大值 */

Applications/math/fast_math.c

@@ -3,6 +3,7 @@
 #include <stdlib.h>
 #include <arm_math.h>
 #include "math/fast_math.h"
+#include "os/os_types.h"
 #if 0
 /*
 生成sin/cos 查找表，(0-90°，间隔0.1°)
@@ -275,3 +276,68 @@ void TD_Init(TD_t *td, float wc, float DT) {
 }
 
 
+/**
+ * Fast atan2
+ *
+ * See http://www.dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization
+ *
+ * @param y
+ * y
+ *
+ * @param x
+ * x
+ *
+ * @return
+ * The angle in radians = arctan(y/x)
+ */
+float fast_arctan2(float y, float x) {
+	float abs_y = fabs(y) + 1e-20; // kludge to prevent 0/0 condition
+	float angle;
+
+	if (x >= 0) {
+		float r = (x - abs_y) / (x + abs_y);
+		float rsq = r * r;
+		angle = ((0.1963F * rsq) - 0.9817F) * r + (PI / 4.0F);
+	} else {
+		float r = (x + abs_y) / (abs_y - x);
+		float rsq = r * r;
+		angle = ((0.1963F * rsq) - 0.9817F) * r + (3.0F * PI / 4.0F);
+	}
+
+	UTILS_NAN_ZERO(angle);
+
+	if (y < 0) {
+		return(-angle);
+	} else {
+		return(angle);
+	}
+}
+
+#if 0
+float lp_compestion(float lp, float w, float sin, float cos) {
+	float H = 1.0f/sqrtf(1.0f + SQ(w/(float)CONFIG_CURR_LP_CUT_FREQ));
+	float delta_angle =  -fast_arctan2(w/(float)CONFIG_CURR_LP_CUT_FREQ)/(2*PI) * 360.0f;
+	float sin1, cos1;
+	SinCos_Lut(delta_angle, &sin1, &cos1);
+
+	float value = (sin/(sin*cos1 + cos*sin1)) * (lp / H);
+	return value;
+}
+#endif
+#if 1
+
+#else
+float lp_compestion(float lp, float w, float wc, float angle) {
+	static float A = 0.0f;
+	float H = 1.0f/sqrtf(1.0f + SQ(w/wc));
+	float delta_angle = -fast_arctan2(w, wc)/(2*PI) * 360.0f;
+	float sin1, cos1;
+	SinCos_Lut(delta_angle + angle, &sin1, &cos1);
+	if (sin1 == 0.0f) {
+		return A;
+	}
+	A = lp/(H * sin1);
+	return A;
+}
+
+#endif

Applications/math/fast_math.h

@@ -17,7 +17,15 @@
 #ifndef SQ
 #define SQ(x) ((x)*(x))
 #endif
+// 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))
+#define UTILS_NAN_ZERO(x)	(x = UTILS_IS_NAN(x) ? 0.0F : x)
+
 void fast_sincos(float angle, float *sin, float *cos);
+float fast_arctan2(float y, float x);
+void SinCos_Lut(float angle, float *s, float *c);
+
 static void fast_norm_angle(float *angle) {
 	*angle = fmodf(*angle, 360.0f);
 
@@ -66,6 +74,18 @@ static float limitRPM(float vel_limit, float vel_estimate, float vel_gain, float
 	return Tmax;
 }
 
+#define MAX_H 1.1F
+static float lp_compestion(float w, float wc) {
+	float comp = sqrtf(1.0f + SQ(w/wc));
+	if (comp > MAX_H) {
+		comp = MAX_H;
+	}else if (comp < -MAX_H) {
+		comp = -MAX_H;
+	}
+	return comp;
+}
+
+
 typedef struct {                         /* '<Root>/in' */
   float time;                         /* '<Root>/time' */
   float target;

Applications/math/fix_math.h

@@ -126,6 +126,4 @@ static __INLINE u16 Sqrt_Fix( u32 wInput )
   	return ( wtemprootnew );
 }
 
-void SinCos_Lut(float angle, float *s, float *c);
-
 #endif /* _FIX_MATH_H__ */