rusefi
/
bldc
mirror of https://github.com/rusefi/bldc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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:
Benjamin Vedder 2022-09-18 22:57:15 +02:00 committed by GitHub
parent 937c6fe947 2127da0c0f
commit e9386cb339
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 27 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
applications/app_balance.c
View File

@ -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;
}

9
applications/appconf_default.h
View File

@ -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

9
confgenerator.c
View File

@ -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;

2
confgenerator.h
View File

@ -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);

3
datatypes.h
View File

@ -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;