静音HFI调试

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

Applications/app/app.c

@@ -51,7 +51,7 @@ void app_start(void){
 
 static void app_print_log(void) {
 	//sys_debug("Slow: %d - %d, err:%d, %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time, g_meas_MCTask.exec_max_error_time, g_meas_MCTask.exec_time_error);
-	//sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
+	sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
 	//sys_debug("FOC time err %d %d %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error, g_meas_foc.exec_max_error_time, g_meas_foc.exec_time_error);
 	//sys_debug("acc vol %d, bid %d\n", get_acc_vol(), gpio_board_id());
 	//sys_debug("throttle %f\n", get_throttle_float());
@@ -108,15 +108,20 @@ static u32 app_report_task(void *p) {
 	}
 	return 200;
 }
+
 int plot_type = 0;
-static void plot_smo_angle(void) {
-	float smo_angle = foc_observer_sensorless_angle();
-	float delta = smo_angle - foc()->in.mot_angle;
+static void plot_sensorless_angle(void) {
+	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 s, c;
 	arm_sin_cos(delta, &s, &c);
 	delta = fast_atan2(s, c)/PI*180.0f;
-	can_plot3(foc()->in.mot_angle, smo_angle, delta);
+	can_plot3(foc()->in.mot_angle, est_angle, delta);
 }
+
 static u32 app_plot_task(void * args) {
 	if (plot_type == 1) {
 		s16 plot_arg1 = (s16)foc_observer_sensorless_speed();
@@ -130,9 +135,9 @@ 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), mot_contrl()->target_torque);
+		can_plot2(mot_contrl_get_final_torque(&motor.controller), (s16)motor.controller.hfi.vel_integrator);
 	}else if (plot_type == 3) {
-		plot_smo_angle();
+		plot_sensorless_angle();
 	}else if (plot_type == 4) {
 		can_plot2((s16)foc()->out.curr_dq.d, (s16)foc()->out.curr_dq.q);
 	}else if (plot_type == 5) {

Applications/foc/commands.c

@@ -563,8 +563,8 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		case Foc_Use_SensorLess_Angle:
 		{
 			bool sensorless = decode_u8((u8 *)command->data)?true:false;
-			sys_debug("use smo %d\n", sensorless);
-#if 1
+			sys_debug("use sensorless %d\n", sensorless);
+#if 0
 			if (sensorless && mc_is_start() && foc_observer_sensorless_stable()) {
 				sys_debug("use smo %d\n", sensorless);
 				foc_observer_use_sensorless(sensorless);
@@ -573,10 +573,12 @@ static void process_foc_command(foc_cmd_body_t *command) {
 				foc_observer_use_sensorless(false);
 			}
 #else
-			if (sensorless && mc_is_start()){
-				motor_encoder_produce_error(sensorless);
+			if (sensorless && mc_is_start() && mot_contrl_get_speed_abs(&motor.controller) > CONFIG_ZERO_SPEED_RPM) {
+				sys_debug("use hfi %d\n", sensorless);
+				mot_contrl_enable_hfi(&motor.controller, sensorless);
 			}else {
-				motor_encoder_produce_error(false);
+				sys_debug("unuse hfi\n");
+				mot_contrl_enable_hfi(&motor.controller, false);
 			}
 #endif
 			break;

Applications/foc/core/controller.c

@@ -84,6 +84,21 @@ bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
 	return true;
 }
 
+void mot_contrl_enable_hfi(mot_contrl_t *ctrl, bool enable) {
+	hfi_t *hfi = &ctrl->hfi;
+	if (enable) {
+		hfi->inv_ld_lq = 1.0f/mc_conf()->m.lq - 1.0f/mc_conf()->m.ld;
+		hfi->int_gain2 = 3.0f;
+		hfi->int_gain = 1200.0f;
+		hfi->max_vel = 2000;
+		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;
+	}
+	ctrl->b_hfi = enable;
+}
+
 bool mot_contrl_request_mode(mot_contrl_t *ctrl, u8 mode) {
 	if (mode > CTRL_MODE_EBRAKE) {
 		mot_contrl_set_error(ctrl, FOC_Param_Err);
@@ -214,6 +229,37 @@ static __INLINE void phase_curr_unbal_check(mot_contrl_t *ctrl) {
 	}
 }
 
+#define HFI_DEBUG_ON 1
+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
+	
+	if (hfi->b_sample) {
+		float sample_now = cos * ctrl->foc.in.curr_ab.b - 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);
+		hfi->vel_integrator += err * hfi->int_gain2;
+		hfi->vel_integrator = fclamp(hfi->vel_integrator, -hfi->max_vel, hfi->max_vel);
+		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;
+	}else {
+		float sample_now = cos * ctrl->foc.in.curr_ab.b - 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->b_sample = !hfi->b_sample;
+}
 
 bool mot_contrl_update(mot_contrl_t *ctrl) {
 	foc_t *foc = &ctrl->foc;
@@ -250,6 +296,16 @@ 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 {
+		foc->in.v_ab_inj.a = 0;
+		foc->in.v_ab_inj.b = 0;
+	}
+
 	foc_update(foc);
 
 	float lowpass = foc->mot_vel_radusPers * FOC_CTRL_US * 2;

Applications/foc/core/controller.h

@@ -115,6 +115,23 @@ typedef struct {
 	float torque;
 }prot_limit;
 
+typedef struct {
+	bool b_sample;
+	s16  v_inj;
+	float v_alpha_inj;
+	float v_beta_inj;
+	float sample_prev;
+	float sample_now;
+	float vel_integrator;
+	float angle_rad;
+	float angle_deg;
+	float elec_vel;
+	float max_vel;
+	float int_gain2;
+	float int_gain;
+	float inv_ld_lq;
+}hfi_t;
+
 typedef struct{
 	bool 			b_start;
 	bool			b_ebrk_running;
@@ -163,6 +180,8 @@ typedef struct{
 	float 			phase_b_max;
 	float 			phase_c_max;
 	u32 			phase_unbalance_cnt;
+	bool 			b_hfi;
+	hfi_t           hfi;
 }mot_contrl_t;
 
 void mot_contrl_init(mot_contrl_t *ctrl);
@@ -199,7 +218,7 @@ void mot_contrl_set_pid(mot_contrl_t *ctrl, u8 id, float kp, float ki, float kd)
 void mot_contrl_set_torque_limit_rttime(mot_contrl_t *ctrl, u32 time);
 void mot_contrl_set_vel_limit_rttime(mot_contrl_t *ctrl, u32 time);
 bool mot_contrl_set_force_torque(mot_contrl_t *ctrl, float torque);
-
+void mot_contrl_enable_hfi(mot_contrl_t *ctrl, bool enable);
 
 static __INLINE bool mot_contrl_start(mot_contrl_t *ctrl, u8 mode) {
 	mot_contrl_enable(ctrl, true);

Applications/foc/core/foc.c

@@ -63,7 +63,7 @@ void foc_update(foc_t *foc) {
 	
 	LowPass_Filter(foc->mot_velocity_filterd, foc->in.mot_velocity, 0.01f);
 	foc->mot_vel_radusPers = foc->mot_velocity_filterd / 30.0f * PI * mc_conf()->m.poles;
-	arm_sin_cos(foc->in.mot_angle, &foc->sin, &foc->cos);
+	
 	park(foc, &foc->in.curr_ab, &foc->out.curr_dq);
 	if (!foc->in.b_openloop) {
 		/* limiter Vd output for PI controller */
@@ -98,8 +98,16 @@ void foc_update(foc_t *foc) {
 	}
 #endif
 	rev_park(foc, &foc->out.vol_dq, &foc->out.vol_albeta);
-	
-	SVM_Duty_Fix(&foc->out.vol_albeta, foc->in.dc_vol, foc->half_period, &foc->out);
+	albt_t albt;
+	albt.a = foc->out.vol_albeta.a;
+	albt.b = foc->out.vol_albeta.b;
+	if (foc->in.v_ab_inj.a != 0 || foc->in.v_ab_inj.b != 0) {
+		albt.a += foc->in.v_ab_inj.a;
+		albt.b += foc->in.v_ab_inj.b;
+		saturate_vector_2d(&albt.a, &albt.b, max_Vdc * SQRT3_BY_2);
+	}
+
+	SVM_Duty_Fix(&albt, foc->in.dc_vol, foc->half_period, &foc->out);
 
 	phase_current_point(&foc->out);
 	

Applications/foc/core/foc.h

@@ -17,6 +17,7 @@ typedef struct {
 typedef struct {
 	float 	curr_abc[3];
 	albt_t  curr_ab;
+	albt_t  v_ab_inj;
 	float   vol_abc[3]; //相对地电压
 	float   mot_velocity;   //from hall or encoder
 	float 	mot_angle; //from hall or encoder

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/math/fast_math.h

@@ -19,6 +19,9 @@
 #ifndef SQ
 #define SQ(x) ((x)*(x))
 #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))
@@ -161,6 +164,21 @@ static __INLINE float fast_atan_2(float y, float x) {
     return r;
 }
 
+static __INLINE void saturate_vector_2d(float *x, float *y, float max) {
+	float mag = NORM2_f(*x, *y);
+	max = fabsf(max);
+
+	if (mag < 1e-4f) {
+		mag = 1e-4f;
+	}
+	if (mag > max) {
+		const float f = max / mag;
+		*x *= f;
+		*y *= f;
+	}
+}
+
+
 static void normal_sincosf(float angle, float *sin, float *cos) {
 	*sin = arm_sin_f32(angle);
 	*cos = arm_cos_f32(angle);
@@ -172,6 +190,8 @@ static void normal_sincosf(float angle, float *sin, float *cos) {
 
 #define SIGN(x)				(((x) < 0.0f) ? -1.0f : 1.0f)
 
+#define norm_angle_rad(a) {while (a >= M_PI*2) a-=M_PI*2;while (a < 0) a +=M_PI*2;};
+
 
 /**
  * A simple low pass filter.