common code: Fixed de-emphasis by adding high pass filter

This high-pass filter filters out DC and frequencies below 300 Hz.
Now de-emphases will not cause low frequencies to saturate audio level
range or cause too high levels on low frequencies.

src/common/emphasis.c

@@ -24,10 +24,14 @@
 #include "emphasis.h"
 #include "debug.h"
 
+#define PI		M_PI
+
+#define CUT_OFF_E	300.0	/* cut-off frequency for emphasis filters */
+#define CUT_OFF_H	300.0	/* cut-off frequency for high-pass filters */
 
 int init_emphasis(emphasis_t *state, int samplerate)
 {
-	double factor;
+	double factor, rc, dt;
 
 	memset(state, 0, sizeof(*state));
 	if (samplerate < 24000) {
@@ -35,33 +39,39 @@ int init_emphasis(emphasis_t *state, int samplerate)
 		return -1;
 	}
 
-	factor = 0.97;
+	/* exp (-2 * PI * CUT_OFF * delta_t) */
+	factor = exp(-2.0 * PI * CUT_OFF_E / samplerate); /* 1/samplerate == delta_t */
+	PDEBUG(DDSP, DEBUG_DEBUG, "Emphasis factor = %.3f\n", factor);
 	state->p.factor = factor;
 	state->p.amp = samplerate / 6350.0;
-	state->d.factor = factor;
+	state->d.d_factor = factor;
-	state->d.amp = 1.0 / (samplerate / 6350.0);
+	state->d.amp = 1.0 / (samplerate / 5550.0);
 
+	rc = 1.0 / (CUT_OFF_H * 2.0 *3.14);
+	dt = 1.0 / samplerate;
+	state->d.h_factor = rc / (rc + dt);
+	PDEBUG(DDSP, DEBUG_DEBUG, "High-Pass factor = %.3f\n", state->d.h_factor);
 	return 0;
 }
 
 void pre_emphasis(emphasis_t *state, int16_t *samples, int num)
 {
 	int32_t sample;
-	double old_value, new_value, last_value, factor, amp;
+	double x, y, x_last, factor, amp;
 	int i;
 
-	last_value = state->p.last_value;
+	x_last = state->p.x_last;
 	factor = state->p.factor;
 	amp = state->p.amp;
 
 	for (i = 0; i < num; i++) {
-		old_value = (double)(*samples) / 32768.0;
+		x = (double)(*samples) / 32768.0;
 
-		new_value = old_value - factor * last_value;
+		y = x - factor * x_last;
 
-		last_value = old_value;
+		x_last = x;
 
-		sample = (int)(amp * new_value * 32768.0);
+		sample = (int)(amp * y * 32768.0);
 		if (sample > 32767)
 			sample = 32767;
 		else if (sample < -32768)
@@ -69,27 +79,34 @@ void pre_emphasis(emphasis_t *state, int16_t *samples, int num)
 		*samples++ = sample;
 	}
 
-	state->p.last_value = last_value;
+	state->p.x_last = x_last;
 }
 
 void de_emphasis(emphasis_t *state, int16_t *samples, int num)
 {
 	int32_t sample;
-	double old_value, new_value, last_value, factor, amp;
+	double x, y, z, y_last, z_last, d_factor, h_factor, amp;
 	int i;
 
-	last_value = state->d.last_value;
+	y_last = state->d.y_last;
-	factor = state->d.factor;
+	z_last = state->d.z_last;
+	d_factor = state->d.d_factor;
+	h_factor = state->d.h_factor;
 	amp = state->d.amp;
 
 	for (i = 0; i < num; i++) {
-		old_value = (double)(*samples) / 32768.0;
+		x = (double)(*samples) / 32768.0;
 
-		new_value = old_value + factor * last_value;
+		/* de-emphasis */
+		y = x + d_factor * y_last;
 
-		last_value = new_value;
+		/* high pass */
+		z = h_factor * (z_last + y - y_last);
 
-		sample = (int)(amp * new_value * 32768.0);
+		y_last = y;
+		z_last = z;
+
+		sample = (int)(amp * z * 32768.0);
 		if (sample > 32767)
 			sample = 32767;
 		else if (sample < -32768)
@@ -97,6 +114,7 @@ void de_emphasis(emphasis_t *state, int16_t *samples, int num)
 		*samples++ = sample;
 	}
 
-	state->d.last_value = last_value;
+	state->d.y_last = y_last;
+	state->d.z_last = z_last;
 }
 

src/common/emphasis.h

@@ -1,12 +1,14 @@
 typedef struct emphasis {
 	struct {
-		double last_value;
+		double x_last;
 		double factor;
 		double amp;
 	} p;
 	struct {
-		double last_value;
+		double y_last;
-		double factor;
+		double z_last;
+		double d_factor;
+		double h_factor;
 		double amp;
 	} d;
 } emphasis_t;

src/test/test_emphasis.c

@@ -3,6 +3,7 @@
 #include <math.h>
 #include <string.h>
 #include "../common/emphasis.h"
+#include "../common/debug.h"
 
 #define level2db(level)		(20 * log10(level))
 #define db2level(db)		pow(10, (double)db / 20.0)
@@ -10,39 +11,36 @@
 #define SAMPLERATE	48000
 #define DEVIATION	8000.0
 
-static void check_level(int16_t *samples, const char *desc)
+static double test_freq[] = { 25, 50, 100, 150, 200, 250, 300, 350, 400, 500, 1000, 2000, 4000, 0 };
+
+static void check_level(int16_t *samples, double freq, const char *desc)
 {
 	int i;
 	int last = 0, envelope = 0;
 	int up = 0;
-	int freq;
 
 	for (i = 0; i < SAMPLERATE; i++) {
 		if (last < samples[i]) {
 			up = 1;
 		} else if (last > samples[i]) {
 			if (up) {
-				envelope = last;
+				if (last > envelope)
+					envelope = last;
 			}
 			up = 0;
 		}
-		if ((i % (SAMPLERATE/40)) == 0) {
-			freq = 500 + 500 * (i / (SAMPLERATE / 8));
-			printf("%s: f = %d envelop = %.4f\n", desc, freq, level2db((double)envelope / DEVIATION));
-		}
 		last = samples[i];
 	}
+	printf("%s: f = %.0f envelop = %.4f\n", desc, freq, level2db((double)envelope / DEVIATION));
 }
 
-static void gen_samples(int16_t *samples)
+static void gen_samples(int16_t *samples, double freq)
 {
 	int i;
 	double value;
-	int freq;
 
 	for (i = 0; i < SAMPLERATE; i++) {
-		freq = 500 + 500 * (i / (SAMPLERATE / 8));
+		value = sin(2.0 * M_PI * freq / (double)SAMPLERATE * (double)i);
-		value = sin(2.0 * M_PI * (double)freq / (double)SAMPLERATE * (double)i);
 		samples[i] = value * DEVIATION;
 	}
 }
@@ -51,26 +49,26 @@ int main(void)
 {
 	emphasis_t estate;
 	int16_t samples[SAMPLERATE];
+	int i;
+
+	debuglevel = DEBUG_DEBUG;
 
 	printf("1000 Hz shall be close to 0 dB, that is no significant change in volume.\n\n");
 
-	/* generate sweep 0..4khz */
-	gen_samples(samples);
-
 	init_emphasis(&estate, SAMPLERATE);
 
-//	check_level(samples, "unchanged");
+	for (i = 0; test_freq[i]; i++) {
-
+		gen_samples(samples, test_freq[i]);
-	pre_emphasis(&estate, samples, SAMPLERATE);
+		pre_emphasis(&estate, samples, SAMPLERATE);
-
+		check_level(samples, test_freq[i], "pre-emphasis");
-	check_level(samples, "pre-emphasis");
+	}
 
 	/* generate sweep 0..4khz */
-	gen_samples(samples);
+	for (i = 0; test_freq[i]; i++) {
-
+		gen_samples(samples, test_freq[i]);
-	de_emphasis(&estate, samples, SAMPLERATE);
+		de_emphasis(&estate, samples, SAMPLERATE);
-
+		check_level(samples, test_freq[i], "de-emphasis");
-	check_level(samples, "de-emphasis");
+	}
 
 	return 0;
 }