|
|
@@ -55,6 +55,7 @@ bool mot_contrl_enable(mot_contrl_t *ctrl, bool start) {
|
|
|
mot_contrl_ulimit(ctrl);
|
|
|
mot_contrl_rtlimit(ctrl);
|
|
|
}
|
|
|
+ mot_contrl_enable_hfi(ctrl, start);
|
|
|
ctrl->b_ebrk_running = false;
|
|
|
ctrl->b_AutoHold = false;
|
|
|
ctrl->b_cruiseEna = false;
|
|
|
@@ -117,9 +118,150 @@ float mot_contrl_IF_update(mot_contrl_t *ctrl) {
|
|
|
i_f->curr_angle += i_f->curr_vel * ctrl->foc.ts; //速度积分获取电角度
|
|
|
norm_angle_rad(i_f->curr_angle);
|
|
|
ctrl->if_ctl.angle_deg = pi_2_degree(ctrl->if_ctl.curr_angle);
|
|
|
+ i_f->curr_vel_rpm = i_f->curr_vel/M_PI*30.0f/mc_conf()->m.poles;
|
|
|
return ctrl->if_ctl.angle_deg;
|
|
|
}
|
|
|
|
|
|
+#define HFI_DEBUG_ON 0
|
|
|
+
|
|
|
+#if 0
|
|
|
+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 = 20.0f;
|
|
|
+ }
|
|
|
+ ctrl->b_hfi = enable;
|
|
|
+}
|
|
|
+
|
|
|
+static void mot_contrl_hfi_alpha_beta(mot_contrl_t *ctrl) {
|
|
|
+ hfi_t *hfi = &ctrl->hfi;
|
|
|
+ foc_t *foc = &ctrl->foc;
|
|
|
+ if (hfi->b_sample) {
|
|
|
+ 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);
|
|
|
+ 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);
|
|
|
+ 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 = 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 * foc->cos;
|
|
|
+ hfi->v_beta_inj = hfi->v_inj * foc->sin;
|
|
|
+ }
|
|
|
+ hfi->b_sample = !hfi->b_sample;
|
|
|
+}
|
|
|
+
|
|
|
+#else
|
|
|
+#define HFI_PLL_BAND 150.0F
|
|
|
+#define HFI_IPD_V 6.0F
|
|
|
+#define HFI_RUNNING_V 5.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 = 0;
|
|
|
+ hfi->rad_integrator = degree_2_pi(hfi->angle_deg);
|
|
|
+ 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);
|
|
|
+ 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;
|
|
|
+ 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;
|
|
|
+ hfi->b_ena = enable;
|
|
|
+}
|
|
|
+
|
|
|
+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;
|
|
|
+ float vel_elec = PI_Controller_Run(&hfi->pi, err);
|
|
|
+ LowPass_Filter(hfi->elec_vel, vel_elec, 1.0f);
|
|
|
+ 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;
|
|
|
+ foc_t *foc = &ctrl->foc;
|
|
|
+ float sign = hfi->sign;
|
|
|
+
|
|
|
+ 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->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.1f);
|
|
|
+
|
|
|
+ foc->in.mot_angle = hfi->angle_deg;
|
|
|
+ foc->in.mot_velocity = hfi->rpm_vel;
|
|
|
+
|
|
|
+ if (!hfi->ipd) {
|
|
|
+ float v_inj = hfi->v_inj;
|
|
|
+ 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, 0.5f);
|
|
|
+ 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_d_inj = hfi->v_inj * hfi->sign;// + 1.0f;
|
|
|
+ hfi->v_q_inj = 0;
|
|
|
+ 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;
|
|
|
+}
|
|
|
+#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);
|
|
|
@@ -293,6 +435,11 @@ bool mot_contrl_update(mot_contrl_t *ctrl) {
|
|
|
|
|
|
float enc_angle = motor_encoder_get_angle();
|
|
|
float enc_vel = motor_encoder_get_speed();
|
|
|
+ if (ctrl->if_ctl.b_ena) {
|
|
|
+ enc_angle = mot_contrl_IF_update(ctrl);
|
|
|
+ enc_vel = ctrl->if_ctl.curr_vel_rpm;
|
|
|
+ mot_contrl_set_current(ctrl, ctrl->if_ctl.iq_set);
|
|
|
+ }
|
|
|
if (!foc_observer_diagnostic(enc_angle, enc_vel)){
|
|
|
/* detect encoder angle error, do something here */
|
|
|
if (!foc_observer_sensorless_stable()) {
|
|
|
@@ -303,10 +450,7 @@ bool mot_contrl_update(mot_contrl_t *ctrl) {
|
|
|
enc_vel = foc_observer_sensorless_speed();
|
|
|
}
|
|
|
|
|
|
- if (ctrl->if_ctl.b_ena) {
|
|
|
- foc->in.mot_angle = mot_contrl_IF_update(ctrl);
|
|
|
- mot_contrl_set_current(ctrl, ctrl->if_ctl.iq_set);
|
|
|
- }else if (!ctrl->b_mtpa_calibrate && (ctrl->force_angle != INVALID_ANGLE)) {
|
|
|
+ if (!ctrl->b_mtpa_calibrate && (ctrl->force_angle != INVALID_ANGLE)) {
|
|
|
foc->in.mot_angle = ctrl->force_angle;
|
|
|
}else {
|
|
|
foc->in.mot_angle = enc_angle;
|
|
|
@@ -322,6 +466,14 @@ 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);
|
|
|
}
|
|
|
+ if (ctrl->hfi.b_ena) {
|
|
|
+ mot_contrl_hfi_alpha_beta(ctrl);
|
|
|
+ foc->in.v_dq_inj.d = ctrl->hfi.v_d_inj;
|
|
|
+ foc->in.v_dq_inj.q = ctrl->hfi.v_q_inj;
|
|
|
+ }else {
|
|
|
+ foc->in.v_dq_inj.d = 0;
|
|
|
+ foc->in.v_dq_inj.q = 0;
|
|
|
+ }
|
|
|
|
|
|
foc_update(foc);
|
|
|
|
|
|
@@ -577,6 +729,11 @@ float mot_contrl_get_speed(mot_contrl_t *ctrl) {
|
|
|
float speed = ctrl->foc.in.mot_velocity;
|
|
|
if (!ctrl->b_start || foc_observer_is_encoder()) {
|
|
|
speed = motor_encoder_get_speed();
|
|
|
+ if (ctrl->if_ctl.b_ena) {
|
|
|
+ speed = ctrl->if_ctl.curr_vel_rpm;
|
|
|
+ }else if (ctrl->hfi.b_ena) {
|
|
|
+ speed = ctrl->hfi.rpm_vel;
|
|
|
+ }
|
|
|
}else {
|
|
|
if (foc_observer_sensorless_stable()) {
|
|
|
speed = foc_observer_sensorless_speed();
|
|
|
@@ -894,8 +1051,8 @@ void mot_contrl_calc_current(mot_contrl_t *ctrl) {
|
|
|
iDC x vDC = 3/2(iq x vq + id x vd);
|
|
|
*/
|
|
|
float m_pow = (vd * id + vq * iq);
|
|
|
- float pf_angle = fast_atan2(id, iq) - fast_atan2(vd, vq);
|
|
|
- m_pow = m_pow * arm_cos_f32(pf_angle);
|
|
|
+ //float pf_angle = fast_atan2(id, iq) - fast_atan2(vd, vq);
|
|
|
+ //m_pow = m_pow * arm_cos_f32(pf_angle);
|
|
|
float raw_idc = 0.0f;
|
|
|
float v_dc = get_vbus_float();
|
|
|
if (v_dc != 0.0f) {
|
kevin