update HFI

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

Applications/app/app.c

@@ -111,15 +111,16 @@ static u32 app_report_task(void *p) {
 
 int plot_type = 0;
 static void plot_sensorless_angle(void) {
+	float mot_angle = encoder_get_abi_angle();
 	float est_angle = foc_observer_sensorless_angle();
 	if (motor.controller.b_hfi) {
 		est_angle = motor.controller.hfi.angle_deg;
 	}
-	float delta = est_angle - foc()->in.mot_angle;
+	float delta = est_angle - mot_angle;
 	float s, c;
 	arm_sin_cos(delta, &s, &c);
 	delta = fast_atan2(s, c)/PI*180.0f;
-	can_plot3(foc()->in.mot_angle, est_angle, delta);
+	can_plot3(mot_angle, est_angle, delta);
 }
 
 static u32 app_plot_task(void * args) {
@@ -135,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.rpm_vel);
+		can_plot2(mot_contrl_get_final_torque(&motor.controller), (s16)motor.controller.hfi.elec_vel);
 	}else if (plot_type == 3) {
 		plot_sensorless_angle();
 	}else if (plot_type == 4) {

Applications/foc/core/controller.c

@@ -53,6 +53,7 @@ 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);
 	}
 	ctrl->b_ebrk_running = false;
 	ctrl->b_AutoHold = false;
@@ -95,32 +96,40 @@ void mot_contrl_enable_hfi(mot_contrl_t *ctrl, bool enable) {
 		hfi->vel_integrator = -ctrl->foc.in.mot_velocity;
 		hfi->angle_deg = ctrl->foc.in.mot_angle;
 		hfi->angle_rad = degree_2_pi(hfi->angle_deg);
-		hfi->v_inj = 10.0f;
+		hfi->v_inj = 20.0f;
 	}
 	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
 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->angle_deg = encoder_get_abi_angle();
+		hfi->rad_integrator = degree_2_pi(hfi->angle_deg);
+		hfi->v_inj = HFI_IPD_V;
+		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);
 		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;
+		hfi->rpm_vel = encoder_get_speed();
+		hfi->elec_vel = hfi->rpm_vel / 30.0f * PI * mc_conf()->m.poles;
+		hfi->pi.ui = hfi->elec_vel;
+		hfi->ipd = true;
+		hfi->hfi_ready = false;
 	}
+	hfi->n_samples = 0;
 	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);
@@ -255,16 +264,9 @@ static __INLINE void phase_curr_unbal_check(mot_contrl_t *ctrl) {
 #if 0
 static void mot_contrl_hfi_alpha_beta(mot_contrl_t *ctrl) {
 	hfi_t *hfi = &ctrl->hfi;
-	float sin, cos;
-#if HFI_DEBUG_ON
-	arm_sin_cos(hfi->angle_deg, &sin, &cos);
-#else
-	sin = ctrl->foc.sin;
-	cos = ctrl->foc.cos;
-#endif
-	
+	foc_t *foc = &ctrl->foc;
 	if (hfi->b_sample) {
-		float sample_now = cos * ctrl->foc.in.curr_ab.b - sin * ctrl->foc.in.curr_ab.a;
+		float sample_now = foc->cos * ctrl->foc.in.curr_ab.b - foc->sin * ctrl->foc.in.curr_ab.a;
 		LowPass_Filter(hfi->sample_now, sample_now, 1.0f);
 		float di = hfi->sample_prev - hfi->sample_now;
 		float err = di / ctrl->foc.ts / (hfi->v_inj * hfi->inv_ld_lq);
@@ -273,13 +275,15 @@ static void mot_contrl_hfi_alpha_beta(mot_contrl_t *ctrl) {
 		hfi->angle_rad -= 2.0f * ctrl->foc.ts * (err * hfi->int_gain + hfi->vel_integrator);
 		norm_angle_rad(hfi->angle_rad);
 		hfi->angle_deg = pi_2_degree(hfi->angle_rad);
-		hfi->v_alpha_inj = -hfi->v_inj * cos;
-		hfi->v_beta_inj = -hfi->v_inj * sin;
+		foc->in.mot_angle = hfi->angle_deg;
+		arm_sin_cos(foc->in.mot_angle, &foc->sin, &foc->cos);
+		hfi->v_alpha_inj = -hfi->v_inj * foc->cos;
+		hfi->v_beta_inj = -hfi->v_inj * foc->sin;
 	}else {
-		float sample_now = cos * ctrl->foc.in.curr_ab.b - sin * ctrl->foc.in.curr_ab.a;
+		float sample_now = foc->cos * ctrl->foc.in.curr_ab.b - foc->sin * ctrl->foc.in.curr_ab.a;
 		LowPass_Filter(hfi->sample_prev, sample_now, 1.0f);
-		hfi->v_alpha_inj = hfi->v_inj * cos;
-		hfi->v_beta_inj = hfi->v_inj * sin;
+		hfi->v_alpha_inj = hfi->v_inj * foc->cos;
+		hfi->v_beta_inj = hfi->v_inj * foc->sin;
 	}
 	hfi->b_sample = !hfi->b_sample;
 }
@@ -299,14 +303,9 @@ static __INLINE void hfi_pll_run(hfi_t *hfi, float alpha, float beta) {
 
 static void mot_contrl_hfi_alpha_beta(mot_contrl_t *ctrl) {
 	hfi_t *hfi = &ctrl->hfi;
-	float sin, cos;
+	foc_t *foc = &ctrl->foc;
 	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 {
@@ -321,15 +320,34 @@ static void mot_contrl_hfi_alpha_beta(mot_contrl_t *ctrl) {
 
 	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;
+	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);
+		hfi->v_inj = v_inj;
+	}else {
+		float v_inj = hfi->v_inj;
+		step_towards(&v_inj, HFI_IPD_V, 5.0f);
+		hfi->v_inj = v_inj;
+		if (hfi->n_samples < HFI_SAMPLE_NUM) {
+			hfi->n_samples++;
+		}
+		if (hfi->n_samples >= HFI_SAMPLE_NUM) {
+			hfi->ipd = false;
+			hfi->hfi_ready = true;
+		}
+	}
+	hfi->v_alpha_inj = hfi->v_inj * foc->cos * hfi->sign;
+	hfi->v_beta_inj = hfi->v_inj * foc->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);
+	hfi->rpm_vel = LowPass_Filter(hfi->rpm_vel, (hfi->elec_vel/M_PI*30/mc_conf()->m.poles) ,0.004f);
 }
 
 #endif
@@ -368,12 +386,13 @@ bool mot_contrl_update(mot_contrl_t *ctrl) {
 		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

@@ -135,7 +135,10 @@ typedef struct {
 #else
 typedef struct {
 	float sign;
-	s16  v_inj;
+	bool ipd; //初始位置检测
+	bool hfi_ready;
+	float  v_inj;
+	int  n_samples;
 	float v_alpha_inj;
 	float v_beta_inj;
 	albt_t sample_prev;
@@ -331,5 +334,9 @@ static __INLINE s16 mot_contrl_get_speed_abs(mot_contrl_t *ctrl) {
 	return ABS(speed);
 }
 
+static __INLINE bool mot_contrl_hfi_is_ipd(mot_contrl_t *ctrl) {
+	return true; //ctrl->hfi.ipd;
+}
+
 #endif /* _CONTROLLER_H__ */
 

Applications/foc/foc_config.h

@@ -61,7 +61,7 @@
 #define CONFIG_Volvec_Delay_Comp /* 电压矢量角度补偿 */
 #define CONFIG_Volvec_Delay_Comp_Start_Vel 500 // rpm
 
-#define CONFIG_ENABLE_IAB_REC 0   // for phase current debug
+#define CONFIG_ENABLE_IAB_REC 1   // for phase current debug
 
 #ifdef CONFIG_SPEED_LADRC
 	#define CONFIG_LADRC_Wo  200.0F

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];
-		ib[iab_w_count] = (s16)motor.controller.foc.in.curr_abc[1];
+		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);
 		iab_w_count ++;
 	}
 #endif
@@ -1418,6 +1418,9 @@ static void mc_process_epm_move(void) {
 	if (!motor.b_epm || (motor.epm_dir == EPM_Dir_None)){
 		return;
 	}
+	if (mot_contrl_hfi_is_ipd(mot_contrl())) {
+		return;
+	}
 	float target_vel = mc_conf()->c.max_epm_rpm;
 	float target_trq = mc_conf()->c.max_epm_torque;
 	if (motor.epm_dir == EPM_Dir_Back) {
@@ -1572,6 +1575,9 @@ static void mc_process_throttle_torque(float vol) {
 	}
 
 	if (motor.controller.mode_running == CTRL_MODE_TRQ) {
+		if (mot_contrl_hfi_is_ipd(mot_contrl())) {
+			return;
+		}
 		throttle_set_torque(&motor.controller, torque);
 	}else if (motor.controller.mode_running == CTRL_MODE_SPD) {
 		if (!mc_is_cruise_enabled()) {