diff --git a/foc_math.c b/foc_math.c index 0f76a87b3..fb2da6279 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; } } @@ -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 } diff --git a/foc_math.h b/foc_math.h index 174a835b6..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; @@ -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 57501c5b8..554f6b0ec 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; } /** @@ -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); + 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 ); }