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

Biquad filters: add config/reset functions 

Also, swap bq_process arguments mimicking C++

Signed-off-by: Dado Mista <dadomista@gmail.com>
This commit is contained in:
Dado Mista 2021-07-06 11:12:05 -07:00
parent b3c069a378
commit 02caad398a
1 changed files with 36 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
applications/app_balance.c
View File

@ -70,6 +70,11 @@ typedef struct{
float z1, z2;
} Biquad;
typedef enum {
BQ_LOWPASS,
BQ_HIGHPASS
} BiquadType;
// Balance thread
static THD_FUNCTION(balance_thread, arg);
static THD_WORKING_AREA(balance_thread_wa, 2048); // 2kb stack for this thread
@ -128,12 +133,33 @@ static void app_balance_sample_debug(void);
static void app_balance_experiment(void);
// Utility Functions
float biquad_process(float in, Biquad *biquad) {
float biquad_process(Biquad *biquad, float in) {
float out = in * biquad->a0 + biquad->z1;
biquad->z1 = in * biquad->a1 + biquad->z2 - biquad->b1 * out;
biquad->z2 = in * biquad->a2 - biquad->b2 * out;
return out;
}
void biquad_config(Biquad *biquad, BiquadType type, float Fc) {
float K = tanf(M_PI * Fc); // -0.0159;
float Q = 0.707; // maximum sharpness (0.5 = maximum smoothness)
float norm = 1 / (1 + K / Q + K * K);
if (type == BQ_LOWPASS) {
biquad->a0 = K * K * norm;
biquad->a1 = 2 * biquad->a0;
biquad->a2 = biquad->a0;
}
else if (type == BQ_HIGHPASS) {
biquad->a0 = 1 * norm;
biquad->a1 = -2 * biquad->a0;
biquad->a2 = biquad->a0;
}
biquad->b1 = 2 * (K * K - 1) * norm;
biquad->b2 = (1 - K / Q + K * K) * norm;
}
void biquad_reset(Biquad *biquad) {
biquad->z1 = 0;
biquad->z2 = 0;
}
// Exposed Functions
void app_balance_configure(balance_config *conf, imu_config *conf2) {
@ -166,36 +192,15 @@ void app_balance_configure(balance_config *conf, imu_config *conf2) {
}
if(balance_conf.kd_biquad_lowpass > 0){
float Fc = balance_conf.kd_biquad_lowpass / balance_conf.hertz;
float Q = 0.707; // Flat response
float K = tan(M_PI * Fc);
float norm = 1 / (1 + K / Q + K * K);
d_biquad_lowpass.a0 = K * K * norm;
d_biquad_lowpass.a1 = 2 * d_biquad_lowpass.a0;
d_biquad_lowpass.a2 = d_biquad_lowpass.a0;
d_biquad_lowpass.b1 = 2 * (K * K - 1) * norm;
d_biquad_lowpass.b2 = (1 - K / Q + K * K) * norm;
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;
float Q = 0.707; // Flat response
float K = tan(M_PI * Fc);
float norm = 1 / (1 + K / Q + K * K);
d_biquad_highpass.a0 = 1 * norm;
d_biquad_highpass.a1 = -2 * d_biquad_highpass.a0;
d_biquad_highpass.a2 = d_biquad_highpass.a0;
d_biquad_highpass.b1 = 2 * (K * K - 1) * norm;
d_biquad_highpass.b2 = (1 - K / Q + K * K) * norm;
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;
float Q = 0.707; // Flat response
float K = tan(M_PI * Fc);
float norm = 1 / (1 + K / Q + K * K);
torquetilt_current_biquad.a0 = K * K * norm;
torquetilt_current_biquad.a1 = 2 * torquetilt_current_biquad.a0;
torquetilt_current_biquad.a2 = torquetilt_current_biquad.a0;
torquetilt_current_biquad.b1 = 2 * (K * K - 1) * norm;
torquetilt_current_biquad.b2 = (1 - K / Q + K * K) * norm;
biquad_config(&torquetilt_current_biquad, BQ_LOWPASS, Fc);
}
// Variable nose angle adjustment / tiltback (setting is per 1000erpm, convert to per erpm)
@ -283,10 +288,8 @@ static void reset_vars(void){
yaw_last_proportional = 0;
d_pt1_lowpass_state = 0;
d_pt1_highpass_state = 0;
d_biquad_lowpass.z1 = 0;
d_biquad_lowpass.z2 = 0;
d_biquad_highpass.z1 = 0;
d_biquad_highpass.z2 = 0;
biquad_reset(&d_biquad_lowpass);
biquad_reset(&d_biquad_highpass);
// Set values for startup
setpoint = pitch_angle;
setpoint_target_interpolated = pitch_angle;
@ -295,8 +298,7 @@ static void reset_vars(void){
torquetilt_target = 0;
torquetilt_interpolated = 0;
torquetilt_filtered_current = 0;
torquetilt_current_biquad.z1 = 0;
torquetilt_current_biquad.z2 = 0;
biquad_reset(&torquetilt_current_biquad);
turntilt_target = 0;
turntilt_interpolated = 0;
setpointAdjustmentType = CENTERING;
@ -450,7 +452,7 @@ static void apply_noseangling(void){
static void apply_torquetilt(void){
// Filter current (Biquad)
if(balance_conf.torquetilt_filter > 0){
torquetilt_filtered_current = biquad_process(motor_current, &torquetilt_current_biquad);
torquetilt_filtered_current = biquad_process(&torquetilt_current_biquad, motor_current);
}else{
torquetilt_filtered_current = motor_current;
}
@ -701,10 +703,10 @@ static THD_FUNCTION(balance_thread, arg) {
derivative = derivative - d_pt1_highpass_state;
}
if(balance_conf.kd_biquad_lowpass > 0){
derivative = biquad_process(derivative, &d_biquad_lowpass);
derivative = biquad_process(&d_biquad_lowpass, derivative);
}
if(balance_conf.kd_biquad_highpass > 0){
derivative = biquad_process(derivative, &d_biquad_highpass);
derivative = biquad_process(&d_biquad_highpass, derivative);
}
pid_value = (balance_conf.kp * proportional) + (balance_conf.ki * integral) + (balance_conf.kd * derivative);