高频注入改为方波注入，通过PLL锁相环取角度和速度

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

Applications/app/app.c

@@ -135,7 +135,7 @@ static u32 app_plot_task(void * args) {
 		}
 		can_plot2(plot_arg1, plot_arg2);
 	}else if (plot_type == 2) {
-		can_plot2(mot_contrl_get_final_torque(&motor.controller), (s16)motor.controller.hfi.vel_integrator);
+		can_plot2(mot_contrl_get_final_torque(&motor.controller), (s16)motor.controller.hfi.rpm_vel);
 	}else if (plot_type == 3) {
 		plot_sensorless_angle();
 	}else if (plot_type == 4) {

Applications/foc/core/controller.c

@@ -84,6 +84,7 @@ bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
 	return true;
 }
 
+#if 0
 void mot_contrl_enable_hfi(mot_contrl_t *ctrl, bool enable) {
 	hfi_t *hfi = &ctrl->hfi;
 	if (enable) {
@@ -98,7 +99,28 @@ void mot_contrl_enable_hfi(mot_contrl_t *ctrl, bool enable) {
 	}
 	ctrl->b_hfi = enable;
 }
-
+#else
+#define HFI_PLL_BAND 100.0F
+void mot_contrl_enable_hfi(mot_contrl_t *ctrl, bool enable) {
+	hfi_t *hfi = &ctrl->hfi;
+	if (enable) {
+		PI_Controller_Reset(&hfi->pi, 0);
+		hfi->elec_vel = 0;//ctrl->foc.mot_vel_radusPers;
+		hfi->angle_deg = 0;//ctrl->foc.in.mot_angle;
+		hfi->rad_integrator = 0;//degree_2_pi(hfi->angle_deg);
+		hfi->v_inj = 15.0f;
+		hfi->pi.ts = ctrl->foc.ts;
+		hfi->pi.kp = 2 * HFI_PLL_BAND;
+		hfi->pi.ki = SQ(HFI_PLL_BAND);
+		hfi->pi.kd = 0;
+		hfi->pi.max = 2000.0f/30*M_PI*mc_conf()->m.poles;
+		hfi->pi.min = -hfi->pi.max;
+		hfi->sign = -1;
+		hfi->rpm_vel = 0;//ctrl->foc.in.mot_velocity;
+	}
+	ctrl->b_hfi = enable;
+}
+#endif
 bool mot_contrl_request_mode(mot_contrl_t *ctrl, u8 mode) {
 	if (mode > CTRL_MODE_EBRAKE) {
 		mot_contrl_set_error(ctrl, FOC_Param_Err);
@@ -229,7 +251,8 @@ static __INLINE void phase_curr_unbal_check(mot_contrl_t *ctrl) {
 	}
 }
 
-#define HFI_DEBUG_ON 1
+#define HFI_DEBUG_ON 0
+#if 0
 static void mot_contrl_hfi_alpha_beta(mot_contrl_t *ctrl) {
 	hfi_t *hfi = &ctrl->hfi;
 	float sin, cos;
@@ -260,7 +283,56 @@ static void mot_contrl_hfi_alpha_beta(mot_contrl_t *ctrl) {
 	}
 	hfi->b_sample = !hfi->b_sample;
 }
+#else
+static __INLINE void hfi_pll_run(hfi_t *hfi, float alpha, float beta) {
+	float mag = NORM2_f(alpha, beta) + 1e-10f;
+	alpha /= mag;
+	beta  /= mag;
+	float sin, cos;
+	arm_sin_cos(hfi->angle_deg, &sin, &cos);
+	float err = alpha * sin - beta * cos;
+	hfi->elec_vel = PI_Controller_Run(&hfi->pi, err);
+	hfi->rad_integrator += hfi->elec_vel * hfi->pi.ts;
+	norm_angle_rad(hfi->rad_integrator);
+	hfi->angle_deg = pi_2_degree(hfi->rad_integrator);
+}
 
+static void mot_contrl_hfi_alpha_beta(mot_contrl_t *ctrl) {
+	hfi_t *hfi = &ctrl->hfi;
+	float sin, cos;
+	float sign = hfi->sign;
+#if HFI_DEBUG_ON
+	arm_sin_cos(hfi->angle_deg, &sin, &cos);
+#else
+	sin = ctrl->foc.sin;
+	cos = ctrl->foc.cos;
+#endif
+	if (hfi->sign > 0) {
+		hfi->sign = -1;
+	}else {
+		hfi->sign = 1;
+	}
+
+	float hi_alpha = (ctrl->foc.in.curr_ab.a - hfi->sample_prev.a) * 0.5f * sign;
+	float hi_beta = (ctrl->foc.in.curr_ab.b - hfi->sample_prev.b) * 0.5f * sign;
+
+	float alpha = (ctrl->foc.in.curr_ab.a + hfi->sample_prev.a) * 0.5f;
+	float beta = (ctrl->foc.in.curr_ab.b + hfi->sample_prev.b) * 0.5f;
+
+	hfi_pll_run(hfi, hi_alpha, hi_beta);
+
+	hfi->v_alpha_inj = hfi->v_inj * cos * hfi->sign;
+	hfi->v_beta_inj = hfi->v_inj * sin  * hfi->sign;
+	hfi->sample_prev.a = ctrl->foc.in.curr_ab.a;
+	hfi->sample_prev.b = ctrl->foc.in.curr_ab.b;
+
+	ctrl->foc.in.curr_ab.a = alpha;
+	ctrl->foc.in.curr_ab.b = beta;
+
+	hfi->rpm_vel = LowPass_Filter(hfi->rpm_vel, (hfi->elec_vel/M_PI*30/mc_conf()->m.poles) ,0.005f);
+}
+
+#endif
 bool mot_contrl_update(mot_contrl_t *ctrl) {
 	foc_t *foc = &ctrl->foc;
 

Applications/foc/core/controller.h

@@ -115,6 +115,7 @@ typedef struct {
 	float torque;
 }prot_limit;
 
+#if 0
 typedef struct {
 	bool b_sample;
 	s16  v_inj;
@@ -131,7 +132,21 @@ typedef struct {
 	float int_gain;
 	float inv_ld_lq;
 }hfi_t;
+#else
+typedef struct {
+	float sign;
+	s16  v_inj;
+	float v_alpha_inj;
+	float v_beta_inj;
+	albt_t sample_prev;
+	float rad_integrator;
+	float angle_deg;
+	float elec_vel;
+	float rpm_vel;
+	PI_Controller pi;
+}hfi_t;
 
+#endif
 typedef struct{
 	bool 			b_start;
 	bool			b_ebrk_running;

Applications/math/fast_math.h

@@ -21,7 +21,6 @@
 #endif
 
 #define NORM2_f(x,y)		(sqrtf(SQ(x) + SQ(y)))
-
 // 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))