滑膜观测器使用arctan和pll实现，单独pll效果不好

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

Applications/app/app.c

@@ -114,8 +114,8 @@ static u32 _app_plot_task(void * args) {
 	//can_report_plot_values(0x45);
 	//can_plot3(PMSM_FOC_Get()->out.n_Duty[0], PMSM_FOC_Get()->out.n_Duty[1], PMSM_FOC_Get()->out.n_Duty[2]);
 	//can_plot2(PMSM_FOC_Get()->rtLim.rpmLimRamp.interpolation, PMSM_FOC_GetSpeed());
-	//can_plot2(PMSM_FOC_GetSpeed(), smo_observer_est_rpm());
-	can_plot2(PMSM_FOC_Get()->in.s_iABC[1], PMSM_FOC_Get()->in.s_iABC[2]);
+	can_plot2(PMSM_FOC_GetSpeed(), smo_observer_est_rpm());
+	//can_plot2(PMSM_FOC_Get()->in.s_iABC[1], PMSM_FOC_Get()->in.s_iABC[2]);
 #if 0
 	if (!_mc_start) {
 		_mc_start = true;

Applications/bsp/board_mc100_v1.h

@@ -32,7 +32,7 @@
 
 #define CONFIG_96V_MODE_VOL (55.0F)
 
-//#define CONFIG_SMO_OBSERVER 1
+#define CONFIG_SMO_OBSERVER 1
 
 #define SCHED_TIMER TIMER5
 #define SCHED_TIMER_RCU RCU_TIMER5

Applications/foc/core/PMSM_FOC_Core.c

@@ -236,7 +236,7 @@ void PMSM_FOC_CoreInit(void) {
 #ifdef CONFIG_SMO_OBSERVER
 	smo_observer_init(CONFIG_SMO_PLL_BANDWITH, CONFIG_SMO_LFP_WC, CONFIG_SMO_GAIN_K, CONFIG_SMO_SIGMOID_MAX);
 #endif
-	gFoc_Ctrl.plot_type = Plot_None;
+	gFoc_Ctrl.plot_type = Plot_SMO_OBS;
 }
 
 //#define PHASE_LFP_FIR
@@ -262,8 +262,6 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	gFoc_Ctrl.in.s_hallAngle = 0;
 	gFoc_Ctrl.in.s_motAngle = 0;
 #endif
-	SinCos_Lut(gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
-	
 	gFoc_Ctrl.in.s_motRPM = motor_encoder_get_speed();
 	gFoc_Ctrl.in.s_vDC = get_vbus_int();
 	//sample current
@@ -284,12 +282,12 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	gFoc_Ctrl.in.s_iABCFilter[0] = -(gFoc_Ctrl.in.s_iABCFilter[1] + gFoc_Ctrl.in.s_iABCFilter[2]);
 #endif
 	Clark(iabc[0], iabc[1], iabc[2], &iAB);
-
-	Park(&iAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealIdq);
-
 #ifdef CONFIG_SMO_OBSERVER
 	gFoc_Ctrl.in.s_smoAngle = smo_observer_update(gFoc_Ctrl.out.s_OutVAB.a, gFoc_Ctrl.out.s_OutVAB.b, iAB.a, iAB.b);
 #endif
+	SinCos_Lut(gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.sin, &gFoc_Ctrl.out.cos);
+
+	Park(&iAB, gFoc_Ctrl.in.s_motAngle, &gFoc_Ctrl.out.s_RealIdq);
 }
 
 static __INLINE void PMSM_FOC_Update_PI_Idq(void) {
@@ -389,13 +387,13 @@ void PMSM_FOC_Schedule(void) {
 				plot_3data16(FtoS16(gFoc_Ctrl.in.s_vABC[0]), FtoS16(gFoc_Ctrl.in.s_vABC[1]), FtoS16(gFoc_Ctrl.in.s_vABC[2]));
 			}else if (gFoc_Ctrl.plot_type == Plot_SMO_OBS) {
 #ifdef CONFIG_SMO_OBSERVER
-				float delta = gFoc_Ctrl.in.s_smoAngle - gFoc_Ctrl.in.s_motAngle;
-				if (delta > M_PI) {
-					delta -= 2 * M_PI;
-				}else if (delta < -M_PI) {
-					delta += 2 * M_PI;
+				float delta = gFoc_Ctrl.in.s_smoAngle - gFoc_Ctrl.in.s_hallAngle;
+				if (delta > 180) {
+					delta -= 360;
+				}else if (delta < -180) {
+					delta += 360;
 				}
-				plot_3data16(gFoc_Ctrl.in.s_motAngle, gFoc_Ctrl.in.s_smoAngle, delta);
+				plot_3data16(gFoc_Ctrl.in.s_hallAngle, gFoc_Ctrl.in.s_smoAngle, delta);
 #endif
 			}
 		}

Applications/foc/core/smo_observer.c

@@ -3,16 +3,20 @@
 #include "PMSM_FOC_Core.h"
 #include "smo_observer.h"
 
+#define USE_ARCTAN 1
 static smo_observer_t smo;
-
 static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta);
+#ifdef USE_ARCTAN
+static void smo_arctan(void);
+#else
 static void smo_pll(void);
+#endif
 void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta) {
-	smo.ts = 1.0f/(float)FOC_PWM_FS;
+	smo.ts = FOC_CTRL_US;
 	smo.bandwith = pll_bandwith;
-	smo.pll_kp = pll_bandwith * 2 * M_PI * 2;
+	smo.pll_kp = pll_bandwith * 2;
 	smo.pll_ki = 0.25f * SQ(smo.pll_kp);
-	smo.pll_max_rad_pers = CONFIG_MAX_MOT_RPM/30.0f * M_PI;
+	smo.pll_max_rad_pers = CONFIG_MAX_MOT_RPM/30.0f * M_PI * nv_get_motor_params()->poles;
 	smo.lpf_wc = lpf_wc;
 	smo.lpf_ceof = (lpf_wc*2*M_PI/(float)FOC_PWM_FS);
 	smo.Ksmo = Ksmo;
@@ -21,15 +25,22 @@ void smo_observer_init(float pll_bandwith, float lpf_wc, float Ksmo, float Ksta)
 	smo.motor_ld = nv_get_motor_params()->ld;
 	smo.motor_lq = nv_get_motor_params()->lq;
 	smo.motor_poles = nv_get_motor_params()->poles;
-
+	smo.dir_ccw = true;
 	smo.ldq_diff_dm = (smo.motor_ld-smo.motor_lq)/smo.motor_lq*smo.motor_ld;
 	smo.ld_inv = 1.0f / smo.motor_ld;
+
+	smo.pll.DT = smo.ts;
+	smo.pll.kp = pll_bandwith * 2;
+	smo.pll.ki = 0.25f * SQ(smo.pll.kp);
 }	
 
 float smo_observer_update(float uAlpha, float uBeta, float iAlpha, float iBeta) {
-
 	smo_observer(uAlpha, uBeta, iAlpha, iBeta);
+#ifdef USE_ARCTAN
+	smo_arctan();
+#else
 	smo_pll();
+#endif
 	return pi_2_degree(smo.est_angle_out);
 }
 
@@ -60,8 +71,41 @@ static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta) {
 
 }
 #define angle_clamp(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};
+#ifdef USE_ARCTAN
+static void smo_arctan(void) {
+	float ealpha_in = -smo.est_eAlpha_Filted;
+	float ebeta_in  =  smo.est_eBeta_Filted;
 
-#define ANGLE_COMP_RPM_CEOF (2.44E-4F)
+	float angle = fast_atan_2(ealpha_in, ebeta_in); //通过反正切获取电角度
+	angle_clamp(angle);
+	float prev_angle = smo.est_angle;
+	float comp_angle = 0.0f;
+	if (smo.dir_ccw) {
+		if (prev_angle > angle) {
+			comp_angle = 2 * M_PI;
+		}
+	}else {
+		if (prev_angle < angle) {
+			comp_angle = -2 * M_PI;
+		}
+	}
+	smo.est_angle = angle;
+	smo.est_rad_pers = PLL_run(&smo.pll, angle, comp_angle);
+	if (smo.est_rad_pers > smo.pll_max_rad_pers) {
+		smo.est_rad_pers = smo.pll_max_rad_pers;
+		smo.pll.out = smo.est_rad_pers;
+	}
+	smo.est_rad_pers_filted = smo.est_rad_pers;
+	smo.est_angle_out = smo.est_angle + fast_atan_2(smo.est_rad_pers_filted, smo.lpf_wc*2*M_PI);
+	angle_clamp(smo.est_angle_out);
+	smo.est_rpm = (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
+	if (smo.est_rpm > CONFIG_MAX_MOT_RPM) {
+		smo.est_rpm = CONFIG_MAX_MOT_RPM;
+	}else if (smo.est_rpm < 0) {
+		smo.est_rpm = 0;
+	}
+}
+#else
 static void smo_pll(void) {
 	float eab_sqr = sqrtf(SQ(smo.est_eAlpha_Filted) + SQ(smo.est_eBeta_Filted) + (1e-10f));
 	float ealpha_in = -smo.est_eAlpha_Filted/eab_sqr;
@@ -86,12 +130,11 @@ static void smo_pll(void) {
 		smo.est_rpm = 0;
 	}
 	/* 对低通进行角度补偿 */
-	smo.est_angle_out = smo.est_angle;// + fast_arctan2(smo.est_rad_pers_filted, smo.lpf_wc*2*M_PI);
-	/* 通过速度对角度再次补偿, ANGLE_COMP_RPM_CEOF 这个值通过校准获取 */
-	smo.est_angle_out += smo.est_rpm * ANGLE_COMP_RPM_CEOF;
+	smo.est_angle_out = smo.est_angle + fast_atan_2(smo.est_rad_pers_filted, smo.lpf_wc*2*M_PI);
+	//smo.est_angle_out = fast_atan_2(ealpha_in, ebeta_in) + fast_atan_2(smo.est_rad_pers_filted, smo.lpf_wc*2*M_PI);
 	angle_clamp(smo.est_angle_out);
 }
-
+#endif
 
 float smo_observer_est_angle(void) {
 	return pi_2_degree(smo.est_angle_out);

Applications/foc/core/smo_observer.h

@@ -10,6 +10,8 @@ typedef struct {
 	float Ksmo;      //滑膜增益
 	float Ksta;      //sigmoid 函数最大饱和
 
+	PLL_t pll;
+
 	float pll_kp;
 	float pll_ki;
 	float pll_integrator;
@@ -21,6 +23,8 @@ typedef struct {
 	float est_eAlpha_Filted;
 	float est_eBeta_Filted;
 
+	bool  dir_ccw;
+
 	float est_angle; //degree
 	float est_angle_out;
 	float est_rad_pers; //每秒度数

Applications/foc/foc_config.h

@@ -5,7 +5,7 @@
 
 #define CONFIG_DEFAULT_IDC_LIM 45
 #define CONFIG_DEFAULT_PHASE_CURR_LIM 200
-#define CONFIG_DEFAULT_RPM_LIM       3500
+#define CONFIG_DEFAULT_RPM_LIM       7000
 #define CONFIG_DEFAULT_LOCK_PHASE_CURR_LIM 100
 
 #define CONFIG_DEFAULT_EPM_PHASE_CURR 50
@@ -58,11 +58,12 @@
 #define CONFIG_MAX_NEG_CURRENT 2.0F
 
 #ifdef CONFIG_SMO_OBSERVER
-	#define CONFIG_SMO_MIN_SPEED    1200 //RPM
-	#define CONFIG_SMO_PLL_BANDWITH 2000.0f
+	#define CONFIG_SMO_MIN_SPEED    500 //RPM
+	//#define CONFIG_SMO_PLL_BANDWITH 2000.0f //计算角度和速度的pll
+	#define CONFIG_SMO_PLL_BANDWITH 100.0f //计算速度的pll
 	#define CONFIG_SMO_LFP_WC       50.0F
-	#define CONFIG_SMO_GAIN_K            1.0F
-	#define CONFIG_SMO_SIGMOID_MAX   90.0F
+	#define CONFIG_SMO_GAIN_K        1.0F
+	#define CONFIG_SMO_SIGMOID_MAX   100.0F
 #endif
 #endif /* _FOC_CONFIG_H__ */
 

Applications/math/fast_math.c

@@ -275,44 +275,6 @@ void TD_Init(TD_t *td, float wc, float DT) {
 	td->time = wc *wc;
 }
 
-
-/**
- * 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));

Applications/math/fast_math.h

@@ -23,7 +23,6 @@
 #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);
 
 #define MATH_sat(in, minOut, maxOut) (min((maxOut), MAX((in), (minOut))))
@@ -113,6 +112,28 @@ static __INLINE float fast_atan2(float y, float x) {
 	}
 }
 
+static __INLINE float fast_atan_2(float y, float x) {
+    // a := min (|x|, |y|) / max (|x|, |y|)
+    float abs_y = ABS(y);
+    float abs_x = ABS(x);
+    // inject FLT_MIN in denominator to avoid division by zero
+    float a = min(abs_x, abs_y) / (MAX(abs_x, abs_y) + 1e-20f);
+    // s := a * a
+    float s = a * a;
+    // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
+    float r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;
+    // if |y| > |x| then r := 1.57079637 - r
+    if (abs_y > abs_x)
+        r = 1.57079637f - r;
+    // if x < 0 then r := 3.14159274 - r
+    if (x < 0.0f)
+        r = 3.14159274f - r;
+    // if y < 0 then r := -r
+    if (y < 0.0f)
+        r = -r;
+
+    return r;
+}
 
 static void normal_sincosf(float angle, float *sin, float *cos) {
 	*sin = arm_sin_f32(angle);
@@ -138,9 +159,10 @@ static void normal_sincosf(float angle, float *sin, float *cos) {
  * @param filter_constant
  * Filter constant. Range 0.0 to 1.0, where 1.0 gives the unfiltered value.
  */
-//#define LowPass_Filter(value, sample, filter_constant)	(value = value * (1.0f - filter_constant) + sample * filter_constant)
+/* 前向差分离散化 */
 #define LowPass_Filter(value, sample, filter_constant)	(value = ((float)sample - (float)value) * filter_constant + value)
-#define do_lpf(value, sample, filter_constant)	(((float)sample - (float)value) * filter_constant + value)
+/* 后向差分离散化 */
+#define do_lpf(value, sample, filter_constant)	((sample * filter_constant + value)/(1.0f + filter_constant))
 
 
 static float limitRPM(float vel_limit, float vel_estimate, float vel_gain, float torque) {