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 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 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 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 float motor_timeout;
|
||||||
static systime_t brake_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);
|
float RC = 1.0 / ( 2.0 * M_PI * balance_conf.kd_pt1_highpass_frequency);
|
||||||
d_pt1_highpass_k = dT / (RC + dT);
|
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
|
if(balance_conf.torquetilt_filter > 0){ // Torquetilt Current Biquad
|
||||||
float Fc = balance_conf.torquetilt_filter / balance_conf.hertz;
|
float Fc = balance_conf.torquetilt_filter / balance_conf.hertz;
|
||||||
biquad_config(&torquetilt_current_biquad, BQ_LOWPASS, Fc);
|
biquad_config(&torquetilt_current_biquad, BQ_LOWPASS, Fc);
|
||||||
|
@ -303,8 +294,6 @@ static void reset_vars(void){
|
||||||
yaw_last_proportional = 0;
|
yaw_last_proportional = 0;
|
||||||
d_pt1_lowpass_state = 0;
|
d_pt1_lowpass_state = 0;
|
||||||
d_pt1_highpass_state = 0;
|
d_pt1_highpass_state = 0;
|
||||||
biquad_reset(&d_biquad_lowpass);
|
|
||||||
biquad_reset(&d_biquad_highpass);
|
|
||||||
// Set values for startup
|
// Set values for startup
|
||||||
setpoint = pitch_angle;
|
setpoint = pitch_angle;
|
||||||
setpoint_target_interpolated = pitch_angle;
|
setpoint_target_interpolated = pitch_angle;
|
||||||
|
@ -733,6 +722,11 @@ static THD_FUNCTION(balance_thread, arg) {
|
||||||
integral = integral + proportional;
|
integral = integral + proportional;
|
||||||
derivative = last_pitch_angle - pitch_angle;
|
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
|
// Apply D term filters
|
||||||
if(balance_conf.kd_pt1_lowpass_frequency > 0){
|
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);
|
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);
|
d_pt1_highpass_state = d_pt1_highpass_state + d_pt1_highpass_k * (derivative - d_pt1_highpass_state);
|
||||||
derivative = 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);
|
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];
|
proportional2 = pid_value - gyro[1];
|
||||||
integral2 = integral2 + proportional2;
|
integral2 = integral2 + proportional2;
|
||||||
derivative2 = last_gyro_y - gyro[1];
|
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);
|
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;
|
return filtered_loop_overshoot;
|
||||||
case(13):
|
case(13):
|
||||||
return filtered_diff_time;
|
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:
|
default:
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
@ -445,18 +445,15 @@
|
||||||
#ifndef APPCONF_BALANCE_YAW_CURRENT_CLAMP
|
#ifndef APPCONF_BALANCE_YAW_CURRENT_CLAMP
|
||||||
#define APPCONF_BALANCE_YAW_CURRENT_CLAMP 0.0
|
#define APPCONF_BALANCE_YAW_CURRENT_CLAMP 0.0
|
||||||
#endif
|
#endif
|
||||||
|
#ifndef APPCONF_BALANCE_KI_LIMIT
|
||||||
|
#define APPCONF_BALANCE_KI_LIMIT 0
|
||||||
|
#endif
|
||||||
#ifndef APPCONF_BALANCE_KD_PT1_LOWPASS_FREQUENCY
|
#ifndef APPCONF_BALANCE_KD_PT1_LOWPASS_FREQUENCY
|
||||||
#define APPCONF_BALANCE_KD_PT1_LOWPASS_FREQUENCY 0
|
#define APPCONF_BALANCE_KD_PT1_LOWPASS_FREQUENCY 0
|
||||||
#endif
|
#endif
|
||||||
#ifndef APPCONF_BALANCE_KD_PT1_HIGHPASS_FREQUENCY
|
#ifndef APPCONF_BALANCE_KD_PT1_HIGHPASS_FREQUENCY
|
||||||
#define APPCONF_BALANCE_KD_PT1_HIGHPASS_FREQUENCY 0
|
#define APPCONF_BALANCE_KD_PT1_HIGHPASS_FREQUENCY 0
|
||||||
#endif
|
#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
|
#ifndef APPCONF_BALANCE_BOOSTER_ANGLE
|
||||||
#define APPCONF_BALANCE_BOOSTER_ANGLE 8
|
#define APPCONF_BALANCE_BOOSTER_ANGLE 8
|
||||||
#endif
|
#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_float32_auto(buffer, conf->app_balance_conf.brake_current, &ind);
|
||||||
buffer_append_uint16(buffer, conf->app_balance_conf.brake_timeout, &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.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_lowpass_frequency, &ind);
|
||||||
buffer_append_uint16(buffer, conf->app_balance_conf.kd_pt1_highpass_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_angle, &ind);
|
||||||
buffer_append_float32_auto(buffer, conf->app_balance_conf.booster_ramp, &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);
|
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_current = buffer_get_float32_auto(buffer, &ind);
|
||||||
conf->app_balance_conf.brake_timeout = buffer_get_uint16(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.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_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_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_angle = buffer_get_float32_auto(buffer, &ind);
|
||||||
conf->app_balance_conf.booster_ramp = 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);
|
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_current = APPCONF_BALANCE_BRAKE_CURRENT;
|
||||||
conf->app_balance_conf.brake_timeout = APPCONF_BALANCE_BRAKE_TIMEOUT;
|
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.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_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_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_angle = APPCONF_BALANCE_BOOSTER_ANGLE;
|
||||||
conf->app_balance_conf.booster_ramp = APPCONF_BALANCE_BOOSTER_RAMP;
|
conf->app_balance_conf.booster_ramp = APPCONF_BALANCE_BOOSTER_RAMP;
|
||||||
conf->app_balance_conf.booster_current = APPCONF_BALANCE_BOOSTER_CURRENT;
|
conf->app_balance_conf.booster_current = APPCONF_BALANCE_BOOSTER_CURRENT;
|
||||||
|
|
|
@ -9,7 +9,7 @@
|
||||||
|
|
||||||
// Constants
|
// Constants
|
||||||
#define MCCONF_SIGNATURE 2217312675
|
#define MCCONF_SIGNATURE 2217312675
|
||||||
#define APPCONF_SIGNATURE 3688838172
|
#define APPCONF_SIGNATURE 3494444161
|
||||||
|
|
||||||
// Functions
|
// Functions
|
||||||
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);
|
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);
|
||||||
|
|
|
@ -808,10 +808,9 @@ typedef struct {
|
||||||
float brake_current;
|
float brake_current;
|
||||||
uint16_t brake_timeout;
|
uint16_t brake_timeout;
|
||||||
float yaw_current_clamp;
|
float yaw_current_clamp;
|
||||||
|
float ki_limit;
|
||||||
uint16_t kd_pt1_lowpass_frequency;
|
uint16_t kd_pt1_lowpass_frequency;
|
||||||
uint16_t kd_pt1_highpass_frequency;
|
uint16_t kd_pt1_highpass_frequency;
|
||||||
float kd_biquad_lowpass;
|
|
||||||
float kd_biquad_highpass;
|
|
||||||
float booster_angle;
|
float booster_angle;
|
||||||
float booster_ramp;
|
float booster_ramp;
|
||||||
float booster_current;
|
float booster_current;
|
||||||
|
|
Loading…
Reference in New Issue