shithub: soundpipe

ref: 25a9fcc24ad00c6a406042818b9e6f2e7bba0dd1
dir: soundpipe/modules/diode.c

/*
 * Diode
 *
 * Based on Will Pirkle's Diode Ladder filter design.
 * Code adapted from the CCRMA Chugin WPDiodeLadder
 *
 */

#include <stdlib.h>
#include <math.h>
#include "soundpipe.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

int sp_diode_create(sp_diode **p)
{
    *p = malloc(sizeof(sp_diode));
    return SP_OK;
}

int sp_diode_destroy(sp_diode **p)
{
    free(*p);
    return SP_OK;
}

static SPFLOAT sp_diode_opva_fdbk_out(sp_data *sp, sp_diode *p, int filt)
{
    USED(sp);

    return p->opva_beta[filt] *
        (p->opva_z1[filt] + p->opva_fdbk[filt] * p->opva_delta[filt]);
}

static SPFLOAT sp_diode_opva_compute(sp_data *sp, sp_diode *p, SPFLOAT in, int filt)
{
    /*
	double x_in = (xn*m_dGamma + m_dFeedback + m_dEpsilon*getFeedbackOutput());

	double vn = (m_da0*x_in - m_dZ1)*m_dAlpha;
	double out = vn + m_dZ1;
	m_dZ1 = vn + out;
    */

	/* m_dBeta*(m_dZ1 + m_dFeedback*m_dDelta); */

	SPFLOAT x_in = (in*p->opva_gamma[filt]
        + p->opva_fdbk[filt]
        + p->opva_eps[filt] * sp_diode_opva_fdbk_out(sp, p, filt));
	SPFLOAT vn = (p->opva_a0[filt]*x_in -
        p->opva_z1[filt])*p->opva_alpha[filt];
	SPFLOAT out = vn + p->opva_z1[filt];
	p->opva_z1[filt] = vn + out;
    return out;
}

static void sp_diode_update(sp_data *sp, sp_diode *p)
{
	/* calculate alphas */
	SPFLOAT G1, G2, G3, G4;
	SPFLOAT wd = 2*M_PI*p->freq;
	SPFLOAT T  = 1/(SPFLOAT)sp->sr;
	SPFLOAT wa = (2/T)*tan(wd*T/2);
	SPFLOAT g = wa*T/2;
    int i;

    /* Big G's */
	G4 = 0.5*g/(1.0 + g);
	G3 = 0.5*g/(1.0 + g - 0.5*g*G4);
	G2 = 0.5*g/(1.0 + g - 0.5*g*G3);
	G1 = g/(1.0 + g - g*G2);

    /* big G value gamma */
    p->gamma  = G4*G3*G2*G1;

    p->SG[0] =  G4*G3*G2;
	p->SG[1] =  G4*G3;
	p->SG[2] =  G4;
	p->SG[3] =  1.0;

	/* set alphas */
    for (i = 0; i < 4; i++) p->opva_alpha[i] = g/(1.0 + g);

    /* set betas */
    p->opva_beta[0] = 1.0/(1.0 + g - g*G2);
	p->opva_beta[1] = 1.0/(1.0 + g - 0.5*g*G3);
	p->opva_beta[2] = 1.0/(1.0 + g - 0.5*g*G4);
	p->opva_beta[3] = 1.0/(1.0 + g);

	/* set gammas */
	p->opva_gamma[0] = 1.0 + G1*G2;
	p->opva_gamma[1] = 1.0 + G2*G3;
	p->opva_gamma[2] = 1.0 + G3*G4;

    /* set deltas */
	p->opva_delta[0] = g;
	p->opva_delta[1] = 0.5*g;
	p->opva_delta[2] = 0.5*g;

    /* set epsilons */
	p->opva_eps[0] = G2;
	p->opva_eps[1] = G3;
	p->opva_eps[2] = G4;
}

int sp_diode_init(sp_data *sp, sp_diode *p)
{
    int i;
    /* initialize the 4 one-pole VA filters */

    for (i = 0; i < 4; i++) {
        p->opva_alpha[i] = 1.0;
        p->opva_beta[i] = -1.0;
        p->opva_gamma[i] = 1.0;
        p->opva_delta[i] = 0.0;
        p->opva_eps[i] = 1.0;
        p->opva_fdbk[i] = 0.0;
        p->opva_a0[i] = 1.0;
        p->opva_z1[i] = 0.0;

        p->SG[i] = 0.0;
    }

	p->gamma = 0.0;
	p->K = 0.0;

	/* Filter coeffs that are constant */
	/* set a0s */
    p->opva_a0[0] = 1.0;
    p->opva_a0[1] = 0.5;
    p->opva_a0[2] = 0.5;
    p->opva_a0[3] = 0.5;

	/* last LPF has no feedback path */
    p->opva_gamma[3] = 1.0;
    p->opva_delta[3] = 0.0;
    p->opva_eps[3] = 0.0;
    p->opva_fdbk[3] = 0.0;

    /* default cutoff to 1000hz */
    p->freq = 1000;
    p->res = 0;
    /* update filter coefs */

    sp_diode_update(sp, p);
    return SP_OK;
}

int sp_diode_compute(sp_data *sp, sp_diode *p, SPFLOAT *in, SPFLOAT *out)
{
    int i;
    SPFLOAT sigma;
    SPFLOAT un;
    SPFLOAT tmp;

    /* update filter coefficients */
    p->K = p->res * 17;
    sp_diode_update(sp, p);

    p->opva_fdbk[2] = sp_diode_opva_fdbk_out(sp, p, 3);
    p->opva_fdbk[1] = sp_diode_opva_fdbk_out(sp, p, 2);
    p->opva_fdbk[0] = sp_diode_opva_fdbk_out(sp, p, 1);

    sigma =
        p->SG[0] * sp_diode_opva_fdbk_out(sp, p, 0) +
        p->SG[1] * sp_diode_opva_fdbk_out(sp, p, 1) +
        p->SG[2] * sp_diode_opva_fdbk_out(sp, p, 2) +
        p->SG[3] * sp_diode_opva_fdbk_out(sp, p, 3);

    un = (*in - p->K * sigma) / (1 + p->K * p->gamma);
    tmp = un;
    for (i = 0; i < 4; i++) {
        tmp = sp_diode_opva_compute(sp, p, tmp, i);
    }
    *out = tmp;
    return SP_OK;
}