Add shelving filter support for biquad

jamast · Jul 18, 2016 · e8783ef · e8783ef
1 parent 913b4dc
commit e8783ef
Showing 1 changed file with 38 additions and 0 deletions.
diff --git a/filter_biquad.h b/filter_biquad.h
@@ -109,6 +109,44 @@ class AudioFilterBiquad : public AudioStream
 		/* a2 */ coef[4] = (1.0 - alpha) * scale;
 		setCoefficients(stage, coef);
 	}
+	void setLowShelf(uint32_t stage, float frequency, float gain, float slope = 1.0f) {
+		int coef[5];
+		double a = pow(10.0, gain/40.0);
+		double w0 = frequency * (2 * 3.141592654 / AUDIO_SAMPLE_RATE_EXACT);
+		double sinW0 = sin(w0);
+		//double alpha = (sinW0 * sqrt((a+1/a)*(1/slope-1)+2) ) / 2.0;
+		double cosW0 = cos(w0);
+		//generate three helper-values (intermediate results):
+		double sinsq = sinW0 * sqrt( (pow(a,2.0)+1.0)*(1.0/slope-1.0)+2.0*a );
+		double aMinus = (a-1.0)*cosW0;
+		double aPlus = (a+1.0)*cosW0;
+		double scale = 1073741824.0 / ( (a+1.0) + aMinus + sinsq);
+		/* b0 */ coef[0] =		a *	( (a+1.0) - aMinus + sinsq	) * scale;
+		/* b1 */ coef[1] =  2.0*a * ( (a-1.0) - aPlus  			) * scale;
+		/* b2 */ coef[2] =		a * ( (a+1.0) - aMinus - sinsq 	) * scale;
+		/* a1 */ coef[3] = -2.0*	( (a-1.0) + aPlus			) * scale;
+		/* a2 */ coef[4] =  		( (a+1.0) + aMinus - sinsq	) * scale;
+		setCoefficients(stage, coef);
+	}
+	void setHighShelf(uint32_t stage, float frequency, float gain, float slope = 1.0f) {
+		int coef[5];
+		double a = pow(10.0, gain/40.0);
+		double w0 = frequency * (2 * 3.141592654 / AUDIO_SAMPLE_RATE_EXACT);
+		double sinW0 = sin(w0);
+		//double alpha = (sinW0 * sqrt((a+1/a)*(1/slope-1)+2) ) / 2.0;
+		double cosW0 = cos(w0);
+		//generate three helper-values (intermediate results):
+		double sinsq = sinW0 * sqrt( (pow(a,2.0)+1.0)*(1.0/slope-1.0)+2.0*a );
+		double aMinus = (a-1.0)*cosW0;
+		double aPlus = (a+1.0)*cosW0;
+		double scale = 1073741824.0 / ( (a+1.0) - aMinus + sinsq);
+		/* b0 */ coef[0] =		a *	( (a+1.0) + aMinus + sinsq	) * scale;
+		/* b1 */ coef[1] = -2.0*a * ( (a-1.0) + aPlus  			) * scale;
+		/* b2 */ coef[2] =		a * ( (a+1.0) + aMinus - sinsq 	) * scale;
+		/* a1 */ coef[3] =  2.0*	( (a-1.0) - aPlus			) * scale;
+		/* a2 */ coef[4] =  		( (a+1.0) - aMinus - sinsq	) * scale;
+		setCoefficients(stage, coef);
+	}
 
 private:
 	int32_t definition[32];  // up to 4 cascaded biquads