Chebyshev filters

source/engine/dsp.c

@@ -4,6 +4,7 @@
 #include "limits.h"
 #include "math.h"
 #include "stdlib.h"
+#include "iir.h"
 
 #define PI 3.14159265
 
@@ -193,19 +194,225 @@ void osc_fillbuf(struct osc *osc, short *buf, int n)
 {
     for (int i = 0; i < n; i++) {
         short val = SHRT_MAX * osc->f(phasor_step(&osc->p));
-        buf[i*2] = buf[i*2+1] = val;
+        buf[i*CHANNELS] = buf[i*CHANNELS+1] = val;
     }
 }
 
-void gen_whitenoise(void *data, short *buf, int n)
+void gen_whitenoise(void *data, short *out, int n)
 {
     for (int i = 0; i < n; i++) {
-        for (int j = 0; j < 2; j++) {
-            buf[i*2+j] = (rand()>>15) - USHRT_MAX;
+        for (int j = 0; j < CHANNELS; j++) {
+            out[i*CHANNELS+j] = (rand()>>15) - USHRT_MAX;
         }
     }
 }
 
+void gen_pinknoise(void *data, short *out, int n)
+{
+    gen_whitenoise(NULL, out, n);
+
+    double b[2][7] = {0};
+    double ccof[6] = {0.99886, 0.99332, 0.96900, 0.8550, 0.55000, -0.76160};
+    double dcof[6] = {0.0555179, 0.0750759, 0.1538520, 0.3104856, 0.5329522, 0.0168960};
+
+    for (int i = 0; i < n; i++) {
+        for (int j = 0; j < CHANNELS; j++) {
+            double pink;
+            double white = (double)out[i*CHANNELS+j]/SHRT_MAX;
+
+            for (int k = 0; k < 5; k++) {
+                b[j][k] = ccof[k]*b[j][k] + white * dcof[k];
+                pink += b[j][k];
+            }
+
+            pink += b[j][5] + white*0.5362;
+            b[j][5] = white*0.115926;
+
+            out[i*CHANNELS+j] = pink * SHRT_MAX;
+        }
+    }
+    /*
+      *  The above is a loopified version of this
+      *  https://www.firstpr.com.au/dsp/pink-noise/
+    b0 = 0.99886 * b0 + white * 0.0555179;
+    b1 = 0.99332 * b1 + white * 0.0750759;
+    b2 = 0.96900 * b2 + white * 0.1538520;
+    b3 = 0.86650 * b3 + white * 0.3104856;
+    b4 = 0.55000 * b4 + white * 0.5329522;
+    b5 = -0.7616 * b5 - white * 0.0168980;
+    pink = b0 + b1 + b2 + b3 + b4 + b5 + b6 + white * 0.5362;
+    b6 = white * 0.115926;
+    */
+}
+
+short iir_filter(struct dsp_iir *iir, short val)
+{
+    float a = 0.f;
+
+    iir->dx[0] = (float)val/SHRT_MAX;
+    for (int i = 0; i < iir->n; i++)
+        a += iir->ccof[i] * iir->dx[i];
+
+    for (int i = iir->n-1; i > 0; i--)
+        iir->dx[i] = iir->dx[i-1];
+
+
+    for (int i =0; i < iir->n; i++)
+        a -= iir->dcof[i] * iir->dy[i];
+
+    iir->dy[0] = a;
+
+    for (int i = iir->n-1; i > 0; i--)
+        iir->dy[i] = iir->dy[i-1];
+
+
+
+    return a * SHRT_MAX;
+}
+
+void dsp_iir_fillbuf(struct dsp_iir *iir, short *out, int n)
+{
+    dsp_run(iir->in, out, n);
+
+    for (int i = 0; i < n; i++) {
+        short v = iir_filter(iir, out[i*CHANNELS]);
+
+        for (int j = 0; j < CHANNELS; j++) {
+           out[i*CHANNELS+j] = v;
+        }
+    }
+}
+
+struct dsp_iir new_iir(int poles, float freq)
+{
+    struct dsp_iir new;
+    new.poles = poles;
+    new.freq = freq;
+    new.n = new.poles+1;
+    new.ccof = malloc(new.n*sizeof(float));
+    new.dcof = malloc(new.n*sizeof(float));
+    new.dy = malloc(new.n*sizeof(float));
+    new.dx = malloc(new.n*sizeof(float));
+    return new;
+}
+
+struct dsp_filter lpf_make(int poles, float freq)
+{
+    struct dsp_iir *new = malloc(sizeof(*new));
+    *new = new_iir(poles, freq);
+
+  double fcf = new->freq*2/SAMPLERATE;
+  double sf = sf_bwlp(poles, fcf);
+
+  printf("Making LPF filter, fcf: %f, coeffs: %i, scale %1.15lf\n", fcf, new->n, sf);
+
+  int *ccof = ccof_bwlp(new->poles);
+  double *dcof = dcof_bwlp(new->poles, fcf);
+
+  for (int i = 0; i < new->n; i++) {
+      new->ccof[i] = ccof[i] * sf;
+      new->dcof[i] = dcof[i];
+  }
+
+  new->dcof[0] = 0.f;
+
+  free(ccof);
+  free(dcof);
+
+  printf("LPF coefficients are:\n");
+
+  for (int i = 0; i < new->n; i++) {
+      printf("%f, %f\n", new->ccof[i], new->dcof[i]);
+  }
+
+  struct dsp_filter lpf;
+  lpf.data = new;
+  lpf.filter = dsp_iir_fillbuf;
+
+  return lpf;
+}
+
+struct dsp_filter hpf_make(int poles, float freq)
+{
+  struct dsp_iir *new = malloc(sizeof(*new));
+  *new = new_iir(poles, freq);
+
+  double fcf = new->freq*2/SAMPLERATE;
+  double sf = sf_bwhp(new->poles, fcf);
+
+  int *ccof = ccof_bwhp(new->poles);
+  double *dcof = dcof_bwhp(new->poles, fcf);
+
+  for (int i = 0; i < new->n; i++) {
+      new->ccof[i] = ccof[i] * sf;
+      new->dcof[i] = dcof[i];
+  }
+
+    for (int i = 0; i < new->n; i++) {
+      printf("%f, %f\n", new->ccof[i], new->dcof[i]);
+  }
+
+  free(ccof);
+  free(dcof);
+
+  struct dsp_filter hpf;
+  hpf.data = new;
+  hpf.filter = dsp_iir_fillbuf;
+
+  return hpf;
+}
+
+short fir_filter(struct dsp_fir *fir, short val)
+{
+    float ret = 0.f;
+    fir->dx[fir->head] = (float)val/SHRT_MAX;
+
+    for (int i = 0; i < fir->n; i++) {
+        ret += fir->cof[i] * fir->dx[fir->head--];
+
+        if (fir->head < 0) fir->head = fir->n-1;
+    }
+
+    return ret * SHRT_MAX;
+}
+
+void dsp_fir_fillbuf(struct dsp_fir *fir, short *out, int n)
+{
+    dsp_run(fir->in, out, n);
+
+    for (int i = 0; i < n; i++) {
+        short val = fir_filter(fir, out[i*CHANNELS]);
+       // printf("%hd\n", val);
+
+        for (int j = 0; j < CHANNELS; j++)
+            out[i*CHANNELS + j] = val*5;
+    }
+}
+
+struct dsp_filter lp_fir_make(float freq)
+{
+    struct dsp_fir fir;
+    fir.freq = freq;
+    fir.n = 9;
+    fir.head = 0;
+    double fcf = freq * 2 / SAMPLERATE;
+    fir.dx = calloc(sizeof(float),  fir.n);
+    fir.cof = fir_lp(fir.n, fcf);
+
+    struct dsp_filter new;
+    new.data = malloc(sizeof(fir));
+    *(struct dsp_fir*)(new.data) = fir;
+    new.filter = dsp_fir_fillbuf;
+
+    for (int i = 0; i < fir.n; i++) {
+        printf("%f\n", fir.cof[i]);
+    }
+
+    return new;
+}
+
+
+
 struct dsp_delay dsp_delay_make(unsigned int ms_delay)
 {
     struct dsp_delay new;
@@ -226,7 +433,7 @@ void dsp_delay_filbuf(struct dsp_delay *delay, short *buf, int n)
     static short cache[BUF_FRAMES*2];
     dsp_run(delay->in, cache, n);
 
-    for (int i = 0; i < n*2; i++) {
+    for (int i = 0; i < n*CHANNELS; i++) {
         cbuf_push(&delay->buf, cache[i] / 2);
         buf[i] = cache[i] + cbuf_shift(&delay->buf);
     }