-
Notifications
You must be signed in to change notification settings - Fork 1.4k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
FOC minor optimizations #467
base: master
Are you sure you want to change the base?
Changes from all commits
aac5ff0
ba3130d
9a6f85d
5b73c36
324db20
b6aa199
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -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; | ||
} | ||
|
||
/** | ||
|
@@ -2541,8 +2535,10 @@ 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 inv_v_bus = 1.0 / motor_now->m_motor_state.v_bus; | ||
|
||
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 ) { | ||
|
@@ -2672,15 +2668,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 +2976,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; | ||
|
@@ -3705,14 +3698,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); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. The name max_mod_norm makes a lot of sense here. After the changes it is much more difficult to see what this section does. There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Can we address that with comments? TBH, It's also not clear if this section runs once every never, or it runs almost every loop. Answering those questions might help make it clear if it's worth saving a division or not. There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. d_gain_scale is a scaling factor of the d-axis controller gain (as the name suggests). max_mod_norm is a normalized maximum modulation (duty cycle), meaning a value between 0 and 1 that maps where abs_duty is between 0 and max_duty. The section scales down the D axis gain when max_mod_norm is above foc_d_gain_scale_start down to foc_d_gain_scale_max_mod when max_mod_norm is 1. The reason for doing that is that one way of making the controller more stable when you hit max modulation is to scale down the d axis gain compared to the q axis gain. This section runs every iteration as the motor approaches full speed as that is when the modulation approaches maximum. Before it was quite easy for me to see what it does, but the changes make it quite a bit more obscure. |
||
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; | ||
|
@@ -3854,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 | ||
); | ||
} | ||
|
@@ -3906,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 | ||
); | ||
} | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@vedderb I can't see why this
volatile
would have been necessary. The variables are used immediately after in the function, so there's not opportunity for things to be updated elsewhere. The functions themselves shouldn't be optimized away, not unless the underlying variables they're using should somehow themselves be volatile.