svfilter.cpp

00001 #include "signal_processing.h"
00002 
00003 //---------------------------------------------------------------------------
00004 // Reference
00005 // stanford edu "CCRMA Digital State Variable Filters"
00006 //  https://ccrma.stanford.edu/~jos/svf/
00007 // musicdsp "State Variable"
00008 //  http://musicdsp.org/archive.php?classid=3#23
00009 //---------------------------------------------------------------------------
00010 
00011     // State Variable Filter
00012     // The f * f_factor must be < Fs/6.
00013 SVFilter::SVFilter( uint32_t a_block_size ) : amakusa::AbstractFilter ( a_block_size )
00014 {
00015     this->fc = 440;
00016     this->f_factor = 1.0f;
00017     this->set_Fs( SAMPLING_FREQUENCY );
00018     this->set_Q( 1.414f );
00019     this->set_mode( lpf );
00020     
00021     this->d1 = 0.0f;
00022     this->d2 = 0.0f;
00023 }
00024  
00025 /************************************************************************
00026 *           +--------------------------------------> hp
00027 *           |
00028 *           |                 +--------------------> bp
00029 *           |  f        D1    | f
00030 * x(n)->(+)-+-|>-(+)->[z^-1]->+-|>->(+)--------+---> lp
00031 *        A        |           |      |         |
00032 *        |        +<----------+      +<-[z^-1]-+
00033 *        |        -q          |          D2    |
00034 *       (+)<------<|----------+                |
00035 *        |                  -1                 |
00036 *        +<-----------------<|-----------------+
00037 *
00038 *    bp = D1
00039 *    lp = D1 * f + D2
00040 *    hp = x - q * bp - lp
00041 *
00042 *    D1 = hp * f + D1
00043 *    D2 = lp
00044 *    
00045 ************************************************************************/    
00046 void SVFilter::run( float32_t *pSrc, float32_t *pDst )
00047 {
00048     float32_t bp, lp, hp;
00049     
00050     
00051     for ( int i= 0; i<this->block_size; i++ )
00052     {
00053             // calc the filter
00054         bp = this->d1;
00055         lp = bp * this->f + this->d2;
00056         hp = pSrc[i] - this->q * bp - lp;
00057         
00058             // update delay 
00059         this->d1 += hp * this->f;
00060         this->d2 = lp;
00061         
00062             // mode dependent output
00063         switch ( this->mode )
00064         {
00065         case lpf :
00066             pDst[i] = lp;
00067             break;
00068         case hpf :
00069             pDst[i] = hp;
00070             break;
00071         case bpf :
00072             pDst[i] = bp;
00073             break;
00074         };
00075     }
00076 }
00077 
00078     // Q can be [0.1,inf]
00079 void SVFilter::set_Q( float32_t Q )
00080 {
00081     if ( Q < 0.1f )
00082         Q = 0.1f;
00083         
00084     this->q = 1.0f/Q;
00085 }
00086 
00087 
00088 void SVFilter::set_Fs( int new_Fs )              // Hz
00089 {
00090     this->Fs = new_Fs;
00091     this->update_parameters();
00092 }
00093 
00094     // fc is control frequency.
00095 void SVFilter::set_fc( int new_fc )                // Hz
00096 {
00097     this->fc = new_fc;
00098     this->update_parameters();
00099 }
00100 
00101     // fc * f_factor must be less then Fs/6
00102 void SVFilter::set_f_factor( float32_t new_f_factor )
00103 {
00104     this->f_factor = new_f_factor;
00105     this->update_parameters();
00106 }
00107 
00108 void SVFilter::set_mode( svf_mode new_mode )
00109 {
00110     this->mode = new_mode;
00111 }
00112 
00113 
00114 void SVFilter::update_parameters( void )
00115 {
00116     this->f = 2.0f * sin( 3.141592f * this->fc * this->f_factor / this->Fs );
00117 }