From 582bdc708064bb1f67732b04817e36e3d9a8cb2f Mon Sep 17 00:00:00 2001 From: Benjamin Vedder Date: Tue, 31 May 2022 15:28:06 +0200 Subject: [PATCH] Speed PID update to work better with propellers --- conf_general.h | 2 +- motor/foc_math.c | 10 ++++------ 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/conf_general.h b/conf_general.h index ccc3a209..2c865bc1 100755 --- a/conf_general.h +++ b/conf_general.h @@ -24,7 +24,7 @@ #define FW_VERSION_MAJOR 6 #define FW_VERSION_MINOR 00 // Set to 0 for building a release and iterate during beta test builds -#define FW_TEST_VERSION_NUMBER 50 +#define FW_TEST_VERSION_NUMBER 51 #include "datatypes.h" diff --git a/motor/foc_math.c b/motor/foc_math.c index d0d733c1..eb3789da 100644 --- a/motor/foc_math.c +++ b/motor/foc_math.c @@ -483,15 +483,13 @@ void foc_run_pid_control_speed(float dt, motor_all_state_t *motor) { motor->m_speed_prev_error = error; // Calculate output - utils_truncate_number_abs(&p_term, 1.0); - utils_truncate_number_abs(&d_term, 1.0); float output = p_term + motor->m_speed_i_term + d_term; - float pre_output = output; utils_truncate_number_abs(&output, 1.0); - float output_saturation = output - pre_output; + // Integrator windup protection + motor->m_speed_i_term += error * conf_now->s_pid_ki * dt * (1.0 / 20.0); + utils_truncate_number_abs(&motor->m_speed_i_term, 1.0); - motor->m_speed_i_term += error * (conf_now->s_pid_ki * dt) * (1.0 / 20.0) + output_saturation; if (conf_now->s_pid_ki < 1e-9) { motor->m_speed_i_term = 0.0; } @@ -507,7 +505,7 @@ void foc_run_pid_control_speed(float dt, motor_all_state_t *motor) { } } - motor->m_iq_set = output * conf_now->l_current_max * conf_now->l_current_max_scale; + motor->m_iq_set = output * conf_now->lo_current_max * conf_now->l_current_max_scale; } float foc_correct_encoder(float obs_angle, float enc_angle, float speed,