Skip to content
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.

Sign up for GitHub

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

Jump to bottom

FOC minor optimizations #467

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
+36 −32
Open

FOC minor optimizations #467

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
aac5ff0
[FOC] Fix double `;`
kubark42 Apr 10, 2022
ba3130d
[FOC] Remove volatile
kubark42 Apr 10, 2022
9a6f85d
[FOC] Only calculate 1/v_bus once
kubark42 Apr 10, 2022
5b73c36
[FOC] Eliminate a division
kubark42 Apr 10, 2022
324db20
[FOC] Abstract out magic number `ind_scale_factor`
kubark42 Apr 11, 2022
b6aa199
[FOC] Eliminate a few divisions by introducing `p_inv_inv_ld_lq`
kubark42 Apr 11, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 5 additions & 5 deletions foc_math.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}

Expand Down Expand Up @@ -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_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_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
}
9 changes: 8 additions & 1 deletion foc_math.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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_ */
49 changes: 23 additions & 26 deletions mcpwm_foc.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

/**
Expand Down Expand Up @@ -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;
Copy link
Contributor Author

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.

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 ) {
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Copy link
Owner

Choose a reason for hiding this comment

The 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.

Copy link
Contributor Author

@kubark42 kubark42 Apr 11, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can we address that with comments? TBH, max_mod_norm didn't speak to me, and even after working with the math it's not clear to me what this section is doing. The math is clear, but the why of it all is obscure without a deeper understanding of the code. For instance, d_gain_scale is set and modified up to three times, and why that is good, desirable behavior is left as an exercise to the reader.

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.

Copy link
Owner

Choose a reason for hiding this comment

The 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;
Expand Down Expand Up @@ -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
);
}
Expand Down Expand Up @@ -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
);
}
Expand Down