Implement automatic gain control

src/lsi/beatdetect.c

@@ -32,6 +32,16 @@
 
 #define CHISTSIZE 512
 
+#define SHISTSIZE 200
+
+#define CLIPDELTA (20)
+#define SMAXMIN (24576)
+#define SMAXMAX (31129)
+#define DOWNDELTA (3)
+#define UPDELTA (1)
+#define VOLMIN (1)
+#define VOLMAX (255)
+
 double outputslist[NOUTPUTS] = {
 	1, 3, 7, 15, 31, 63, 127, 255, 511, 1023
 };
@@ -67,14 +77,17 @@ int minp, maxp;
 double lastsum[NOUTPUTS];
 double history[NOUTPUTS][HISTSIZE];
 double chistory[NOUTPUTS][CHISTSIZE];
+int shistory[SHISTSIZE];
 int histptr;
 int chistptr;
+int shistptr;
 char buffer[BUFSIZE];
 char *bufptr;
 int bufleft;
 double phase;
 double confidence;
 int audio_initialised = 0;
+int volume;
 
 #define HISTORY(i, x) (history[i][((histptr+(x) >= HISTSIZE) ? histptr+(x)-HISTSIZE : histptr+(x))])
 
@@ -96,6 +109,16 @@ int audio_initialised = 0;
 			chistptr = 0;			\
 		 } while (0)
 
+#define SHISTORY(x) (shistory[((shistptr+(x) >= SHISTSIZE) ? shistptr+(x)-SHISTSIZE : shistptr+(x))])
+
+#define SHISTWRITE(x) shistory[shistptr] = (x)
+
+#define SHISTNEXT do {					\
+		    shistptr++;				\
+		    if (shistptr >= SHISTSIZE)		\
+			shistptr = 0;			\
+		 } while (0)
+
 #define SUMSQ(a, b) ((a) * (a) + (b) * (b))
 #define MAGSQ(i) SUMSQ(freqs[2*(i)], freqs[2*(i)+1])
 
@@ -131,7 +154,7 @@ int beatdetect_init(void)
 	info.record.channels = 1;
 	info.record.precision = 16;
 	info.record.encoding = AUDIO_ENCODING_SLINEAR;
-	info.record.gain = oinfo.record.gain;
+	volume = info.record.gain = oinfo.record.gain;
 	info.record.port = oinfo.record.port;
 	info.record.balance = oinfo.record.balance;
 
@@ -160,6 +183,24 @@ int beatdetect_init(void)
 	return 1;
 }
 
+void beatdetect_volume(int vol)
+{
+	audio_info_t info;
+
+	if (audiofd < 0)
+		return;
+	if (ioctl(audiofd, AUDIO_GETINFO, &info) < 0) {
+		printf("ERROR: can't get audio info\n");
+		return;
+	}
+	info.record.gain = vol;
+/*	printf("Set record gain to %d\n", vol); */
+	if (ioctl(audiofd, AUDIO_SETINFO, &info) < 0) {
+		printf("ERROR: can't set audio info\n");
+		return;
+	}
+}
+
 int beatdetect_read(void)
 {
 #if 0
@@ -168,6 +209,8 @@ int beatdetect_read(void)
 	int i, j, n, p;
 	int rv;
 	int clip;
+	int smax, smaxh;
+	int changevol;
 	double localsum[NOUTPUTS];
 	fft_type *freqs;
 	int count;
@@ -179,6 +222,7 @@ int beatdetect_read(void)
 	double cmax;
 	int mi, mj, mp;
 	int cmi, cmj, cmp;
+	int ovolume;
 
 	if (!audio_initialised)
 		return 0;
@@ -216,14 +260,55 @@ int beatdetect_read(void)
 		fft_data_signed16((int16_t *)buffer);
 
 		clip = 0;
+		smax = 0;
 
 		/* Check for clip and compute rms */
 		for (i = 0; i < BUFSIZE/2; i++) {
 			int sample = ((int16_t *)buffer)[i];
+			if (abs(sample) > smax)
+				smax = abs(sample);
 			if ((sample == INT16_MAX) || (sample == (-1 - INT16_MAX)))
 				clip = 1;
 		}
 
+		SHISTWRITE(smax);
+		SHISTNEXT;
+
+		smaxh = 0;
+		changevol = 0;
+
+		for (i = 0; i < SHISTSIZE; i++) {
+			if (SHISTORY(i) > smaxh)
+				smaxh = SHISTORY(i);
+		}
+
+		/* Ideal smax range is between 75% and 95% of full signal */
+		
+		ovolume = volume;
+
+		if (clip) {
+			volume = volume - CLIPDELTA;
+			changevol = 1;
+		} else {
+			if (smax > SMAXMAX) {
+				volume = volume - DOWNDELTA;
+				changevol = 1;
+			}
+			if (smaxh < SMAXMIN) {
+				volume = volume + UPDELTA;
+				changevol = 1;
+			}
+		}	
+
+		if (changevol) {
+			if (volume < VOLMIN)
+				volume = 1;
+			if (volume > VOLMAX)
+				volume = VOLMAX;
+			if (volume != ovolume)
+				beatdetect_volume(volume);
+		}
+
 		fft_window();
 		fft_compute();
 		freqs = fft_getresult();