HL-PDJ-1/DSP_LIB/IIR.c

#include "IIR.h"
#include "arm_math.h"
#include "arm_const_structs.h"

//Log<6F><67>Ҫ<EFBFBD><D2AA><EFBFBD>õ<EFBFBD>ͷ<EFBFBD>ļ<EFBFBD>
#include "nrf_log.h"
#include "nrf_log_ctrl.h"
#include "nrf_log_default_backends.h"

#define bp_numStages  1                /* 2<><32>IIR<49>˲<EFBFBD><CBB2>ĸ<EFBFBD><C4B8><EFBFBD> */
#define bs_numStages  2                /* 2<><32>IIR<49>˲<EFBFBD><CBB2>ĸ<EFBFBD><C4B8><EFBFBD> */

#define BLOCK_SIZE           1    	 /* <20><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>arm_biquad_cascade_df1_f32<33><32><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */

uint32_t blockSize = BLOCK_SIZE;
static float32_t IIRStateF32_bp[4*bp_numStages];                      /* <20><>ͨ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD> */
static float32_t IIRStateF32_bs[4*bs_numStages];                      /* <20><><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD>״̬<D7B4><CCAC><EFBFBD><EFBFBD> */

arm_biquad_casd_df1_inst_f32 S_BS;
arm_biquad_casd_df1_inst_f32 S_BP;
float32_t bpScaleValue;
float32_t bsScaleValue;

/* Elliptic<69><63>ͨ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD>ϵ<EFBFBD><CFB5>20Hz 500Hz*/                                                                                                                                         
const float32_t IIRCoeffs32BP[5*bp_numStages] = {
    1.0f,  0,  -1.0f,  0.638776097661321373699649939226219430566f,  0.339488565819422882796629892254713922739f
};  
/* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˹<EFBFBD><CBB9><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD>ϵ<EFBFBD><CFB5>48Hz 52Hz*/                                                                                                                                           
const float32_t IIRCoeffs32BS[5*bs_numStages] = {
    1.0f,  -1.975854434025588135526163569011259824038f,  1.0f,  1.937393708057424390744927222840487957001f,  -0.966392678521233783328625577269122004509f,                    
    1.0f,  -1.975854434025588135526163569011259824038f,  1.0f,  1.952884644070242314839447317353915423155f,  -0.972373462682551759073135144717525690794f                    

}; 

void arm_iir_f32_bs(void)
{
	/* <20><>ʼ<EFBFBD><CABC> */
	arm_biquad_cascade_df1_init_f32(&S_BS, bs_numStages, (float32_t *)&IIRCoeffs32BS[0], (float32_t *)&IIRStateF32_bs[0]);      
	/*<2A><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5> */
	bsScaleValue = 0.984570131944842996674083224206697195768f*0.984570131944842996674083224206697195768f; 
}

void bs_bp(float32_t *bpinputF32_IN,float32_t *bsoutputF32_IN)
{
	float32_t bpoutputF32_IN __attribute__((aligned(4)));

	arm_biquad_cascade_df1_f32(&S_BP, bpinputF32_IN,  &bpoutputF32_IN,  blockSize);
	bpoutputF32_IN = (bpoutputF32_IN)*bpScaleValue;
	
	arm_biquad_cascade_df1_f32(&S_BS, &bpoutputF32_IN,  bsoutputF32_IN,  blockSize);
	*bsoutputF32_IN = (*bsoutputF32_IN)*bsScaleValue;	
}


/*
*********************************************************************************************************
*	<EFBFBD><EFBFBD> <EFBFBD><EFBFBD> <EFBFBD><EFBFBD>: arm_iir_f32_bp
*	<EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><EFBFBD>: <EFBFBD><EFBFBD><EFBFBD>ú<EFBFBD><EFBFBD><EFBFBD>arm_iir_f32_hpʵ<EFBFBD>ִ<EFBFBD>ͨ<EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD>
*	<EFBFBD><EFBFBD>    <EFBFBD>Σ<EFBFBD><EFBFBD><EFBFBD>
*	<EFBFBD><EFBFBD> <EFBFBD><EFBFBD> ֵ: <EFBFBD><EFBFBD>
*********************************************************************************************************
*/
void arm_iir_f32_bp(void)
{
	/* <20><>ʼ<EFBFBD><CABC> */
	arm_biquad_cascade_df1_init_f32(&S_BP, bp_numStages, (float32_t *)&IIRCoeffs32BP[0], (float32_t *)&IIRStateF32_bp[0]);
	        
	/*<2A><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5> */
	bpScaleValue = 0.66974428290971144139831494612735696137f;
}

#define TEST_LENGTH_SAMPLES  400    /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
//static float32_t testInput_f32_50Hz_200Hz[TEST_LENGTH_SAMPLES]; /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */

//static float32_t testOutput_f32_50Hz_200Hz[TEST_LENGTH_SAMPLES];

float math_caculate_signal(uint16_t sampleHz) 
{
    static uint16_t i = 0;
    float singalOutput;
    
    /* 50Hz<48><7A><EFBFBD>Ҳ<EFBFBD> + 200Hz<48><7A><EFBFBD>Ҳ<EFBFBD> */
    singalOutput = arm_sin_f32(2 * 3.1415926f * 50 * i / sampleHz) + 
                   arm_sin_f32(2 * 3.1415926f * 200 * i / sampleHz);
    
    /* <20><><EFBFBD><EFBFBD><EFBFBD>߼<EFBFBD><DFBC><EFBFBD><EFBFBD><EFBFBD>i<EFBFBD>ﵽһ<EFB5BD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵIJ<DAB5><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ */
    if (++i >= (sampleHz / 50)) {
        i = 0;
    }
    
    return singalOutput;
}

void FilterInit()
{
	arm_iir_f32_bs();   // <20><><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD>
	arm_iir_f32_bp();   // <20><>ͨ<EFBFBD>˲<EFBFBD><CBB2><EFBFBD>
	
	////test code
	
//	CreateData();
//	
//	for (int i = 0; i < TEST_LENGTH_SAMPLES; i++)
//	{
//		arm_biquad_cascade_df1_f32(&S_BS, &testInput_f32_50Hz_200Hz[i],  &testOutput_f32_50Hz_200Hz[i],  1);
//	}
	/*
	NRF_LOG_INFO("raw\r\n");
	char stf[10];
	for (int i = 0; i < TEST_LENGTH_SAMPLES; i++)
	{
		sprintf(stf,"%0.3f", testInput_f32_50Hz_200Hz[i]);
		
		NRF_LOG_INFO("%s",stf);
		if (NRF_LOG_PROCESS() == false) 	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>log
		{
		
		}
	}
	

	NRF_LOG_INFO("proccess\r\n");
	NRF_LOG_PROCESS();
	arm_biquad_cascade_df1_f32(&S_BS, testInput_f32_50Hz_200Hz,  testOutput_f32_50Hz_200Hz,  TEST_LENGTH_SAMPLES);
	for (int i = 0; i < TEST_LENGTH_SAMPLES; i++)
	{
		sprintf(stf,"%0.3f", testOutput_f32_50Hz_200Hz[i]);
		
		NRF_LOG_INFO("%s",stf);
		if (NRF_LOG_PROCESS() == false) 	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>log
		{
		
		}
	}
	while(1)
	{
		NRF_LOG_PROCESS();
	}
	*/
	
	//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>log

	//////end
}