Merge pull request vedderb#315 from surfdado/bqfix

Balance App - biquad filters: add config/reset functions

Author: vedderb

applications/app_balance.c

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