hfi调试

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

Applications/app/app.c

@@ -136,7 +136,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.elec_vel);
+		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

@@ -53,8 +53,8 @@ bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
 		mot_contrl_pid(ctrl);
 		mot_contrl_ulimit(ctrl);
 		mot_contrl_rtlimit(ctrl);
-		mot_contrl_enable_hfi(ctrl, true);
 	}
+	mot_contrl_enable_hfi(ctrl, start);
 	ctrl->b_ebrk_running = false;
 	ctrl->b_AutoHold = false;
 	ctrl->b_cruiseEna = false;
@@ -101,17 +101,17 @@ void mot_contrl_enable_hfi(mot_contrl_t *ctrl, bool enable) {
 	ctrl->b_hfi = enable;
 }
 #else
-#define HFI_PLL_BAND 100.0F
-#define HFI_IPD_V 30.0F
-#define HFI_RUNNING_V 30.0F
-#define HFI_SAMPLE_NUM 5000
+#define HFI_PLL_BAND 50.0F
+#define HFI_IPD_V 20.0F
+#define HFI_RUNNING_V 10.0F
+#define HFI_SAMPLE_NUM 8000
 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->angle_deg = encoder_get_abi_angle();
 		hfi->rad_integrator = degree_2_pi(hfi->angle_deg);
-		hfi->v_inj = HFI_IPD_V;
+		hfi->v_inj = 0;
 		hfi->pi.ts = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
 		hfi->pi.kp = 2.0f * HFI_PLL_BAND;
 		hfi->pi.ki = SQ(HFI_PLL_BAND);
@@ -295,7 +295,8 @@ static __INLINE void hfi_pll_run(hfi_t *hfi, float alpha, float beta) {
 	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);
+	float vel_elec = PI_Controller_Run(&hfi->pi, err);
+	LowPass_Filter(hfi->elec_vel, vel_elec, 0.05f);
 	hfi->rad_integrator += hfi->elec_vel * hfi->pi.ts;
 	norm_angle_rad(hfi->rad_integrator);
 	hfi->angle_deg = pi_2_degree(hfi->rad_integrator);
@@ -318,18 +319,23 @@ static void mot_contrl_hfi_alpha_beta(mot_contrl_t *ctrl) {
 	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->hi_alpha_beta.a = hi_alpha;
+	hfi->hi_alpha_beta.b = hi_beta;
+
+
 	hfi_pll_run(hfi, hi_alpha, hi_beta);
+	hfi->rpm_vel = LowPass_Filter(hfi->rpm_vel, (hfi->elec_vel/M_PI*30/mc_conf()->m.poles) ,0.003f);
 
 	foc->in.mot_angle = hfi->angle_deg;
 	foc->in.mot_velocity = hfi->rpm_vel;
-	arm_sin_cos(foc->in.mot_angle, &foc->sin, &foc->cos);
+	
 	if (!hfi->ipd) {
 		float v_inj = hfi->v_inj;
-		step_towards(&v_inj, HFI_RUNNING_V, 1.0f);
+		step_towards(&v_inj, HFI_RUNNING_V, 0.1f);
 		hfi->v_inj = v_inj;
 	}else {
 		float v_inj = hfi->v_inj;
-		step_towards(&v_inj, HFI_IPD_V, 5.0f);
+		step_towards(&v_inj, HFI_IPD_V, 0.5f);
 		hfi->v_inj = v_inj;
 		if (hfi->n_samples < HFI_SAMPLE_NUM) {
 			hfi->n_samples++;
@@ -346,8 +352,6 @@ static void mot_contrl_hfi_alpha_beta(mot_contrl_t *ctrl) {
 
 	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.004f);
 }
 
 #endif
@@ -385,14 +389,12 @@ bool mot_contrl_update(mot_contrl_t *ctrl) {
 		foc->in.target_vol_dq.d = line_ramp_get_interp(&ctrl->ramp_target_vd);
 		foc->in.target_vol_dq.q = line_ramp_get_interp(&ctrl->ramp_target_vq);
 	}
-
-	
+	arm_sin_cos(foc->in.mot_angle, &foc->sin, &foc->cos);
 	if (ctrl->b_hfi) {
 		mot_contrl_hfi_alpha_beta(ctrl);
 		foc->in.v_ab_inj.a = ctrl->hfi.v_alpha_inj;
 		foc->in.v_ab_inj.b = ctrl->hfi.v_beta_inj;
 	}else {
-		arm_sin_cos(foc->in.mot_angle, &foc->sin, &foc->cos);
 		foc->in.v_ab_inj.a = 0;
 		foc->in.v_ab_inj.b = 0;
 	}

Applications/foc/core/controller.h

@@ -141,6 +141,7 @@ typedef struct {
 	int  n_samples;
 	float v_alpha_inj;
 	float v_beta_inj;
+	albt_t hi_alpha_beta;
 	albt_t sample_prev;
 	float rad_integrator;
 	float angle_deg;
@@ -335,7 +336,7 @@ static __INLINE s16 mot_contrl_get_speed_abs(mot_contrl_t *ctrl) {
 }
 
 static __INLINE bool mot_contrl_hfi_is_ipd(mot_contrl_t *ctrl) {
-	return true; //ctrl->hfi.ipd;
+	return ctrl->hfi.ipd;
 }
 
 #endif /* _CONTROLLER_H__ */

Applications/foc/foc_config.h

@@ -58,7 +58,7 @@
 #define CONFIG_PHASE_UNBALANCE_THROLD 4.0F
 #define CONFIG_PHASE_UNBALANCE_R      0.1F
 
-#define CONFIG_Volvec_Delay_Comp /* 电压矢量角度补偿 */
+//#define CONFIG_Volvec_Delay_Comp /* 电压矢量角度补偿 */
 #define CONFIG_Volvec_Delay_Comp_Start_Vel 500 // rpm
 
 #define CONFIG_ENABLE_IAB_REC 1   // for phase current debug

Applications/foc/motor/motor.c

@@ -1267,8 +1267,8 @@ void ADC_IRQHandler(void) {
 
 #if (CONFIG_ENABLE_IAB_REC==1)
 	if (b_iab_rec && (iab_w_count < CONFIG_IAB_REC_COUNT)) {
-		ia[iab_w_count] = (s16)(motor.controller.foc.in.curr_abc[0] * 10.0f);
-		ib[iab_w_count] = (s16)(motor.controller.foc.in.curr_abc[1] * 10.0f);
+		ia[iab_w_count] = (s16)(motor.controller.hfi.hi_alpha_beta.a * 10.0f);
+		ib[iab_w_count] = (s16)(motor.controller.hfi.hi_alpha_beta.b * 10.0f);
 		iab_w_count ++;
 	}
 #endif