mirror of https://github.com/rusefi/bldc.git
Merge pull request #526 from Mitchlol/balance_iterm_limit
Balance App: Add I-Term Limiter, Remove D-Term Biquad Filters
This commit is contained in:
commit
e9386cb339
|
@ -120,7 +120,6 @@ static systime_t current_time, last_time, diff_time, loop_overshoot;
|
|||
static float filtered_loop_overshoot, loop_overshoot_alpha, filtered_diff_time;
|
||||
static systime_t fault_angle_pitch_timer, fault_angle_roll_timer, fault_switch_timer, fault_switch_half_timer, fault_duty_timer;
|
||||
static float d_pt1_lowpass_state, d_pt1_lowpass_k, d_pt1_highpass_state, d_pt1_highpass_k;
|
||||
static Biquad d_biquad_lowpass, d_biquad_highpass;
|
||||
static float motor_timeout;
|
||||
static systime_t brake_timeout;
|
||||
|
||||
|
@ -200,14 +199,6 @@ void app_balance_configure(balance_config *conf, imu_config *conf2) {
|
|||
float RC = 1.0 / ( 2.0 * M_PI * balance_conf.kd_pt1_highpass_frequency);
|
||||
d_pt1_highpass_k = dT / (RC + dT);
|
||||
}
|
||||
if(balance_conf.kd_biquad_lowpass > 0){
|
||||
float Fc = balance_conf.kd_biquad_lowpass / balance_conf.hertz;
|
||||
biquad_config(&d_biquad_lowpass, BQ_LOWPASS, Fc);
|
||||
}
|
||||
if(balance_conf.kd_biquad_highpass > 0){
|
||||
float Fc = balance_conf.kd_biquad_highpass / balance_conf.hertz;
|
||||
biquad_config(&d_biquad_highpass, BQ_HIGHPASS, Fc);
|
||||
}
|
||||
if(balance_conf.torquetilt_filter > 0){ // Torquetilt Current Biquad
|
||||
float Fc = balance_conf.torquetilt_filter / balance_conf.hertz;
|
||||
biquad_config(&torquetilt_current_biquad, BQ_LOWPASS, Fc);
|
||||
|
@ -303,8 +294,6 @@ static void reset_vars(void){
|
|||
yaw_last_proportional = 0;
|
||||
d_pt1_lowpass_state = 0;
|
||||
d_pt1_highpass_state = 0;
|
||||
biquad_reset(&d_biquad_lowpass);
|
||||
biquad_reset(&d_biquad_highpass);
|
||||
// Set values for startup
|
||||
setpoint = pitch_angle;
|
||||
setpoint_target_interpolated = pitch_angle;
|
||||
|
@ -733,6 +722,11 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
integral = integral + proportional;
|
||||
derivative = last_pitch_angle - pitch_angle;
|
||||
|
||||
// Apply I term Filter
|
||||
if(balance_conf.ki_limit > 0 && fabsf(integral * balance_conf.ki) > balance_conf.ki_limit){
|
||||
integral = balance_conf.ki_limit / balance_conf.ki * SIGN(integral);
|
||||
}
|
||||
|
||||
// Apply D term filters
|
||||
if(balance_conf.kd_pt1_lowpass_frequency > 0){
|
||||
d_pt1_lowpass_state = d_pt1_lowpass_state + d_pt1_lowpass_k * (derivative - d_pt1_lowpass_state);
|
||||
|
@ -742,12 +736,6 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
d_pt1_highpass_state = d_pt1_highpass_state + d_pt1_highpass_k * (derivative - d_pt1_highpass_state);
|
||||
derivative = derivative - d_pt1_highpass_state;
|
||||
}
|
||||
if(balance_conf.kd_biquad_lowpass > 0){
|
||||
derivative = biquad_process(&d_biquad_lowpass, derivative);
|
||||
}
|
||||
if(balance_conf.kd_biquad_highpass > 0){
|
||||
derivative = biquad_process(&d_biquad_highpass, derivative);
|
||||
}
|
||||
|
||||
pid_value = (balance_conf.kp * proportional) + (balance_conf.ki * integral) + (balance_conf.kd * derivative);
|
||||
|
||||
|
@ -755,6 +743,12 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
proportional2 = pid_value - gyro[1];
|
||||
integral2 = integral2 + proportional2;
|
||||
derivative2 = last_gyro_y - gyro[1];
|
||||
|
||||
// Apply I term Filter
|
||||
if(balance_conf.ki_limit > 0 && fabsf(integral2 * balance_conf.ki2) > balance_conf.ki_limit){
|
||||
integral2 = balance_conf.ki_limit / balance_conf.ki2 * SIGN(integral2);
|
||||
}
|
||||
|
||||
pid_value = (balance_conf.kp2 * proportional2) + (balance_conf.ki2 * integral2) + (balance_conf.kd2 * derivative2);
|
||||
}
|
||||
|
||||
|
@ -934,6 +928,14 @@ static float app_balance_get_debug(int index){
|
|||
return filtered_loop_overshoot;
|
||||
case(13):
|
||||
return filtered_diff_time;
|
||||
case(14):
|
||||
return integral;
|
||||
case(15):
|
||||
return integral * balance_conf.ki;
|
||||
case(16):
|
||||
return integral2;
|
||||
case(17):
|
||||
return integral2 * balance_conf.ki2;
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
|
|
|
@ -445,18 +445,15 @@
|
|||
#ifndef APPCONF_BALANCE_YAW_CURRENT_CLAMP
|
||||
#define APPCONF_BALANCE_YAW_CURRENT_CLAMP 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_KI_LIMIT
|
||||
#define APPCONF_BALANCE_KI_LIMIT 0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_KD_PT1_LOWPASS_FREQUENCY
|
||||
#define APPCONF_BALANCE_KD_PT1_LOWPASS_FREQUENCY 0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_KD_PT1_HIGHPASS_FREQUENCY
|
||||
#define APPCONF_BALANCE_KD_PT1_HIGHPASS_FREQUENCY 0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_KD_BIQUAD_LOWPASS
|
||||
#define APPCONF_BALANCE_KD_BIQUAD_LOWPASS 0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_KD_BIQUAD_HIGHPASS
|
||||
#define APPCONF_BALANCE_KD_BIQUAD_HIGHPASS 0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_BOOSTER_ANGLE
|
||||
#define APPCONF_BALANCE_BOOSTER_ANGLE 8
|
||||
#endif
|
||||
|
|
|
@ -341,10 +341,9 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
|
|||
buffer_append_float32_auto(buffer, conf->app_balance_conf.brake_current, &ind);
|
||||
buffer_append_uint16(buffer, conf->app_balance_conf.brake_timeout, &ind);
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.yaw_current_clamp, &ind);
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.ki_limit, &ind);
|
||||
buffer_append_uint16(buffer, conf->app_balance_conf.kd_pt1_lowpass_frequency, &ind);
|
||||
buffer_append_uint16(buffer, conf->app_balance_conf.kd_pt1_highpass_frequency, &ind);
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.kd_biquad_lowpass, &ind);
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.kd_biquad_highpass, &ind);
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.booster_angle, &ind);
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.booster_ramp, &ind);
|
||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.booster_current, &ind);
|
||||
|
@ -736,10 +735,9 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
|
|||
conf->app_balance_conf.brake_current = buffer_get_float32_auto(buffer, &ind);
|
||||
conf->app_balance_conf.brake_timeout = buffer_get_uint16(buffer, &ind);
|
||||
conf->app_balance_conf.yaw_current_clamp = buffer_get_float32_auto(buffer, &ind);
|
||||
conf->app_balance_conf.ki_limit = buffer_get_float32_auto(buffer, &ind);
|
||||
conf->app_balance_conf.kd_pt1_lowpass_frequency = buffer_get_uint16(buffer, &ind);
|
||||
conf->app_balance_conf.kd_pt1_highpass_frequency = buffer_get_uint16(buffer, &ind);
|
||||
conf->app_balance_conf.kd_biquad_lowpass = buffer_get_float32_auto(buffer, &ind);
|
||||
conf->app_balance_conf.kd_biquad_highpass = buffer_get_float32_auto(buffer, &ind);
|
||||
conf->app_balance_conf.booster_angle = buffer_get_float32_auto(buffer, &ind);
|
||||
conf->app_balance_conf.booster_ramp = buffer_get_float32_auto(buffer, &ind);
|
||||
conf->app_balance_conf.booster_current = buffer_get_float32_auto(buffer, &ind);
|
||||
|
@ -1115,10 +1113,9 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
|
|||
conf->app_balance_conf.brake_current = APPCONF_BALANCE_BRAKE_CURRENT;
|
||||
conf->app_balance_conf.brake_timeout = APPCONF_BALANCE_BRAKE_TIMEOUT;
|
||||
conf->app_balance_conf.yaw_current_clamp = APPCONF_BALANCE_YAW_CURRENT_CLAMP;
|
||||
conf->app_balance_conf.ki_limit = APPCONF_BALANCE_KI_LIMIT;
|
||||
conf->app_balance_conf.kd_pt1_lowpass_frequency = APPCONF_BALANCE_KD_PT1_LOWPASS_FREQUENCY;
|
||||
conf->app_balance_conf.kd_pt1_highpass_frequency = APPCONF_BALANCE_KD_PT1_HIGHPASS_FREQUENCY;
|
||||
conf->app_balance_conf.kd_biquad_lowpass = APPCONF_BALANCE_KD_BIQUAD_LOWPASS;
|
||||
conf->app_balance_conf.kd_biquad_highpass = APPCONF_BALANCE_KD_BIQUAD_HIGHPASS;
|
||||
conf->app_balance_conf.booster_angle = APPCONF_BALANCE_BOOSTER_ANGLE;
|
||||
conf->app_balance_conf.booster_ramp = APPCONF_BALANCE_BOOSTER_RAMP;
|
||||
conf->app_balance_conf.booster_current = APPCONF_BALANCE_BOOSTER_CURRENT;
|
||||
|
|
|
@ -9,7 +9,7 @@
|
|||
|
||||
// Constants
|
||||
#define MCCONF_SIGNATURE 2217312675
|
||||
#define APPCONF_SIGNATURE 3688838172
|
||||
#define APPCONF_SIGNATURE 3494444161
|
||||
|
||||
// Functions
|
||||
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);
|
||||
|
|
|
@ -808,10 +808,9 @@ typedef struct {
|
|||
float brake_current;
|
||||
uint16_t brake_timeout;
|
||||
float yaw_current_clamp;
|
||||
float ki_limit;
|
||||
uint16_t kd_pt1_lowpass_frequency;
|
||||
uint16_t kd_pt1_highpass_frequency;
|
||||
float kd_biquad_lowpass;
|
||||
float kd_biquad_highpass;
|
||||
float booster_angle;
|
||||
float booster_ramp;
|
||||
float booster_current;
|
||||
|
|
Loading…
Reference in New Issue