编码器jitter判断提取出来, 使用fclamp替代最大最小范围处理

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

Applications/foc/motor/encoder.c

@@ -137,6 +137,12 @@ static __INLINE float _eccentricity_compensation(int cnt) {
 #endif
 }
 
+#define MAX_CPR_CNT_PER_CTL 40
+static __INLINE bool encoder_ab_is_jitter(s16 cnt) {
+	return ((cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2)) ||
+					(cnt < (MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt < (MAX_CPR_CNT_PER_CTL*2)));
+}
+
 #define CONFIG_ENC_DETECT_ERR
 #define CONFIG_ENC_ERR_TEST
 #define CONFIG_ENC_DIANOSTIC_MIN_CNT (10.0F * ENC_MAX_RES * FOC_CTRL_US) //600rpm, 每隔控制周期 0.2232 机械角度
@@ -150,7 +156,7 @@ static s16 enc_test1 = 0;
 static s16 enc_test2 = 0;
 static s16 enc_test3 = 0;
 static s16 enc_test4 = 0;
-#define MAX_CPR_CNT_PER_CTL 40
+
 /* 9000RPM = 150 RPS = 150 * ENC_MAX_RES * FOC_CTRL_US = 39, 每个控制周期 39个count */
 static void encoder_detect_error(s16 cnt) {
 #ifdef CONFIG_ENC_DETECT_ERR
@@ -159,8 +165,7 @@ static void encoder_detect_error(s16 cnt) {
 		s16 delta_cnt = cnt - g_encoder.last_cnt;
 		bool skip = false;
 		if (g_encoder.b_timer_ov) {
-			bool is_jitter = ((cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2)) ||
-					(cnt < (MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt < (MAX_CPR_CNT_PER_CTL*2))); //需要处理低速在overflow附近震荡
+			bool is_jitter = encoder_ab_is_jitter(cnt); //需要处理低速在overflow附近震荡
 			if (!is_jitter) {
 				s16 com = _pll_over_comp();
 				if (com > 0) {
@@ -256,8 +261,7 @@ float encoder_get_theta(bool detect_err) {
 	__NOP();__NOP();__NOP();__NOP();
 	if (ENC_OverFlow()) {
 		cnt = _abi_count();
-		if((cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt > (ENC_MAX_RES - MAX_CPR_CNT_PER_CTL*2)) ||
-				(cnt < (MAX_CPR_CNT_PER_CTL*2) && g_encoder.last_cnt < (MAX_CPR_CNT_PER_CTL*2))) { //需要处理低速在overflow附近震荡
+		if(encoder_ab_is_jitter(cnt)) { //需要处理低速在overflow附近震荡
 			g_encoder.b_timer_ov = false;
 		}else {
 			g_encoder.b_timer_ov = true;
@@ -277,11 +281,7 @@ float encoder_get_theta(bool detect_err) {
 	}else {
 		if (cnt == g_encoder.last_cnt) {
 			g_encoder.interpolation += g_encoder.est_vel_cnt_filter * FOC_CTRL_US;
-			if (g_encoder.interpolation > ENC_MAX_interpolation) {
-				g_encoder.interpolation = ENC_MAX_interpolation;
-			}else if (g_encoder.interpolation < -ENC_MAX_interpolation) {
-				g_encoder.interpolation = -ENC_MAX_interpolation;
-			}
+			g_encoder.interpolation = fclamp(g_encoder.interpolation, -ENC_MAX_interpolation, ENC_MAX_interpolation);
 		}else {
 			g_encoder.interpolation = 0.0f;
 		}