Work on emphasis filter: Neutral @ 1000 Hz; Use filter before de-emphasis

5 files changed, 89 insertions, 69 deletions

diff --git a/src/cnetz/cnetz.c b/src/cnetz/cnetz.c
index c811ba7..57cd289 100644
--- a/src/cnetz/cnetz.c
+++ b/src/cnetz/cnetz.c
@@ -143,7 +143,7 @@
 /* uncomment this to do echo debugging (-L) on Speech Channel */
 //#define DEBUG_SPK
 
-#define CUT_OFF_EMPHASIS_CNETZ	796 /* 200 uS time constant */
+#define CUT_OFF_EMPHASIS_CNETZ	796.0 /* 200 uS time constant */
 
 /* Call reference for calls from mobile station to network
    This offset of 0x400000000 is required for MNCC interface. */
diff --git a/src/common/debug.c b/src/common/debug.c
index fe4de48..ab79b0f 100644
--- a/src/common/debug.c
+++ b/src/common/debug.c
@@ -126,11 +126,11 @@ const char *debug_db(double level_db)
 	static char text[128];
 	int l;
 
-	strcpy(text, ":  .  :  .  :  .  :  .  :  .  :  .  :  .  :  .  :   ");
+	strcpy(text, ":  .  :  .  :  .  :  .  :  .  :  .  :  .  :  .  |  .  :  .  :  .  :  .  :  .  :  .  :  .  :  .  :");
 	if (level_db <= 0.0)
 		return text;
 	l = (int)round(level2db(level_db));
-	if (l > 3)
+	if (l > 48)
 		return text;
 	if (l < -48)
 		return text;
diff --git a/src/common/emphasis.c b/src/common/emphasis.c
index 57273d4..93c2b6f 100644
--- a/src/common/emphasis.c
+++ b/src/common/emphasis.c
@@ -21,37 +21,60 @@
 #include <stdint.h>
 #include <string.h>
 #include <math.h>
+#include "filter.h"
 #include "emphasis.h"
 #include "debug.h"
 
 #define PI		M_PI
 
-#define CUT_OFF_H	200.0	/* cut-off frequency for high-pass filters */
+#define CUT_OFF_H	300.0	/* cut-off frequency for high-pass filters */
+
+static void gen_sine(double *samples, int num, int samplerate, double freq)
+{
+	int i;
+
+	for (i = 0; i < num; i++)
+		samples[i] = cos(2.0 * M_PI * freq / (double)samplerate * (double)i);
+}
+
+static double get_level(double *samples, int num)
+{
+	int i;
+	double envelope = 0;
+	for (i = num/2; i < num; i++) {
+		if (samples[i] > envelope)
+			envelope = samples[i];
+	}
+
+	return envelope;
+}
 
 int init_emphasis(emphasis_t *state, int samplerate, double cut_off)
 {
-	double factor, rc, dt;
+	double factor;
+	double test_samples[samplerate / 10];
 
 	memset(state, 0, sizeof(*state));
-	if (samplerate < 24000) {
-		PDEBUG(DDSP, DEBUG_ERROR, "Sample rate must be at least 24000 Hz!\n");
-		return -1;
-	}
 
 	/* exp (-2 * PI * CUT_OFF * delta_t) */
-	factor = exp(-2.0 * PI * cut_off / samplerate); /* 1/samplerate == delta_t */
+	factor = exp(-2.0 * PI * cut_off / (double)samplerate); /* 1/samplerate == delta_t */
+
 	PDEBUG(DDSP, DEBUG_DEBUG, "Emphasis factor = %.3f\n", factor);
 	state->p.factor = factor;
-	state->p.amp = samplerate / 6400.0;
-	state->d.d_factor = factor;
-	state->d.amp = 1.0 / (samplerate / 5750.0);
-
-	/* high-pass filter prevents low frequency noise and dc level
-	 * from being amplified by de-emphasis */
-	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);
+	state->p.amp = 1.0;
+	state->d.factor = factor;
+	state->d.amp = 1.0;
+
+	filter_highpass_init(&state->d.hp, CUT_OFF_H, samplerate, 1);
+
+	/* calibrate amplification to be neutral at 1000 Hz */
+	gen_sine(test_samples, sizeof(test_samples) / sizeof(test_samples[0]), samplerate, 1000.0);
+	pre_emphasis(state, test_samples, sizeof(test_samples) / sizeof(test_samples[0]));
+	state->p.amp = 1.0 / get_level(test_samples, sizeof(test_samples) / sizeof(test_samples[0]));
+	gen_sine(test_samples, sizeof(test_samples) / sizeof(test_samples[0]), samplerate, 1000.0);
+	de_emphasis(state, test_samples, sizeof(test_samples) / sizeof(test_samples[0]));
+	state->d.amp = 1.0 / get_level(test_samples, sizeof(test_samples) / sizeof(test_samples[0]));
+
 	return 0;
 }
 
@@ -65,13 +88,14 @@ void pre_emphasis(emphasis_t *state, double *samples, int num)
 	amp = state->p.amp;
 
 	for (i = 0; i < num; i++) {
-		x = *samples / 32768.0;
+		x = *samples;
 
+		/* pre-emphasis */
 		y = x - factor * x_last;
 
 		x_last = x;
 
-		*samples++ = (int)(amp * y * 32768.0);
+		*samples++ = amp * y;
 	}
 
 	state->p.x_last = x_last;
@@ -79,31 +103,26 @@ void pre_emphasis(emphasis_t *state, double *samples, int num)
 
 void de_emphasis(emphasis_t *state, double *samples, int num)
 {
-	double x, y, z, y_last, z_last, d_factor, h_factor, amp;
+	double x, y, y_last, factor, amp;
 	int i;
 
+	filter_process(&state->d.hp, samples, num);
+
 	y_last = state->d.y_last;
-	z_last = state->d.z_last;
-	d_factor = state->d.d_factor;
-	h_factor = state->d.h_factor;
+	factor = state->d.factor;
 	amp = state->d.amp;
 
 	for (i = 0; i < num; i++) {
-		x = *samples / 32768.0;
+		x = *samples;
 
 		/* de-emphasis */
-		y = x + d_factor * y_last;
-
-		/* high pass */
-		z = h_factor * (z_last + y - y_last);
+		y = x + factor * y_last;
 
 		y_last = y;
-		z_last = z;
 
-		*samples++ = (int)(amp * z * 32768.0);
+		*samples++ = amp * y;
 	}
 
 	state->d.y_last = y_last;
-	state->d.z_last = z_last;
 }
 
diff --git a/src/common/emphasis.h b/src/common/emphasis.h
index 39d5246..53450d8 100644
--- a/src/common/emphasis.h
+++ b/src/common/emphasis.h
@@ -5,15 +5,14 @@ typedef struct emphasis {
 		double amp;
 	} p;
 	struct {
+		filter_t hp;
 		double y_last;
-		double z_last;
-		double d_factor;
-		double h_factor;
+		double factor;
 		double amp;
 	} d;
 } emphasis_t;
 
-#define CUT_OFF_EMPHASIS_DEFAULT	300.0
+#define CUT_OFF_EMPHASIS_DEFAULT 300.0
 
 int init_emphasis(emphasis_t *state, int samplerate, double cut_off);
 void pre_emphasis(emphasis_t *state, double *samples, int num);
diff --git a/src/test/test_emphasis.c b/src/test/test_emphasis.c
index 79799d2..c5fa1af 100644
--- a/src/test/test_emphasis.c
+++ b/src/test/test_emphasis.c
@@ -3,6 +3,7 @@
 #include <math.h>
 #include <string.h>
 #include "../common/sample.h"
+#include "../common/filter.h"
 #include "../common/emphasis.h"
 #include "../common/debug.h"
 
@@ -10,29 +11,17 @@
 #define db2level(db)		pow(10, (double)db / 20.0)
 
 #define SAMPLERATE	48000
-#define DEVIATION	8000.0
 
-static double test_freq[] = { 25, 50, 100, 200, 250, 300, 400, 500, 1000, 2000, 4000, 0 };
-
-static void check_level(sample_t *samples, double freq, const char *desc)
+static double get_level(double *samples)
 {
 	int i;
-	int last = 0, envelope = 0;
-	int up = 0;
-
-	for (i = 0; i < SAMPLERATE; i++) {
-		if (last < samples[i]) {
-			up = 1;
-		} else if (last > samples[i]) {
-			if (up) {
-				if (last > envelope)
-					envelope = last;
-			}
-			up = 0;
-		}
-		last = samples[i];
+	double envelope = 0;
+	for (i = SAMPLERATE/2; i < SAMPLERATE; i++) {
+		if (samples[i] > envelope)
+			envelope = samples[i];
 	}
-	printf("%s: f = %.0f envelop = %.4f\n", desc, freq, level2db((double)envelope / DEVIATION));
+
+	return envelope;
 }
 
 static void gen_samples(double *samples, double freq)
@@ -41,34 +30,47 @@ static void gen_samples(double *samples, double freq)
 	double value;
 
 	for (i = 0; i < SAMPLERATE; i++) {
-		value = sin(2.0 * M_PI * freq / (double)SAMPLERATE * (double)i);
-		samples[i] = value * DEVIATION;
+		value = cos(2.0 * M_PI * freq / (double)SAMPLERATE * (double)i);
+		samples[i] = value;
 	}
 }
 
 int main(void)
 {
 	emphasis_t estate;
+	double cut_off = CUT_OFF_EMPHASIS_DEFAULT;
 	double samples[SAMPLERATE];
-	int i;
+	double level;
+	double i;
 
 	debuglevel = DEBUG_DEBUG;
 
-	printf("1000 Hz shall be close to 0 dB, that is no significant change in volume.\n\n");
+	init_emphasis(&estate, SAMPLERATE, cut_off);
 
-	init_emphasis(&estate, SAMPLERATE, CUT_OFF_EMPHASIS_DEFAULT);
+	printf("testing pre-emphasis filter with cut-off frequency %.1f\n", cut_off);
 
-	for (i = 0; test_freq[i]; i++) {
-		gen_samples(samples, test_freq[i]);
+	for (i = 31.25; i < 4001; i = i * sqrt(sqrt(2.0))) {
+		gen_samples(samples, (double)i);
 		pre_emphasis(&estate, samples, SAMPLERATE);
-		check_level(samples, test_freq[i], "pre-emphasis");
+		level = get_level(samples);
+		printf("%s%.0f Hz: %.1f dB", debug_db(level), i, level2db(level));
+		if ((int)round(i) == 1000)
+			printf(" level=%.6f\n", level);
+		else
+			printf("\n");
 	}
 
-	/* generate sweep 0..4khz */
-	for (i = 0; test_freq[i]; i++) {
-		gen_samples(samples, test_freq[i]);
+	printf("testing de-emphasis filter with cut-off frequency %.1f\n", cut_off);
+
+	for (i = 31.25; i < 4001; i = i * sqrt(sqrt(2.0))) {
+		gen_samples(samples, (double)i);
 		de_emphasis(&estate, samples, SAMPLERATE);
-		check_level(samples, test_freq[i], "de-emphasis");
+		level = get_level(samples);
+		printf("%s%.0f Hz: %.1f dB", debug_db(level), i, level2db(level));
+		if ((int)round(i) == 1000)
+			printf(" level=%.6f\n", level);
+		else
+			printf("\n");
 	}
 
 	return 0;