void MLProcBiquad::calcCoeffs(const int frames)
{
static MLSymbol modeSym("mode");
int mode = (int)getParam(modeSym);
const MLSignal& frequency = getInput(2);
const MLSignal& q = getInput(3);
int coeffFrames;
float twoPiOverSr = kMLTwoPi*getContextInvSampleRate();
bool paramSignalsAreConstant = frequency.isConstant() && q.isConstant();
if (paramSignalsAreConstant)
{
coeffFrames = 1;
}
else
{
coeffFrames = frames;
}
// set proper constant state for coefficient signals
mA0.setConstant(paramSignalsAreConstant);
mA1.setConstant(paramSignalsAreConstant);
mA2.setConstant(paramSignalsAreConstant);
mB1.setConstant(paramSignalsAreConstant);
mB2.setConstant(paramSignalsAreConstant);
float a0, a1, a2, b0, b1, b2;
float qm1, omega, alpha, sinOmega, cosOmega;
float highLimit = getContextSampleRate() * 0.33f;
// generate coefficient signals
// TODO SSE
for(int n=0; n<coeffFrames; ++n)
{
qm1 = 1.f/(q[n] + 0.05f);
omega = clamp(frequency[n], kLowFrequencyLimit, highLimit) * twoPiOverSr;
sinOmega = fsin1(omega);
cosOmega = fcos1(omega);
alpha = sinOmega * 0.5f * qm1;
b0 = 1.f/(1.f + alpha);
switch (mode)
{
default:
case kLowpass:
a0 = ((1.f - cosOmega) * 0.5f) * b0;
a1 = (1.f - cosOmega);
a2 = a0;
b1 = (-2.f * cosOmega);
b2 = (1.f - alpha);
break;
case kHighpass:
a0 = ((1.f + cosOmega) * 0.5f);
a1 = -(1.f + cosOmega);
a2 = a0;
b1 = (-2.f * cosOmega);
b2 = (1.f - alpha);
break;
case kBandpass:
a0 = alpha;
a1 = 0.f;
a2 = -alpha;
b1 = -2.f * cosOmega;
b2 = (1.f - alpha);
break;
case kNotch:
a0 = 1;
a1 = -2.f * cosOmega;
a2 = 1;
b1 = -2.f * cosOmega;
b2 = (1.f - alpha);
break;
}
mA0[n] = a0*b0;
mA1[n] = a1*b0;
mA2[n] = a2*b0;
mB1[n] = b1*b0;
mB2[n] = b2*b0;
}
}
void MLProcRate::process(const int samples)
{
const MLSignal& x = getInput(1);
const MLSignal& ratioSig = getInput(2);
MLSignal& y = getOutput(1);
const float isr = getContextInvSampleRate();
const float kFeedback = isr*10.f;
// allow ratio change once per buffer
float rIn = ratioSig[samples - 1];
if (rIn != mFloatRatio)
{
mCorrectedRatio = correctRatio(rIn);
mFloatRatio = rIn;
}
// if input phasor is off, reset and return.
if(x[0] < 0.f)
{
mOmega = 0.f;
y.fill(-0.0001f);
return;
}
if(mCorrectedRatio)
{
// run the PLL, correcting the output phasor to the input phasor and ratio.
float r = mCorrectedRatio.getFloat();
float rInv = 1.0f/r;
float numerator = mCorrectedRatio.top;
float numeratorInv = 1.0f/numerator;
float error;
for (int n=0; n<samples; ++n)
{
float px = x[n];
float dxdt = px - mx1;
mx1 = px;
float dydt = ml::max(dxdt*r, 0.f);
float dxy = px - mOmega*rInv;
// get modOffset, such that phase difference is 0 at each integer value of modOffset
float modOffset = dxy*numerator;
// get error term, valid at any phase difference.
error = roundf(modOffset) - modOffset;
// convert back to absolute phase difference
error *= numeratorInv;
// feedback = negative error * time constant
dydt -= kFeedback*error;
// don't ever run clock backwards.
dydt = ml::max(dydt, 0.f);
// wrap phasor
mOmega += dydt;
if(mOmega >= 1.0f)
{
mOmega -= 1.0f;
}
y[n] = mOmega;
}
}
else
{
// don't correct the phase, just run phasor at float ratio of input rate.
for (int n=0; n<samples; ++n)
{
float px = x[n];
float dxdt = px - mx1;
mx1 = px;
float dydt = ml::max(dxdt*mFloatRatio, 0.f);
// wrap phasor
mOmega += dydt;
if(mOmega >= 1.0f)
{
mOmega -= 1.0f;
}
y[n] = mOmega;
}
}
}
