145 lines
4.4 KiB
C
145 lines
4.4 KiB
C
#include "IIR.h"
|
||
#include "arm_math.h"
|
||
#include "arm_const_structs.h"
|
||
|
||
//Log需要引用的头文件
|
||
#include "nrf_log.h"
|
||
#include "nrf_log_ctrl.h"
|
||
#include "nrf_log_default_backends.h"
|
||
|
||
#define bp_numStages 1 /* 2阶IIR滤波的个数 */
|
||
#define bs_numStages 2 /* 2阶IIR滤波的个数 */
|
||
|
||
#define BLOCK_SIZE 1 /* 调用一次arm_biquad_cascade_df1_f32处理的采样点个数 */
|
||
|
||
uint32_t blockSize = BLOCK_SIZE;
|
||
static float32_t IIRStateF32_bp[4*bp_numStages]; /* 带通滤波器状态缓存 */
|
||
static float32_t IIRStateF32_bs[4*bs_numStages]; /* 带阻滤波器状态缓存 */
|
||
|
||
arm_biquad_casd_df1_inst_f32 S_BS;
|
||
arm_biquad_casd_df1_inst_f32 S_BP;
|
||
float32_t bpScaleValue;
|
||
float32_t bsScaleValue;
|
||
|
||
/* Elliptic带通滤波器系数20Hz 500Hz*/
|
||
const float32_t IIRCoeffs32BP[5*bp_numStages] = {
|
||
1.0f, 0, -1.0f, 0.638776097661321373699649939226219430566f, 0.339488565819422882796629892254713922739f
|
||
};
|
||
/* 巴特沃斯带阻滤波器系数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)
|
||
{
|
||
/* 初始化 */
|
||
arm_biquad_cascade_df1_init_f32(&S_BS, bs_numStages, (float32_t *)&IIRCoeffs32BS[0], (float32_t *)&IIRStateF32_bs[0]);
|
||
/*放缩系数 */
|
||
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;
|
||
}
|
||
|
||
|
||
/*
|
||
*********************************************************************************************************
|
||
* 函 数 名: arm_iir_f32_bp
|
||
* 功能说明: 调用函数arm_iir_f32_hp实现带通滤波器
|
||
* 形 参：无
|
||
* 返 回 值: 无
|
||
*********************************************************************************************************
|
||
*/
|
||
void arm_iir_f32_bp(void)
|
||
{
|
||
/* 初始化 */
|
||
arm_biquad_cascade_df1_init_f32(&S_BP, bp_numStages, (float32_t *)&IIRCoeffs32BP[0], (float32_t *)&IIRStateF32_bp[0]);
|
||
|
||
/*放缩系数 */
|
||
bpScaleValue = 0.66974428290971144139831494612735696137f;
|
||
}
|
||
|
||
#define TEST_LENGTH_SAMPLES 400 /* 采样点数 */
|
||
//static float32_t testInput_f32_50Hz_200Hz[TEST_LENGTH_SAMPLES]; /* 采样点 */
|
||
|
||
//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正弦波 + 200Hz正弦波 */
|
||
singalOutput = arm_sin_f32(2 * 3.1415926f * 50 * i / sampleHz) +
|
||
arm_sin_f32(2 * 3.1415926f * 200 * i / sampleHz);
|
||
|
||
/* 归零逻辑：当i达到一个完整周期的采样点数时 */
|
||
if (++i >= (sampleHz / 50)) {
|
||
i = 0;
|
||
}
|
||
|
||
return singalOutput;
|
||
}
|
||
|
||
void FilterInit()
|
||
{
|
||
arm_iir_f32_bs(); // 带阻滤波器
|
||
arm_iir_f32_bp(); // 带通滤波器
|
||
|
||
////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) //处理挂起的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) //处理挂起的log
|
||
{
|
||
|
||
}
|
||
}
|
||
while(1)
|
||
{
|
||
NRF_LOG_PROCESS();
|
||
}
|
||
*/
|
||
|
||
//处理挂起的log
|
||
|
||
//////end
|
||
}
|