From 39419a2ae685f2773f3d26f70281a51838f1e8a9 Mon Sep 17 00:00:00 2001 From: Gabriele Russo Date: Tue, 25 Nov 2025 22:51:41 +0100 Subject: [PATCH] fast filtering on phase current values Apply some very light filtering on phase current readings using a sliding window average (3 values) and 2 cascaded first order LPF. Should be very fast to compute and also introduce minimal phase delay. --- motor/foc_math.h | 3 ++ motor/mcpwm_foc.c | 103 ++++++++++++++++++++++++++++++++++------------ 2 files changed, 80 insertions(+), 26 deletions(-) diff --git a/motor/foc_math.h b/motor/foc_math.h index 3f1eb3e18e..01fc52505b 100644 --- a/motor/foc_math.h +++ b/motor/foc_math.h @@ -44,6 +44,8 @@ typedef struct { float phase_sin; float i_alpha; float i_beta; + float i_alpha_filter; + float i_beta_filter; float i_abs; float i_abs_filter; float i_bus; @@ -170,6 +172,7 @@ typedef struct { float m_speed_est_fast_corrected; // Same as m_speed_est_fast, but always based on the corrected position float m_speed_est_faster; mc_sample_t m_samples; + bool is_sampling; int m_tachometer; int m_tachometer_abs; float m_pos_pid_now; diff --git a/motor/mcpwm_foc.c b/motor/mcpwm_foc.c index 34face252d..036780ba66 100644 --- a/motor/mcpwm_foc.c +++ b/motor/mcpwm_foc.c @@ -371,6 +371,7 @@ void mcpwm_foc_init(mc_configuration *conf_m1, mc_configuration *conf_m2) { m_motor_1.m_hall_dt_diff_last = 1.0; m_motor_1.m_hall_dt_diff_now = 1.0; m_motor_1.m_ang_hall_int_prev = -1; + m_motor_1.is_sampling = false; foc_precalc_values((motor_all_state_t*)&m_motor_1); update_hfi_samples(m_motor_1.m_conf->foc_hfi_samples, &m_motor_1); init_audio_state(&m_motor_1.m_audio); @@ -383,6 +384,7 @@ void mcpwm_foc_init(mc_configuration *conf_m1, mc_configuration *conf_m2) { m_motor_2.m_hall_dt_diff_last = 1.0; m_motor_2.m_hall_dt_diff_now = 1.0; m_motor_2.m_ang_hall_int_prev = -1; + m_motor_2.is_sampling = false; foc_precalc_values((motor_all_state_t*)&m_motor_2); update_hfi_samples(m_motor_2.m_conf->foc_hfi_samples, &m_motor_2); init_audio_state(&m_motor_2.m_audio); @@ -1839,7 +1841,7 @@ int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, fl motor->m_samples.avg_current_tot = 0.0; motor->m_samples.avg_voltage_tot = 0.0; motor->m_samples.sample_num = 0; - + motor->is_sampling = true; int cnt = 0; while (motor->m_samples.sample_num < samples) { chThdSleepMilliseconds(1); @@ -1853,6 +1855,7 @@ int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, fl motor->m_id_set = 0.0; motor->m_iq_set = 0.0; motor->m_phase_override = false; + motor->is_sampling = false; motor->m_control_mode = CONTROL_MODE_NONE; motor->m_state = MC_STATE_OFF; stop_pwm_hw((motor_all_state_t*)motor); @@ -1863,7 +1866,7 @@ int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, fl return fault; } } - + motor->is_sampling = false; const float current_avg = motor->m_samples.avg_current_tot / (float)motor->m_samples.sample_num; const float voltage_avg = motor->m_samples.avg_voltage_tot / (float)motor->m_samples.sample_num; @@ -3227,16 +3230,45 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { FOC_PROFILE_LINE_FINE(); + static float ia = 0; + static float ib = 0; + static float ic = 0; + static float ia_v[3] = {0.0}; + static float ib_v[3] = {0.0}; + static float ic_v[3] = {0.0}; + static uint_fast8_t i_v_index = 0; + + ia_v[i_v_index] = curr0; + ib_v[i_v_index] = curr1; + ic_v[i_v_index] = curr2; + + if (motor_now->m_state == MC_STATE_RUNNING) { + float ia_avg = (ia_v[0] + ia_v[1] + ia_v[2])/3.0; + float ib_avg = (ib_v[0] + ib_v[1] + ib_v[2])/3.0; + float ic_avg = (ic_v[0] + ic_v[1] + ic_v[2])/3.0; + const float lp_k = 0.8; + //apply two first order low pass filters to reduce phase lag + float ia_2 = ia - (lp_k * (ia - ia_avg)); + float ib_2 = ib - (lp_k * (ib - ib_avg)); + float ic_2 = ic - (lp_k * (ic - ic_avg)); + ia -= (lp_k * (ia - ia_2)); + ib -= (lp_k * (ib - ib_2)); + ic -= (lp_k * (ic - ic_2)); + } else { + ia = curr0; + ib = curr1; + ic = curr2; + } + + // update vector index for moving average + i_v_index = (i_v_index + 1)%3; + // Store the currents for sampling - ADC_curr_norm_value[0 + norm_curr_ofs] = curr0; - ADC_curr_norm_value[1 + norm_curr_ofs] = curr1; - ADC_curr_norm_value[2 + norm_curr_ofs] = curr2; - - float ia = curr0; - float ib = curr1; - float ic = curr2; + ADC_curr_norm_value[0 + norm_curr_ofs] = ia; + ADC_curr_norm_value[1 + norm_curr_ofs] = ib; + ADC_curr_norm_value[2 + norm_curr_ofs] = ic; - UTILS_LP_FAST(state_now->v_bus, GET_INPUT_VOLTAGE(), 0.1); + UTILS_LP_FAST(state_now->v_bus, GET_INPUT_VOLTAGE(), 0.05); volatile float enc_ang = 0; volatile bool encoder_is_being_used = false; @@ -3282,12 +3314,18 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { if (motor_now->m_state == MC_STATE_RUNNING) { if (full_clarke) { // Full Clarke Transform - state_now->i_alpha = (2.0 / 3.0) * ia - (1.0 / 3.0) * ib - (1.0 / 3.0) * ic; - state_now->i_beta = ONE_BY_SQRT3 * ib - ONE_BY_SQRT3 * ic; + state_now->i_alpha = (2.0 / 3.0) * curr0 - (1.0 / 3.0) * curr1 - (1.0 / 3.0) * curr2; + state_now->i_beta = ONE_BY_SQRT3 * curr1 - ONE_BY_SQRT3 * curr2; + //filtered values + state_now->i_alpha_filter = (2.0 / 3.0) * ia - (1.0 / 3.0) * ib - (1.0 / 3.0) * ic; + state_now->i_beta_filter = ONE_BY_SQRT3 * ib - ONE_BY_SQRT3 * ic; } else { // Clarke transform assuming balanced currents - state_now->i_alpha = ia; - state_now->i_beta = ONE_BY_SQRT3 * ia + TWO_BY_SQRT3 * ib; + state_now->i_alpha = curr0; + state_now->i_beta = ONE_BY_SQRT3 * curr0 + TWO_BY_SQRT3 * curr1; + //filtered values + state_now->i_alpha_filter = ia; + state_now->i_beta_filter = ONE_BY_SQRT3 * ia + TWO_BY_SQRT3 * ib; } motor_now->m_i_alpha_sample_with_offset = state_now->i_alpha; @@ -3296,6 +3334,8 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { if (motor_now->m_i_alpha_beta_has_offset) { state_now->i_alpha = 0.5 * (state_now->i_alpha + motor_now->m_i_alpha_sample_next); state_now->i_beta = 0.5 * (state_now->i_beta + motor_now->m_i_beta_sample_next); + state_now->i_alpha_filter = 0.5 * (state_now->i_alpha_filter + motor_now->m_i_alpha_sample_next); + state_now->i_beta_filter = 0.5 * (state_now->i_beta_filter + motor_now->m_i_beta_sample_next); motor_now->m_i_alpha_beta_has_offset = false; } @@ -3443,7 +3483,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { { if (!motor_now->m_phase_override) { foc_observer_update(state_now->v_alpha, state_now->v_beta, - state_now->i_alpha, state_now->i_beta, + state_now->i_alpha_filter, state_now->i_beta_filter, dt, &(motor_now->m_observer_state), &motor_now->m_phase_now_observer, motor_now); // Compensate from the phase lag caused by the switching frequency. This is important for motors @@ -3672,6 +3712,8 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { // The current is 0 when the motor is undriven state_now->i_alpha = 0.0; state_now->i_beta = 0.0; + state_now->i_alpha_filter = 0.0; + state_now->i_beta_filter = 0.0; state_now->id = 0.0; state_now->iq = 0.0; state_now->id_filter = 0.0; @@ -3688,8 +3730,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { update_valpha_vbeta(motor_now, 0.0, 0.0, 1.5 / state_now->v_bus); // Run observer - foc_observer_update(state_now->v_alpha, state_now->v_beta, - state_now->i_alpha, state_now->i_beta, + foc_observer_update(state_now->v_alpha, state_now->v_beta, state_now->i_alpha_filter, state_now->i_beta_filter, dt, &(motor_now->m_observer_state), 0, motor_now); motor_now->m_phase_now_observer = utils_fast_atan2(motor_now->m_x2_prev + motor_now->m_observer_state.x2, motor_now->m_x1_prev + motor_now->m_observer_state.x1); @@ -4120,11 +4161,11 @@ static void timer_update(motor_all_state_t *motor, float dt) { } // Samples - if (motor->m_state == MC_STATE_RUNNING) { + if (motor->is_sampling) { const volatile float vd_tmp = motor->m_motor_state.vd; const volatile float vq_tmp = motor->m_motor_state.vq; - const volatile float id_tmp = motor->m_motor_state.id; - const volatile float iq_tmp = motor->m_motor_state.iq; + const volatile float id_tmp = motor->m_motor_state.id_filter; + const volatile float iq_tmp = motor->m_motor_state.iq_filter; motor->m_samples.avg_current_tot += NORM2_f(id_tmp, iq_tmp); motor->m_samples.avg_voltage_tot += NORM2_f(vd_tmp, vq_tmp); @@ -4606,10 +4647,14 @@ static void control_current(motor_all_state_t *motor, float dt) { utils_truncate_number(&max_duty, 0.0, conf_now->l_max_duty); // Park transform: transforms the currents from stator to the rotor reference frame - state_m->id = c * state_m->i_alpha + s * state_m->i_beta; - state_m->iq = c * state_m->i_beta - s * state_m->i_alpha; - - // Low passed currents are used for less time critical parts, not for the feedback + float m_id = c * state_m->i_alpha + s * state_m->i_beta; + float m_iq = c * state_m->i_beta - s * state_m->i_alpha; + //UTILS_LP_FAST(state_m->id, m_id, 0.5); + //UTILS_LP_FAST(state_m->iq, m_iq, 0.5); + state_m->id = c * state_m->i_alpha_filter + s * state_m->i_beta_filter; + state_m->iq = c * state_m->i_beta_filter - s * state_m->i_alpha_filter; + + // Low passed currents are used for less time critical parts, not for the feedback UTILS_LP_FAST(state_m->id_filter, state_m->id, conf_now->foc_current_filter_const); UTILS_LP_FAST(state_m->iq_filter, state_m->iq, conf_now->foc_current_filter_const); @@ -4695,11 +4740,17 @@ static void control_current(motor_all_state_t *motor, float dt) { #ifdef HW_HAS_INPUT_CURRENT_SENSOR state_m->i_bus = GET_INPUT_CURRENT(); #else - state_m->i_bus = state_m->mod_alpha_measured * state_m->i_alpha + state_m->mod_beta_measured * state_m->i_beta; + //state_m->i_bus = state_m->mod_alpha_measured * state_m->i_alpha + state_m->mod_beta_measured * state_m->i_beta; + float batt_current_m = state_m->mod_alpha_measured * state_m->i_alpha_filter + state_m->mod_beta_measured * state_m->i_beta_filter; + UTILS_LP_FAST(state_m->i_bus, batt_current_m, 0.01); + // TODO: Also calculate motor power based on v_alpha, v_beta, i_alpha and i_beta. This is much more accurate // with phase filters than using the modulation and bus current. #endif - state_m->i_abs = NORM2_f(state_m->id, state_m->iq); + // Use non filtered currents for the absolute maximum current value + //state_m->i_abs = NORM2_f(state_m->id, state_m->iq); + state_m->i_abs = NORM2_f(m_id, m_iq); + // Use the filtered currents for the filtered ABS state_m->i_abs_filter = NORM2_f(state_m->id_filter, state_m->iq_filter); // Inverse Park transform: transforms the (normalized) voltages from the rotor reference frame to the stator frame