very simple filter which can remove a sine at a specified frequency, usefull to get rid of the 50/60hz noise on ultra crappy equipment

probably works only with 1 channel input


git-svn-id: svn://svn.mplayerhq.hu/mplayer/trunk@18083 b3059339-0415-0410-9bf9-f77b7e298cf2

4 files changed, 186 insertions, 0 deletions

diff --git a/libaf/Makefile b/libaf/Makefile
index 8c72f4fec4..62419e88df 100644
--- a/libaf/Makefile
+++ b/libaf/Makefile
@@ -16,6 +16,7 @@ SRCS=af.c \
      af_hrtf.c \
      af_pan.c \
      af_resample.c \
+     af_sinesupress.c \
      af_sub.c \
      af_surround.c \
      af_sweep.c \
diff --git a/libaf/af.c b/libaf/af.c
index 04aa285743..14852ad6c6 100644
--- a/libaf/af.c
+++ b/libaf/af.c
@@ -29,6 +29,7 @@ extern af_info_t af_info_sweep;
 extern af_info_t af_info_hrtf;
 extern af_info_t af_info_ladspa;
 extern af_info_t af_info_center;
+extern af_info_t af_info_sinesupress;
 
 static af_info_t* filter_list[]={ 
    &af_info_dummy,
@@ -57,6 +58,7 @@ static af_info_t* filter_list[]={
    &af_info_ladspa,
 #endif
    &af_info_center,
+   &af_info_sinesupress,
    NULL 
 };
 
diff --git a/libaf/af_sinesupress.c b/libaf/af_sinesupress.c
new file mode 100644
index 0000000000..71306dbc62
--- /dev/null
+++ b/libaf/af_sinesupress.c
@@ -0,0 +1,178 @@
+/*=============================================================================
+//	
+//  This software has been released under the terms of the GNU General Public
+//  license. See http://www.gnu.org/copyleft/gpl.html for details.
+//
+//  Copyright 2006 Michael Niedermayer
+//  Copyright 2004 Alex Beregszaszi & Pierre Lombard (original af_extrastereo.c upon which this is based)
+//
+//=============================================================================
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h> 
+
+#include <unistd.h>
+#include <inttypes.h>
+#include <math.h>
+#include <limits.h>
+
+#include "af.h"
+
+// Data for specific instances of this filter
+typedef struct af_sinesupress_s
+{
+    double freq;
+    double decay;
+    double real;
+    double imag;
+    double ref;
+    double pos;
+}af_sinesupress_t;
+
+static af_data_t* play_s16(struct af_instance_s* af, af_data_t* data);
+static af_data_t* play_float(struct af_instance_s* af, af_data_t* data);
+
+// Initialization and runtime control
+static int control(struct af_instance_s* af, int cmd, void* arg)
+{
+  af_sinesupress_t* s   = (af_sinesupress_t*)af->setup; 
+
+  switch(cmd){
+  case AF_CONTROL_REINIT:{
+    // Sanity check
+    if(!arg) return AF_ERROR;
+    
+    af->data->rate   = ((af_data_t*)arg)->rate;
+    af->data->nch    = 1;
+#if 0
+    if (((af_data_t*)arg)->format == AF_FORMAT_FLOAT_NE)
+    {
+	af->data->format = AF_FORMAT_FLOAT_NE;
+	af->data->bps = 4;
+	af->play = play_float;
+    }// else
+#endif
+    {
+	af->data->format = AF_FORMAT_S16_NE;
+	af->data->bps = 2;
+	af->play = play_s16;
+    }
+
+    return af_test_output(af,(af_data_t*)arg);
+  }
+  case AF_CONTROL_COMMAND_LINE:{
+    float f1,f2;
+    sscanf((char*)arg,"%f:%f", &f1,&f2);
+    s->freq = f1;
+    s->decay = f2;
+    return AF_OK;
+  }
+  case AF_CONTROL_SS_FREQ | AF_CONTROL_SET:
+    s->freq = *(float*)arg;
+    return AF_OK;
+  case AF_CONTROL_SS_FREQ | AF_CONTROL_GET:
+    *(float*)arg = s->freq;
+    return AF_OK;
+  case AF_CONTROL_SS_DECAY | AF_CONTROL_SET:
+    s->decay = *(float*)arg;
+    return AF_OK;
+  case AF_CONTROL_SS_DECAY | AF_CONTROL_GET:
+    *(float*)arg = s->decay;
+    return AF_OK;
+  }
+  return AF_UNKNOWN;
+}
+
+// Deallocate memory 
+static void uninit(struct af_instance_s* af)
+{
+  if(af->data)
+    free(af->data);
+  if(af->setup)
+    free(af->setup);
+}
+
+// Filter data through filter
+static af_data_t* play_s16(struct af_instance_s* af, af_data_t* data)
+{
+  af_sinesupress_t *s = af->setup;
+  register int i = 0;
+  int16_t *a = (int16_t*)data->audio;	// Audio data
+  int len = data->len/2;		// Number of samples
+  int avg, l, r;
+  
+  for (i = 0; i < len; i++)
+  {
+    double co= cos(s->pos);
+    double si= sin(s->pos);
+
+    s->real += co * a[i];
+    s->imag += si * a[i];
+    s->ref  += co * co;
+
+    a[i] -= (s->real * co + s->imag * si) / s->ref;
+
+    s->real -= s->real * s->decay;
+    s->imag -= s->imag * s->decay;
+    s->ref  -= s->ref  * s->decay;
+
+    s->pos += 2 * M_PI * s->freq / data->rate;
+  }
+
+   af_msg(AF_MSG_VERBOSE,"[sinesupress] f:%8.2f: amp:%8.2f\n", s->freq, sqrt(s->real*s->real + s->imag*s->imag) / s->ref);
+
+  return data;
+}
+
+#if 0
+static af_data_t* play_float(struct af_instance_s* af, af_data_t* data)
+{
+  af_sinesupress_t *s = af->setup;
+  register int i = 0;
+  float *a = (float*)data->audio;	// Audio data
+  int len = data->len/4;		// Number of samples
+  float avg, l, r;
+  
+  for (i = 0; i < len; i+=2)
+  {
+    avg = (a[i] + a[i + 1]) / 2;
+    
+/*    l = avg + (s->mul * (a[i] - avg));
+    r = avg + (s->mul * (a[i + 1] - avg));*/
+    
+    a[i] = af_softclip(l);
+    a[i + 1] = af_softclip(r);
+  }
+
+  return data;
+}
+#endif
+
+// Allocate memory and set function pointers
+static int open(af_instance_t* af){
+  af->control=control;
+  af->uninit=uninit;
+  af->play=play_s16;
+  af->mul.n=1;
+  af->mul.d=1;
+  af->data=calloc(1,sizeof(af_data_t));
+  af->setup=calloc(1,sizeof(af_sinesupress_t));
+  if(af->data == NULL || af->setup == NULL)
+    return AF_ERROR;
+
+  ((af_sinesupress_t*)af->setup)->freq = 50.0;
+  ((af_sinesupress_t*)af->setup)->decay = 0.0001;
+  return AF_OK;
+}
+
+// Description of this filter
+af_info_t af_info_sinesupress = {
+    "Sine Supress",
+    "sinesupress",
+    "Michael Niedermayer",
+    "",
+    0,
+    open
+};
diff --git a/libaf/control.h b/libaf/control.h
index dcbb2ddba0..330d9ab155 100644
--- a/libaf/control.h
+++ b/libaf/control.h
@@ -224,4 +224,9 @@ typedef struct af_control_ext_s{
 // Channel number which to inster the filtered data, arg in int*
 #define AF_CONTROL_CENTER_CH		0x00002200 | AF_CONTROL_FILTER_SPECIFIC
 
+
+// SineSupress
+#define AF_CONTROL_SS_FREQ		0x00002300 | AF_CONTROL_FILTER_SPECIFIC
+#define AF_CONTROL_SS_DECAY		0x00002400 | AF_CONTROL_FILTER_SPECIFIC
+
 #endif /*__af_control_h */