File content as of revision 1:8ae4ab73ca6a:

/*
Copyright 2013 Paul Soulsby www.soulsbysynths.com
    This file is part of Atmegatron.

    Atmegatron is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Atmegatron is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with Atmegatron.  If not, see <http://www.gnu.org/licenses/>.
*/

//***15 Biquad filter algorithms***

#include "atmegatron.h"

const float pow_10_025 = 1.7782794100389228012254211951927; // pow(10,0.25)
const float pow_10_075 = 5.6234132519034908039495103977648; // pow(10,0.75)
const float pow_10_250 = 316.22776601683793319988935444327; // pow(10,2.5)

//lets and gets
byte filt_fc = 255;              //filter cutoff - not the actual Fc, just a way to store knob position as byte (0-255)               
byte filt_q = 0;                 //filter resonance - not the actual Q, just a way to store knob position as byte (0-255)
byte filt_type = 1;              //filter type
boolean filt_gainadj = false;    //filter normalise mode (called gain adjust in code)
byte filt_fenvamt = 0;           //filter env amount
byte filt_lfoamt = 0;            //filter lfo amount

//local vars
byte filt_lfolookup[256];        //look up table to convert lfo output to Fc multiplier.  Lookup saves having to do v slow exp calculation every cycle
byte filt_fenvlookup[256];       //look up table to convert env output to Fc multiplier.  Lookup saves having to do v slow exp calculation every cycle
float filt_fenvamtf = 0;         //filter env amount as float (0-1)
float filt_lfoamtf = 0;          //filter lfo amount as float (0-1)
float filt_fc_calc = 0;          //final Fc for use in biquad equation (after mult with env, lfo etc)
float filt_q_calc = 1;           //final Q for use in biquad equation (after mult with env, lfo etc)
float two_pi_over_sf= 0;         //2pi / sample freq (ie interrupt freq)
float pi_over_sf= 0;             //pi / sample freq 

// filter constants - these are the constants used by biquad equation.  Whenever Fc, Q or filter type changes, they need recalculating
float a0=1;
float a1;
float a2;
float b0;
float b1;
float b2;
float A = 1.7782794100389228012254211951927; //pow(10, 0.25);


// lets and gets
//Filter cutoff frequency knob position (0-255)
void Filt_Let_Fc(byte newfc)
{
    filt_fc = newfc;
}
byte Filt_Get_Fc(void)
{
  return filt_fc;
}
//Filter resonance (Q) knob position (0-255)
void Filt_Let_Q(byte newq)
{
  filt_q = newq;
}
byte Filt_Get_Q(void)
{
  return filt_q;
}
//Filter type (0-15)
void Filt_Let_Type(byte newtype){
  if (newtype!=filt_type){        //set new filter type
    filt_type = newtype;
    switch (filt_type){           //calculate var A for shelf and peak filters: A  = sqrt( 10^(dBgain/20) )
        case FILT_PEAK10:
            //A = pow(10, 0.25);
            A = pow_10_025;
            break;
        case FILT_PEAK30:
            //A = pow(10, 0.75);
            A = pow_10_075;
            break;
        case FILT_PEAK100:
            //A = pow(10, 2.5);
            A = pow_10_250;
            break; 
        case FILT_LS10:
            //A = pow(10, 0.25);
            A = pow_10_025;
            break;
        case FILT_LS30:
            //A = pow(10, 0.75);
            A = pow_10_075;
            break;
        case FILT_HS10:
            //A = pow(10, 0.25);
            A = pow_10_025;
            break;
        case FILT_HS30:
            //A = pow(10, 0.75);
            A = pow_10_075;
            break;      
    }
  }
}
byte Filt_Get_Type(void)
{
  return filt_type;
}
//set filter normalise mode (gainadj in code)
void Filt_Let_GainAdj(boolean newadj){
  filt_gainadj = newadj;
}
boolean Filt_Get_GainAdj(void)
{
  return filt_gainadj;
}

// Filter meat
void Filt_CalcVals(void)
{
  unsigned long max_fc;                          //maximum cutoff frequency.  Must be < Sf/2.  Will ring at Sf/2.
  unsigned long f;                               //intermediate value for fict_fc_calc (before converting to float)

  if (filt_type>0){                              //if filter is on
    max_fc = Master_Get_SampleFreq() >> 1UL;     //calculate max cutoff frequency.  Sf / 2
    max_fc -= Master_Get_SampleFreq() >> 6UL;    //minus a bit more for safety.  reduce maxfc by proportion of Fs (about 1.5%)
    
    f = max_fc;                                  //start with Fc at maximum
    f = f * (filt_fc + 1) >> 8UL;                //then scale by filt cutoff knob position (use >>8 so knob=255 = max_fc)
  
    if (filt_lfoamt>0){                          //if filt lfo knob > 0
      f = f * (Filt_Get_LFOGain() + 1) >> FILT_BS;   //mult fc by lfo gain and bit shift.  This is the fixed point maths way of mult fc by lfo (i.e. not using floating point)
    }

    if (filt_fenvamt>0){
      f = f * (Filt_Get_FenvGain() + 1) >> FILT_BS;   //mult fc by env gain and bit shift.  This is the fixed point maths way of mult fc by env (i.e. not using floating point)
    }
    if (f > max_fc){                              //sort out if f > max_fc
      f = max_fc;         
    }
    
    filt_fc_calc = (float)f;                      //convert to float ready for biquad calculation
    filt_q_calc = (float)filt_q * MULTQ + MINQ;   //calculate q based on knob position. 0.5-20
    pi_over_sf = PI/Master_Get_SampleFreq();      //used in biquad equations
    two_pi_over_sf = 2 * pi_over_sf;              //used in biquad equations
    
    switch (filt_type){                           //calulate biquad values
        case FILT_OFF:
          break;
        case FILT_LPF:
            LPValCalculator();  
            break;
        case FILT_HPF:
            HPValCalculator();  
            break;
        case FILT_BPF:
            BPSkirtValCalculator();
            break;
        case FILT_NOTCH:
            NotchValCalculator();  
            break;
        case FILT_PEAK10:
            PeakingEQValCalculator();    
            break;
        case FILT_PEAK30:
            PeakingEQValCalculator();    
            break;
        case FILT_PEAK100:
            PeakingEQValCalculator();    
            break; 
        case FILT_LS10:
            LowShelfValCalculator();  
            break;
        case FILT_LS30:
            LowShelfValCalculator();   
            break;
        case FILT_HS10:
            HighShelfValCalculator();
            break;
        case FILT_HS30:
            HighShelfValCalculator();
            break;
        case FILT_BUTLPF:
            ButterworthLPCalculator();  
            break;
        case FILT_BUTHPF:
            ButterworthHPCalculator();  
            break;
        case FILT_BESLPF:
            BesselLPCalculator();  
            break;
        case FILT_BESHPF:
            BesselHPCalculator();  
            break;        
    }
    
    //This divide would normally be done in the Biquad_Process. However doing the time consuming divide here means that there's only 5 divides, rather than 32 (in Biquad_Process)
    
    b0 /= a0;
    b1 /= a0;
    b2 /= a0;
    a1 /= a0;
    a2 /= a0;

  }
}

// Calculate biquad filter constants for filter
void LPValCalculator(void)
{
    float w = two_pi_over_sf * filt_fc_calc;
    float alpha = sin(w) / filt_q_calc * 0.5;
    float cs = cos(w);
    b0 = (1 - cs) / 2;
    b1 = 1 - cs;
    b2 = b0;
    a0 = 1 + alpha;
    a1 = -2 * cs;
    a2 = 1 - alpha;
}

void HPValCalculator(void)
{
    float w = two_pi_over_sf * filt_fc_calc;
    float alpha = sin(w) / filt_q_calc * 0.5;
    float cs = cos(w);
    b0 = (1 + cs) / 2;
    b1 = -(1 + cs);
    b2 = b0;
    a0 = 1 + alpha;
    a1 = -2 * cs;
    a2 = 1 - alpha;
}

void BPSkirtValCalculator(void)
{
    float w = two_pi_over_sf * filt_fc_calc;
    float alpha = sin(w) / filt_q_calc * 0.5;
    float sn = sin(w);
    float cs = cos(w);
    b0 =   sn/2;
    b1 =   0;
    b2 =  -b0;
    a0 =   1 + alpha;
    a1 =  -2*cs;
    a2 =   1 - alpha;
}
void NotchValCalculator(void)
{
    float w = two_pi_over_sf * filt_fc_calc;
    float alpha = sin(w) / filt_q_calc * 0.5;  
    float cs = cos(w);
    b0 =   1;
    b1 =  -2*cs;
    b2 =   1;
    a0 =   1 + alpha;
    a1 =   b1;
    a2 =   1 - alpha;
}

void PeakingEQValCalculator(void)
{
    float w = two_pi_over_sf * filt_fc_calc;
    float alpha = sin(w) / filt_q_calc * 0.5;
    float cs = cos(w);
    b0 =   1 + alpha*A;
    b1 =  -2*cs;
    b2 =   1 - alpha*A;
    a0 =   1 + alpha/A;
    a1 =   b1;
    a2 =   1 - alpha/A;
}
void LowShelfValCalculator(void)
{
    float w = two_pi_over_sf * filt_fc_calc;
    float cs = cos(w);
    float srA = sqrt(A);
    float alpha = sin(w) / filt_q_calc * 0.5;
    float amcs = (A-1)*cs;
    float apcs = (A+1)*cs;
    float sraaplpha = 2*srA*alpha;
    b0 =    A*( (A+1) - amcs + sraaplpha );
    b1 =  2*A*( (A-1) - apcs             );
    b2 =    A*( (A+1) - amcs - sraaplpha );
    a0 =        (A+1) + amcs + sraaplpha  ;
    a1 =   -2*( (A-1) + apcs             );
    a2 =        (A+1) + amcs - sraaplpha  ;
}
void HighShelfValCalculator(void)
{
    float w = two_pi_over_sf * filt_fc_calc;
    float cs = cos(w);
    float srA = sqrt(A);
    float alpha = sin(w) / filt_q_calc * 0.5;
    float amcs = (A-1)*cs;
    float apcs = (A+1)*cs;
    float sraaplpha = 2*srA*alpha;
    b0 =    A*( (A+1) + amcs + sraaplpha );
    b1 = -2*A*( (A-1) + apcs             );
    b2 =    A*( (A+1) + amcs - sraaplpha );
    a0 =        (A+1) - amcs + sraaplpha  ;
    a1 =    2*( (A-1) - apcs             );
    a2 =        (A+1) - amcs - sraaplpha  ;
}
void ButterworthLPCalculator(void)
{
    float k = tan(filt_fc_calc * pi_over_sf);
    b2 = k * k;
    b0 = b2;
    b1 = 2 * b0;
    a0 = b0 + (SQRT2 * k) + 1;
    a1 = 2 * (b0 - 1);
    a2 = b0 - (SQRT2 * k) + 1;
}
void ButterworthHPCalculator(void)
{
    float k = tan(filt_fc_calc * pi_over_sf);
    float k2p1 = k * k + 1;
    b0 = 1;
    b2 = b0;
    b1 = -2;
    a0 = k2p1 + (SQRT2 * k);
    a1 = 2 * (k2p1 - 2);
    a2 = k2p1 - (SQRT2 * k);
}
void BesselLPCalculator(void)
{
    float w = tan(pi_over_sf * filt_fc_calc);
    b2 = 3 * w * w;
    b0 = b2;
    b1 = 2 * b0;
    a0 = 1 + 3 * w + b0;
    a1 = -2 + b1;
    a2 = 1 - 3 * w + b0;
}
void BesselHPCalculator(void)
{
    float w = tan(pi_over_sf * filt_fc_calc);
    float w2 = w * w;
    b2 = 3;
    b0 = b2;
    b1 = -6;
    a0 = w2 + 3 * w + 3;
    a1 = 2 * w2 - 6;
    a2 = w2 - 3 * w + 3;
}

//Process the wavetable
void Filt_Process(void)
{
  byte i;
  sample_t out, maxout;
  float in, fout, multb, bout;
  
  if (filt_type>0){  
    maxout = 0;                                                        //used by filter normalise mode
    for (i=0;i<WAVE_LEN;i++){                                          //cycle through each sample
        if (filt_gainadj==true){                                       //if filter normalise mode on
          bout = Biquad_process((float)(Wave_Get_Process(i) >> 2));    //get sample and reduce amplitude.  (default 2 = amp / 4), then perform biquad process
        }
        else {    
          bout = Biquad_process((float)Wave_Get_Process(i));           //otherwise just get sample and perform biquad process
        }
        if (bout>127){                                                 //constrain biquad output to max/min char value (by clipping)
          out = 127;
        }
        else if (bout<-128){
          out = -128;
        }
        else{
          out = (sample_t)bout;
        }       
        Wave_Let_Process(i,out);                                       //write sample back to wavetable
        if (filt_gainadj==true){                                       //if filter normalise mode on
          out = abs(out);                                              //see if sample is greatest value in table and set maxout if it is
          if (out > maxout){
            maxout = out;
          }
        }
    }
    if (filt_gainadj==true){                                           //if filter normalise mode
      multb = 128 / (float)maxout;                                     //calculate multiplier to normalise waveform
      for (i=0;i<WAVE_LEN;i++){                                        //cycle through each sample                 
        in = (float)Wave_Get_Process(i);                               //get sample
        fout = in * multb;                                             //multiply by normalise multiplier
        if (fout>127){                                                 //clip sample if necessary
          out = 127;
        }
        else if (fout<-128){
          out = -128;
        }
        else{
          out = (sample_t)fout;
        }
        Wave_Let_Process(i,out);                                       //write back again
      }
    }
  }  
}
//this is the standard biquad filter equation.  normally would divide by a0, but this has already been done in filt_calcvals, to save the number of time consuming divisions required
float Biquad_process(float bi0)
{
  static float bi1;
  static float bi2;
  float bo0;
  static float bo1;
  static float bo2;
    bo0 = b0 * bi0 + b1 * bi1 + b2 * bi2 - a1 * bo1 - a2 * bo2;
    bi2 = bi1;
    bi1 = bi0;
    bo2 = bo1;
    bo1 = bo0;
    return bo0;
}

//Filter LFO amount.  This is stored as a byte representing knob position and a float (0-FILT_MAX)
void Filt_Let_LFOAmt(byte newamt)
{
  if (newamt!=filt_lfoamt){                                //if new value different to current
    filt_lfoamt = newamt;                                  //set new value
    filt_lfoamtf = (float)filt_lfoamt * FILT_MAX / 255;    //calculate new float value used to calculate LFO lookup table (0-FILT_MAX)
    memset(filt_lfolookup,0,sizeof(filt_lfolookup));       //clear the lookup table  (quicker than loop)
  }
}
byte Filt_Get_LFOAmt(void)
{
  return filt_lfoamt;
}
//Return the 'gain' of LFO.  This is stored in a lookup table to save time consuming calculations each time.  See the forums for further explanation of the 'gain' term
byte Filt_Get_LFOGain(void)
{
  byte index;
  float f, g;
  index = LFO_Get_Level() + 127;                          //get the current output level of LFO (-127 - 128) and add offset (can't lookup negative array indexes)
  if (filt_lfolookup[index]==0){                          //if index of lookup table hasn't yet been calculated:
      f = (float)LFO_Get_Level() / 127;                   //convert LFO output to float (0 - 1)
      g = round(exp(f * filt_lfoamtf) * FILT_MULT - 1);   //calculate lookup table.  e.g. lfoamt = 0: exp(0) * 64 - 1 = 63   e.g.  lfoamt = 1 and lfo level = 127: exp(ln(4)) * 64 - 1 = 255  e.g. lfoamt = 1 and lfo = -127: exp(-ln(4)) * 64 - 1 = 15  
      filt_lfolookup[index] = (byte)g;                    //write value to lookup table
  }
  return filt_lfolookup[index];                           //return lookuptable value
}
//Filter Env amount.   This is stored as a byte representing knob position and a float (0-FILT_MAX)
void Filt_Let_FenvAmt(byte newamt)
{
  if (newamt!=filt_fenvamt){                                //if new value different to current
    filt_fenvamt = newamt;                                  //set new value
    filt_fenvamtf = (float)filt_fenvamt * FILT_MAX / 255;   //calculate new float value used to calculate env lookup table (0-FILT_MAX)
    memset(filt_fenvlookup,0,sizeof(filt_fenvlookup));      //clear the lookup table  (quicker than loop)
  }
}
byte Filt_Get_FenvAmt(void)
{
  return filt_fenvamt;
}
//Return the 'gain' of env.  This is stored in a lookup table to save time consuming calculations each time.  See the forums for further explanation of the 'gain' term
byte Filt_Get_FenvGain(void)
{
  byte index;
  float f, g;
  index = Fenv_Get_Level() + 127;                          //get the current output level of env (-127 - 128) and add offset (can't lookup negative array indexes)
  if (filt_fenvlookup[index]==0){                          //if index of lookup table hasn't yet been calculated:
      f = (float)Fenv_Get_Level() / 127;                   //convert env output to float (0 - 1)
      g = round(exp(f * filt_fenvamtf) * FILT_MULT - 1);   //calculate lookup table.  e.g. envamt = 0: exp(0) * 64 - 1 = 63   e.g.  envamt = 1 and lfo level = 127: exp(ln(4)) * 64 - 1 = 255  e.g. envamt = 1 and env = -127: exp(-ln(4)) * 64 - 1 = 15  
      filt_fenvlookup[index] = (byte)g;                    //write value to lookup table
  }
  return filt_fenvlookup[index];                           //return lookuptable value
}