From aac5ff0383ff6c54ff277d82ba66e2a1aae4a330 Mon Sep 17 00:00:00 2001 From: Kenn Sebesta Date: Sun, 10 Apr 2022 00:49:20 -0400 Subject: [PATCH 1/6] [FOC] Fix double `;` --- foc_math.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/foc_math.c b/foc_math.c index 0f76a87b3..8f2a4134e 100644 --- a/foc_math.c +++ b/foc_math.c @@ -348,13 +348,13 @@ void foc_run_pid_control_pos(bool index_found, float dt, motor_all_state_t *moto if (conf_now->m_sensor_port_mode != SENSOR_PORT_MODE_HALL) { if (index_found) { - motor->m_iq_set = output * conf_now->l_current_max * conf_now->l_current_max_scale;; + motor->m_iq_set = output * conf_now->l_current_max * conf_now->l_current_max_scale; } else { // Rotate the motor with 40 % power until the encoder index is found. - motor->m_iq_set = 0.4 * conf_now->l_current_max * conf_now->l_current_max_scale;; + motor->m_iq_set = 0.4 * conf_now->l_current_max * conf_now->l_current_max_scale; } } else { - motor->m_iq_set = output * conf_now->l_current_max * conf_now->l_current_max_scale;; + motor->m_iq_set = output * conf_now->l_current_max * conf_now->l_current_max_scale; } } From ba3130d194e23226b292e918d55832f3b0464426 Mon Sep 17 00:00:00 2001 From: Kenn Sebesta Date: Sun, 10 Apr 2022 00:52:21 -0400 Subject: [PATCH 2/6] [FOC] Remove volatile --- mcpwm_foc.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mcpwm_foc.c b/mcpwm_foc.c index 57501c5b8..4f20f4cf5 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -2541,8 +2541,8 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { UTILS_LP_FAST(motor_now->m_motor_state.v_bus, GET_INPUT_VOLTAGE(), 0.1); - volatile float enc_ang = 0; - volatile bool encoder_is_being_used = false; + float enc_ang = 0; + bool encoder_is_being_used = false; if (virtual_motor_is_connected()) { if (conf_now->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER ) { From 9a6f85d178f839513e22e79bf72b775b7176421e Mon Sep 17 00:00:00 2001 From: Kenn Sebesta Date: Sun, 10 Apr 2022 00:54:34 -0400 Subject: [PATCH 3/6] [FOC] Only calculate 1/v_bus once --- mcpwm_foc.c | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/mcpwm_foc.c b/mcpwm_foc.c index 4f20f4cf5..00cffd603 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -2541,6 +2541,8 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { UTILS_LP_FAST(motor_now->m_motor_state.v_bus, GET_INPUT_VOLTAGE(), 0.1); + float inv_v_bus = 1.0 / motor_now->m_motor_state.v_bus; + float enc_ang = 0; bool encoder_is_being_used = false; @@ -2672,15 +2674,12 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { } } - // Compensation for supply voltage variations - float scale = 1.0 / motor_now->m_motor_state.v_bus; - // Compute error float error = duty_set - motor_now->m_motor_state.duty_now; - // Compute parameters - float p_term = error * conf_now->foc_duty_dowmramp_kp * scale; - motor_now->m_duty_i_term += error * (conf_now->foc_duty_dowmramp_ki * dt) * scale; + // Compute parameters. Make sure to compensate for supply voltage variations + float p_term = error * conf_now->foc_duty_dowmramp_kp * inv_v_bus; + motor_now->m_duty_i_term += error * (conf_now->foc_duty_dowmramp_ki * dt) * inv_v_bus; // I-term wind-up protection utils_truncate_number((float*)&motor_now->m_duty_i_term, -1.0, 1.0); @@ -2983,7 +2982,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { // Update corresponding modulation /* voltage_normalize = 1/(2/3*V_bus) */ - const float voltage_normalize = 1.5 / motor_now->m_motor_state.v_bus; + const float voltage_normalize = 1.5 * inv_v_bus; motor_now->m_motor_state.mod_d = motor_now->m_motor_state.vd * voltage_normalize; motor_now->m_motor_state.mod_q = motor_now->m_motor_state.vq * voltage_normalize; From 5b73c3601454e5967e1c8383e6d2e34d60eeb7a8 Mon Sep 17 00:00:00 2001 From: Kenn Sebesta Date: Sun, 10 Apr 2022 01:01:10 -0400 Subject: [PATCH 4/6] [FOC] Eliminate a division Replace it with a multiplication --- mcpwm_foc.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/mcpwm_foc.c b/mcpwm_foc.c index 00cffd603..6319ee36e 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -3704,14 +3704,18 @@ static void control_current(motor_all_state_t *motor, float dt) { 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); + // TODO: Document this section float d_gain_scale = 1.0; if (conf_now->foc_d_gain_scale_start < 0.99) { - float max_mod_norm = fabsf(state_m->duty_now / max_duty); + float abs_duty_now; if (max_duty < 0.01) { - max_mod_norm = 1.0; + abs_duty_now = fabsf(max_duty); + } else { + abs_duty_now = fabsf(state_m->duty_now); } - if (max_mod_norm > conf_now->foc_d_gain_scale_start) { - d_gain_scale = utils_map(max_mod_norm, conf_now->foc_d_gain_scale_start, 1.0, + float tmp = conf_now->foc_d_gain_scale_start * max_duty; + if (abs_duty_now > tmp) { + d_gain_scale = utils_map(abs_duty_now, tmp, max_duty, 1.0, conf_now->foc_d_gain_scale_max_mod); if (d_gain_scale < conf_now->foc_d_gain_scale_max_mod) { d_gain_scale = conf_now->foc_d_gain_scale_max_mod; From 324db207a85459568eac1a097269ed502b21aad5 Mon Sep 17 00:00:00 2001 From: Kenn Sebesta Date: Sun, 10 Apr 2022 22:04:16 -0400 Subject: [PATCH 5/6] [FOC] Abstract out magic number `ind_scale_factor` --- foc_math.c | 2 +- foc_math.h | 7 +++++++ mcpwm_foc.c | 16 +++++----------- 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/foc_math.c b/foc_math.c index 8f2a4134e..6782fb7a1 100644 --- a/foc_math.c +++ b/foc_math.c @@ -614,5 +614,5 @@ void foc_precalc_values(motor_all_state_t *motor) { motor->p_lq = conf_now->foc_motor_l + conf_now->foc_motor_ld_lq_diff * 0.5; motor->p_ld = conf_now->foc_motor_l - conf_now->foc_motor_ld_lq_diff * 0.5; motor->p_inv_ld_lq = (1.0 / motor->p_lq - 1.0 / motor->p_ld); - motor->p_v2_v3_inv_avg_half = (0.5 / motor->p_lq + 0.5 / motor->p_ld) * 0.9; // With the 0.9 we undo the adjustment from the detection + motor->p_v2_v3_inv_avg_half = (0.5 / motor->p_lq + 0.5 / motor->p_ld) * IND_SCALE_FACTOR; // With the `* IND_SCALE_FACTOR` we undo the adjustment from the detection } diff --git a/foc_math.h b/foc_math.h index 174a835b6..84ef0f1b2 100644 --- a/foc_math.h +++ b/foc_math.h @@ -210,4 +210,11 @@ void foc_run_fw(motor_all_state_t *motor, float dt); void foc_hfi_adjust_angle(float ang_err, motor_all_state_t *motor, float dt); void foc_precalc_values(motor_all_state_t *motor); +// The observer is more stable when the inductance is underestimated compared to overestimated, +// so scale it by `IND_SCALE_FACTOR`. This helps motors that start to saturate at higher currents and when +// the hardware has problems measuring the inductance correctly. Another reason for decreasing the +// measured value is that delays in the hardware and/or a high resistance compared to inductance +// will cause the value to be overestimated. +#define IND_SCALE_FACTOR 0.9 + #endif /* FOC_MATH_H_ */ diff --git a/mcpwm_foc.c b/mcpwm_foc.c index 6319ee36e..2c6355420 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -1833,23 +1833,17 @@ float mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float * mc_interface_unlock(); - // The observer is more stable when the inductance is underestimated compared to overestimated, - // so scale it by 0.8. This helps motors that start to saturate at higher currents and when - // the hardware has problems measuring the inductance correctly. Another reason for decreasing the - // measured value is that delays in the hardware and/or a high resistance compared to inductance - // will cause the value to be overestimated. - // NOTE: This used to be 0.8, but was changed to 0.9 as that works better with HFIv2 on most motors. - float ind_scale_factor = 0.9; - if (curr) { *curr = i_sum / iterations; } - if (ld_lq_diff) { - *ld_lq_diff = (ld_lq_diff_sum / iterations) * 1e6 * ind_scale_factor; + // The observer is more stable when the inductance is underestimated compared to + // overestimated, so use `IND_SCALE_FACTOR` to tune that + if (ld_lq_diff) { + *ld_lq_diff = (ld_lq_diff_sum / iterations) * 1e6 * IND_SCALE_FACTOR; } - return (l_sum / iterations) * 1e6 * ind_scale_factor; + return (l_sum / iterations) * 1e6 * IND_SCALE_FACTOR; } /** From b6aa199159f20f971840f717a07c2ffa226828d7 Mon Sep 17 00:00:00 2001 From: Kenn Sebesta Date: Sun, 10 Apr 2022 22:05:23 -0400 Subject: [PATCH 6/6] [FOC] Eliminate a few divisions by introducing `p_inv_inv_ld_lq` --- foc_math.c | 2 +- foc_math.h | 2 +- mcpwm_foc.c | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/foc_math.c b/foc_math.c index 6782fb7a1..fb2da6279 100644 --- a/foc_math.c +++ b/foc_math.c @@ -613,6 +613,6 @@ void foc_precalc_values(motor_all_state_t *motor) { const mc_configuration *conf_now = motor->m_conf; motor->p_lq = conf_now->foc_motor_l + conf_now->foc_motor_ld_lq_diff * 0.5; motor->p_ld = conf_now->foc_motor_l - conf_now->foc_motor_ld_lq_diff * 0.5; - motor->p_inv_ld_lq = (1.0 / motor->p_lq - 1.0 / motor->p_ld); + motor->p_inv_inv_ld_lq = 1.0 / (1.0 / motor->p_lq - 1.0 / motor->p_ld); motor->p_v2_v3_inv_avg_half = (0.5 / motor->p_lq + 0.5 / motor->p_ld) * IND_SCALE_FACTOR; // With the `* IND_SCALE_FACTOR` we undo the adjustment from the detection } diff --git a/foc_math.h b/foc_math.h index 84ef0f1b2..96901cf88 100644 --- a/foc_math.h +++ b/foc_math.h @@ -191,7 +191,7 @@ typedef struct { // Pre-calculated values float p_lq; float p_ld; - float p_inv_ld_lq; // (1.0/lq - 1.0/ld) + float p_inv_inv_ld_lq; // 1.0 / (1.0/lq - 1.0/ld) float p_v2_v3_inv_avg_half; // (0.5/ld + 0.5/lq) } motor_all_state_t; diff --git a/mcpwm_foc.c b/mcpwm_foc.c index 2c6355420..554f6b0ec 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -3851,7 +3851,7 @@ static void control_current(motor_all_state_t *motor, float dt) { } #endif foc_hfi_adjust_angle( - (di * conf_now->foc_f_zv) / (hfi_voltage * motor->p_inv_ld_lq), + ((di * conf_now->foc_f_zv) / hfi_voltage) * motor->p_inv_inv_ld_lq, motor, hfi_dt ); } @@ -3903,7 +3903,7 @@ static void control_current(motor_all_state_t *motor, float dt) { #endif foc_hfi_adjust_angle( motor->m_hfi.sign_last_sample * ((conf_now->foc_f_zv * di) / - hfi_voltage - motor->p_v2_v3_inv_avg_half) / motor->p_inv_ld_lq, + hfi_voltage - motor->p_v2_v3_inv_avg_half) * motor->p_inv_inv_ld_lq, motor, hfi_dt ); }