File content as of revision 29:8ee84bda128c:

#include "signal_processing.h"

//---------------------------------------------------------------------------
// Reference
// stanford edu "CCRMA Digital State Variable Filters"
//  https://ccrma.stanford.edu/~jos/svf/
// musicdsp "State Variable"
//  http://musicdsp.org/archive.php?classid=3#23
//---------------------------------------------------------------------------

    // State Variable Filter
    // The f * f_factor must be < Fs/6.
SVFilter::SVFilter( uint32_t a_block_size ) : amakusa::AbstractFilter ( a_block_size )
{
    this->fc = 440;
    this->f_factor = 1.0f;
    this->set_Fs( SAMPLING_FREQUENCY );
    this->set_Q( 1.414f );
    this->set_mode( lpf );
    
    this->d1 = 0.0f;
    this->d2 = 0.0f;
}
 
/************************************************************************
*           +--------------------------------------> hp
*           |
*           |                 +--------------------> bp
*           |  f        D1    | f
* x(n)->(+)-+-|>-(+)->[z^-1]->+-|>->(+)--------+---> lp
*        A        |           |      |         |
*        |        +<----------+      +<-[z^-1]-+
*        |        -q          |          D2    |
*       (+)<------<|----------+                |
*        |                  -1                 |
*        +<-----------------<|-----------------+
*
*    bp = D1
*    lp = D1 * f + D2
*    hp = x - q * bp - lp
*
*    D1 = hp * f + D1
*    D2 = lp
*    
************************************************************************/    
void SVFilter::run( float32_t *pSrc, float32_t *pDst )
{
    float32_t bp, lp, hp;
    
    
    for ( int i= 0; i<this->block_size; i++ )
    {
            // calc the filter
        bp = this->d1;
        lp = bp * this->f + this->d2;
        hp = pSrc[i] - this->q * bp - lp;
        
            // update delay 
        this->d1 += hp * this->f;
        this->d2 = lp;
        
            // mode dependent output
        switch ( this->mode )
        {
        case lpf :
            pDst[i] = lp;
            break;
        case hpf :
            pDst[i] = hp;
            break;
        case bpf :
            pDst[i] = bp;
            break;
        };
    }
}

    // Q can be [0.1,inf]
void SVFilter::set_Q( float32_t Q )
{
    if ( Q < 0.1f )
        Q = 0.1f;
        
    this->q = 1.0f/Q;
}


void SVFilter::set_Fs( int new_Fs )              // Hz
{
    this->Fs = new_Fs;
    this->update_parameters();
}

    // fc is control frequency.
void SVFilter::set_fc( int new_fc )                // Hz
{
    this->fc = new_fc;
    this->update_parameters();
}

    // fc * f_factor must be less then Fs/6
void SVFilter::set_f_factor( float32_t new_f_factor )
{
    this->f_factor = new_f_factor;
    this->update_parameters();
}

void SVFilter::set_mode( svf_mode new_mode )
{
    this->mode = new_mode;
}


void SVFilter::update_parameters( void )
{
    this->f = 2.0f * sin( 3.141592f * this->fc * this->f_factor / this->Fs );
}