Tau ReSpeaker Setup V01

Dependencies:   MbedJSONValue mbed

Fork of TAU_ReSpeaker_DSP_Test by Yossi_Students

filters.h

Committer:
Arkadi
Date:
2018-07-12
Revision:
5:ec6f2323a263
Parent:
4:59319802012b
Child:
6:e8b4ca41c691

File content as of revision 5:ec6f2323a263:


// Filter variables:
/*
butter FILTER DESIGN, 23-Nov-2016 09:14:52, 2 sections, for sampling frequency 744000.0 Hz
Filter Order 4, Cut Off Frequency 20000 Hz
*/

///////////////////////////////
//    filter variables:      //
///////////////////////////////

// filter coeficients, best performance to declare in function (constant values).
//float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate  // 50kHz cutoff at 1.4 Mhz sample rate
//    1.000000,  -2.000000,  1.000000,  1.000000,  -1.706510,  0.731145,
//    1.000000,  -2.000000,  1.000000,  1.000000,  -1.852377,  0.879117
//};
//float GscaleHP = 0.801724;
//
//// num sections
//int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6;

// second-order sections filter variables - upto 8 sections
#define MAX_SECTION_NUMBER 8

// convertions
#define ADC2Float (2.0f/4095.0f) // 0.0004884f//
#define Float2ADC (4095.0f/2.0f) // 2047.5f   //
//float ADC2Float=(2.0f/4095.0f); //ADCvalue*(2/0xFFF)-1.0f // 12 bits range
//float Float2ADC=(4095.0f/2.0f); //(ADCvalue+1.0f)*(0xFFF/2) // 12 bits range

///////////////////////////////
//    available filters:     //
///////////////////////////////
// off mode, output vdd/2
inline void offMode(void);
// passthrough function (output equals input)
inline void passthrough(void);
// highpass filter
inline void highpass(void);
// highpass filter + trigger mode
inline void highpassTrig(void);



///////////////////////////////
//    Filter Functions:      //
///////////////////////////////

// off mode, output vdd/2
inline void offMode(void)
{
    uint16_t ADCValueOut;
    // set to vdd/2
    ADCValueOut=(uint16_t)((0.0f +1.0f) * Float2ADC);
    // Output value using DAC
    *(__IO uint32_t *) Dac_Reg  = ADCValueOut;
}// end off mode

// passthrough function (output equals input)
inline void passthrough(void)
{
    uint32_t ADCValue;

    // Read ADC input
    ADCValue = hadc1.Instance->DR;
    // Output value using DAC
    *(__IO uint32_t *) Dac_Reg  = ADCValue;

} // end passthrough


// high pass filter
inline void highpass(void)
{
    ///////////////////////////////
    //    filter variables:      //
    ///////////////////////////////
    // filter coeficients best performance if defined in loop
    float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // closer to 30kHz when io toggle switched off.
        1.000000,  -2.000000,  1.000000,  1.000000,  -1.706510,  0.731145,
        1.000000,  -2.000000,  1.000000,  1.000000,  -1.852377,  0.879117
    };
    float GscaleHP = 0.801724;

    // num sections
    int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6;

    float ADCFloat;
    float ADCFloatFiltered;
    uint16_t ADCValueOut;

    // filter stages variables
    static float CurrInput [MAX_SECTION_NUMBER+1];
    static float LastInput [MAX_SECTION_NUMBER+1];
    static float LLastInput [MAX_SECTION_NUMBER+1];

    ////////////////////
    // Read ADC input //
    ////////////////////
    ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f;

    //////////////////////////////////////////////////////
    // Apply Filter to input                            //
    //////////////////////////////////////////////////////

    LLastInput[0] = LastInput[0];
    LastInput[0] = CurrInput[0];
    CurrInput[0] = ADCFloat;
    for (int ii=1; ii <= NumSectionsHP; ii++) {
        LLastInput[ii] = LastInput[ii];
        LastInput[ii] = CurrInput[ii];
        CurrInput[ii] = SOSMatHP[ii-1][0]*CurrInput[ii-1] + SOSMatHP[ii-1][1]*LastInput[ii-1] +
                        SOSMatHP[ii-1][2]*LLastInput[ii-1] -
                        SOSMatHP[ii-1][4]*LastInput[ii] - SOSMatHP[ii-1][5]*LLastInput[ii];
        ADCFloatFiltered = CurrInput[ii];
    }

    ADCFloatFiltered = ADCFloatFiltered * GscaleHP;

    ////////////////////////////
    // Apply Filter to Output //
    ////////////////////////////

    if (ADCFloatFiltered < -1.0f) {
        ADCValueOut=0; // Min value
    } else if (ADCFloatFiltered > 1.0f) {
        ADCValueOut=0xFFF; // Max value
    } else {
        ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC);
    }

    // Output value using DAC
    // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut);
    *(__IO uint32_t *) Dac_Reg = ADCValueOut;

} // end high pass filter

// highpass filter + trigger mode
inline void highpassTrig(void)
{
    ///////////////////////////////
    //    filter variables:      //
    ///////////////////////////////
    // filter coeficients best performance if defined in loop
    float SOSMatHP[2][6] = { // 25khz cutoff at 920 kHz sample rate // closer to 30kHz when io toggle switched off.
        1.000000,  -2.000000,  1.000000,  1.000000,  -1.706510,  0.731145,
        1.000000,  -2.000000,  1.000000,  1.000000,  -1.852377,  0.879117
    };
    float GscaleHP = 0.801724;

    // num sections
    int NumSectionsHP = sizeof(SOSMatHP)/sizeof(float)/6;

    float ADCFloat;
    float ADCFloatFiltered;
    uint16_t ADCValueOut;

    // filter stages variables
    static float CurrInput [MAX_SECTION_NUMBER+1];
    static float LastInput [MAX_SECTION_NUMBER+1];
    static float LLastInput [MAX_SECTION_NUMBER+1];

    // trigger variables
    static bool trigFlag=0; // flag to indicate trigger event

    ////////////////////
    // Read ADC input //
    ////////////////////
    ADCFloat=((uint16_t)(hadc1.Instance->DR) * ADC2Float)-1.0f;

    //////////////////////////////////////////////////////
    // Apply Filter to input                            //
    //////////////////////////////////////////////////////

    LLastInput[0] = LastInput[0];
    LastInput[0] = CurrInput[0];
    CurrInput[0] = ADCFloat;
    for (int ii=1; ii <= NumSectionsHP; ii++) {
        LLastInput[ii] = LastInput[ii];
        LastInput[ii] = CurrInput[ii];
        CurrInput[ii] = SOSMatHP[ii-1][0]*CurrInput[ii-1] + SOSMatHP[ii-1][1]*LastInput[ii-1] +
                        SOSMatHP[ii-1][2]*LLastInput[ii-1] -
                        SOSMatHP[ii-1][4]*LastInput[ii] - SOSMatHP[ii-1][5]*LLastInput[ii];
        ADCFloatFiltered = CurrInput[ii];
    }

    ADCFloatFiltered = ADCFloatFiltered * GscaleHP;

    ///////////////////////////////////////////////
    // Event detection                           //
    ///////////////////////////////////////////////
    if (trigFlag) { // event already detected
        trigDelay--;
        if (trigDelay == 0) { // pulse pass run out, perform delay and reset variables
            trigDelay = trigDelaySet; //reset counter for next iteration
            trigFlag=0;
            // reset filter
            for (int ii=1; ii <= NumSectionsHP; ii++) {
                LLastInput[ii] = 0;
                LastInput[ii] = 0;
                CurrInput[ii] = 0;
                ADCFloatFiltered = 0;
            }
            // update dac
            ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC);
            *(__IO uint32_t *) Dac_Reg = ADCValueOut;
            // delay for set time
            wait_us(trigPause);
        }
    } else if (ADCFloatFiltered >= trigTresh) {
        trigFlag=1;
    }
    ////////////////////////////
    // Apply Filter to Output //
    ////////////////////////////

    if (ADCFloatFiltered < -1.0f) {
        ADCValueOut=0; // Min value
    } else if (ADCFloatFiltered > 1.0f) {
        ADCValueOut=0xFFF; // Max value
    } else {
        ADCValueOut=(uint16_t)((ADCFloatFiltered +1.0f) * Float2ADC);
    }

    // Output value using DAC
    // HAL_DAC_SetValue(&hdac1, DAC_CHANNEL_1, DAC_ALIGN_12B_R, ADCValueOut);
    *(__IO uint32_t *) Dac_Reg = ADCValueOut/2;
    //wait_us(1);
} // end high pass filter + trigger mode