rad和degree的角度限制一个周期的接口统一放到fast_math.h中，norm_angle_rad/deg

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

Applications/foc/core/controller.c

@@ -213,7 +213,6 @@ static __INLINE void phase_curr_unbal_check(mot_contrl_t *ctrl) {
 		a_max = b_max = c_max = 0;
 	}
 }
-#define norm_rad(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};
 
 static __INLINE void mot_contrl_update_phase_vol(mot_contrl_t *ctrl) {
 	float phase_vol[3];
@@ -232,7 +231,7 @@ static __INLINE void mot_contrl_update_phase_vol(mot_contrl_t *ctrl) {
 	float angle_rad = fast_atan_2(We_hz, PHASE_VOL_LPF_BAND);
 	/* 延时半周期*/
 	angle_rad += (We_hz * 2 * M_PI * ctrl->foc.ts / 2);
-	norm_rad(angle_rad);
+	norm_angle_rad(angle_rad);
 	/* 补偿 alpha-beta的相位 */
 	float sin,cos;
 	arm_sin_cos(-pi_2_degree(angle_rad), &sin, &cos);

Applications/foc/core/foc.c

@@ -93,7 +93,7 @@ void foc_update(foc_t *foc) {
 #ifdef CONFIG_Volvec_Delay_Comp
 	if (foc->mot_velocity_filterd >= CONFIG_Volvec_Delay_Comp_Start_Vel) {
 		float vol_dq_angle = foc->in.mot_angle + foc->mot_velocity_filterd / PI * 180.0f * (foc->ts * 0.8f);
-		rand_angle(vol_dq_angle);
+		norm_angle_deg(vol_dq_angle);
 		arm_sin_cos(vol_dq_angle, &foc->sin, &foc->cos);
 	}
 #endif

Applications/foc/core/ladrc_observer.c

@@ -5,8 +5,6 @@
 
 static ladrc_observer observer;
 
-#define angle_clamp(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};
-
 static __inline float ladrc_observer_band(float vel) {
 	float ration = vel / observer.vel_min;
 	float Wo = observer.Wo;
@@ -80,7 +78,7 @@ float ladrc_observer_update(float va, float vb, float ia, float ib) {
 
 	float angle = fast_atan_2(-observer.Ealpha, observer.Ebeta);
 	UTILS_NAN_ZERO(angle);
-	angle_clamp(angle);
+	norm_angle_rad(angle);
 
 	/* 速度计算 */
 	float delta_angle = angle - observer.angle_atan;
@@ -106,7 +104,7 @@ float ladrc_observer_update(float va, float vb, float ia, float ib) {
 	angle = fast_atan_2(observer.Vel_El, Wo) * 2.0f;
 	/* 电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
 	observer.angle_out = observer.angle_atan + (angle + 0/*observer.Vel_El * observer.ts*/);
-	angle_clamp(observer.angle_out);
+	norm_angle_rad(observer.angle_out);
 
 	LowPass_Filter(observer.Vel_El_filter, observer.Vel_El, 0.01f); //需要再加一级低通滤波，给计算Wo和输出使用
 	

Applications/foc/core/smo_observer.c

@@ -85,7 +85,7 @@ static void smo_observer(float uAlpha, float uBeta, float iAlpha, float iBeta) {
 	smo.est_eBeta_Filted = smo.est_eBeta;
 #endif
 }
-#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;
@@ -93,7 +93,7 @@ static void smo_arctan(void) {
 
 	float angle = fast_atan_2(ealpha_in, ebeta_in); //通过反正切获取电角度
 	UTILS_NAN_ZERO(angle);
-	angle_clamp(angle);
+	norm_angle_rad(angle);
 	float prev_angle = smo.est_angle;
 	float comp_angle = 0.0f;
 	if (smo.dir_ccw) {
@@ -114,7 +114,7 @@ static void smo_arctan(void) {
 	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) + smo.est_rad_pers_filted * smo.ts;
-	angle_clamp(smo.est_angle_out);
+	norm_angle_rad(smo.est_angle_out);
 	smo.est_rpm = (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
 	if (smo.est_rpm > CONFIG_HW_MAX_MOTOR_RPM) {
 		smo.est_rpm = CONFIG_HW_MAX_MOTOR_RPM;
@@ -140,7 +140,7 @@ static void smo_pll(void) {
 	smo.est_angle += smo.ts * smo.est_rad_pers; //角速度积分
 	
 	smo.est_rad_pers_filted = do_lpf(smo.est_rad_pers_filted, smo.est_rad_pers, smo.lpf_ceof); //对速度低通滤波
-	angle_clamp(smo.est_angle);
+	norm_angle_rad(smo.est_angle);
 	smo.est_rpm = (30.0f * smo.est_rad_pers_filted/M_PI/smo.motor_poles);
 	if (smo.est_rpm > CONFIG_HW_MAX_MOTOR_RPM) {
 		smo.est_rpm = CONFIG_HW_MAX_MOTOR_RPM;
@@ -149,7 +149,7 @@ static void smo_pll(void) {
 	}
 	/* 电流和电压滞后一个控制周期，需要通过当前的电角速度对计算的角度进行补偿 */
 	smo.est_angle_out = smo.est_angle + smo.est_rad_pers_filted * smo.ts;
-	angle_clamp(smo.est_angle_out);
+	norm_angle_rad(smo.est_angle_out);
 }
 #endif
 

Applications/foc/motor/encoder.c

@@ -291,7 +291,7 @@ float encoder_get_theta(bool detect_err) {
 	float angle_count = cnt + g_encoder.align_step + g_encoder.interpolation;
 	g_encoder.abi_angle = ENC_Pluse_Nr_2_angle(angle_count) * g_encoder.motor_poles + g_encoder.enc_offset;
 	g_encoder.abi_angle += _eccentricity_compensation(cnt);
-	rand_angle(g_encoder.abi_angle);
+	norm_angle_deg(g_encoder.abi_angle);
 	g_encoder.last_cnt = cnt;
 	g_encoder.last_us = task_get_usecond();
 
@@ -471,7 +471,7 @@ void ENC_PWM_Duty_Handler(float t, float d) {
 		g_encoder.pwm_count = n_nr;
 	}
 	g_encoder.pwm_angle = ENC_Pluse_Nr_2_angle(Nr) * g_encoder.motor_poles + g_encoder.enc_offset;	
-	rand_angle(g_encoder.pwm_angle);
+	norm_angle_deg(g_encoder.pwm_angle);
 	if (!g_encoder.b_index_found && pwm_count++ >= 10) {
 		encoder_sync_pwm_abs();
 	}
@@ -500,7 +500,7 @@ bool ENC_Check_error(void) {
 float encoder_get_pwm_angle(void) {
 #ifdef ENCODER_CC_INVERT
 	g_encoder.pwm_angle = 360.0f - (g_encoder.pwm_angle - g_encoder.enc_offset) + g_encoder.enc_offset;
-	rand_angle(g_encoder.pwm_angle);
+	norm_angle_deg(g_encoder.pwm_angle);
 #endif
 	return g_encoder.pwm_angle;
 }
@@ -508,7 +508,7 @@ float encoder_get_pwm_angle(void) {
 float encoder_get_abi_angle(void) {
 	s16 cnt = _abi_count();
 	float angle = ENC_Pluse_Nr_2_angle((float)(cnt + g_encoder.align_step)) * g_encoder.motor_poles + g_encoder.enc_offset;
-	rand_angle(angle);
+	norm_angle_deg(angle);
 	return angle;
 }
 

Applications/foc/motor/hall.c

@@ -141,7 +141,7 @@ float hall_sensor_get_theta(bool detect_err){
 	}else {
 		el_angle = _sensor_hander.estimate_el_angle - el_angle;
 	}
-	rand_angle(el_angle);
+	norm_angle_deg(el_angle);
 	if (hall_e_count%5 == 0) {
 		plot_2data16((s16)el_angle, _sensor_hander.hall_stat*10);
 	}
@@ -164,7 +164,7 @@ float hall_sensor_get_theta(void){
 		_sensor_hander.estimate_delta_angle -= angle_add;
 		_sensor_hander.estimate_el_angle -= (angle_add + comp_angle);
 	}
-	rand_angle(_sensor_hander.estimate_el_angle);
+	norm_angle_deg(_sensor_hander.estimate_el_angle);
 	test_loop ++;
 	if (test_loop % 20 == 0) {
 		//plot_3data16(_sensor_hander.estimate_el_angle, _sensor_hander.angle_comp_ts * 1000, _sensor_hander.comp_count);
@@ -278,7 +278,7 @@ static void _hall_init_el_angle(void) {
 	_sensor_hander.sensor_error = 0;
   	/* Initialize the measured angle */
 	_sensor_hander.measured_el_angle += PHASE_60_DEGREE;
-	rand_angle(_sensor_hander.measured_el_angle);
+	norm_angle_deg(_sensor_hander.measured_el_angle);
   	_sensor_hander.estimate_el_angle = _sensor_hander.measured_el_angle;
 	_sensor_hander.hall_ticks = task_ticks_abs();
 }
@@ -353,7 +353,7 @@ static float _hall_position(u8 state_now, u8 state_prev) {
 			return 0xFFFFFFFF;
 	}
 	if (theta_now != 0xFFFFFFFF) {
-		rand_angle(theta_now);
+		norm_angle_deg(theta_now);
 	}
 	return theta_now;
 }

Applications/foc/motor/mot_params_ind.c

@@ -202,7 +202,7 @@ void mot_params_high_freq_inject(void) {
 		return;
 	}
 	float hj_angle = hj_w * (float)n_samples;
-	rand_angle(hj_angle);
+	norm_angle_deg(hj_angle);
 	float s, c;
 	arm_sin_cos(hj_angle, &s, &c);
 	float vd = 0, vq = 0;

Applications/foc/motor/motor.c

@@ -1441,7 +1441,7 @@ static bool mc_process_force_running(void) {
 				--_force_wait;
 			}else {
 				_force_angle += 1.5f;
-				rand_angle(_force_angle);
+				norm_angle_deg(_force_angle);
 				mot_contrl_set_angle(&motor.controller, _force_angle);
 			}
 		}

Applications/math/fast_math.h

@@ -169,6 +169,8 @@ static void normal_sincosf(float angle, float *sin, float *cos) {
 }
 #define degree_2_pi(d) ((float)(d) * M_PI / 180.0f)
 #define pi_2_degree(d) ((float)(d) * 180.0f / M_PI)
+#define norm_angle_rad(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};
+#define norm_angle_deg(a) {while (a >= 360.0f) a-=360.0f;while (a < 0) a +=360.0f;};
 
 #define INVALID_ANGLE 0x3DFF
 

Applications/os/os_types.h

@@ -48,7 +48,6 @@ typedef unsigned int	uint32;
 #define PHASE_300_DEGREE (300.0f)
 #define PHASE_360_DEGREE (360.0f)
 
-#define rand_angle(a) {while (a >= PHASE_360_DEGREE) a-=PHASE_360_DEGREE;while (a < 0) a +=PHASE_360_DEGREE;};
 #define ABS(x) 					( (x)>0?(x):-(x) )
 
 #define MAX_S16 (0x7FFF)