实时（按帧）处理的低通滤波C语言实现

news/2024/9/18 20:24:14/ 标签: c语言

写在前面：

低通滤波采用一般的FIR滤波器，因为本次任务，允许的延迟较多，或者说前面损失的信号可以较多，因此，涉及一个很高阶的FIR滤波器，信号起始段的信号点可以不处理，以及，考虑延时，当前时刻向前推一个时刻（当前帧处理的最后一点的时刻）之后的点，当前帧也先不处理。

因为FIR也有延时，经过稍加设计后，这个延时和信号本身的延时差不多一致，这样能将性能和延时兼顾。

简单来说，信号采样率256，延时2秒，第2秒处理第0.5秒到1.5秒的数据，下一秒即第3秒，再处理1.5秒到2.5秒的数据。

设置滤波器长度为128。

对前2秒数据，计算第0.5秒到1.5秒数据，即第128点到384点，为消除滤波延时，滤波数据起始点设置在第128（数据起点）+64（滤波器半长）+ 1=193点，易知此点前有192点，远大于滤波器长的128点；滤波数据终点在第384+64=448点，也没取到最后的512点，即，首尾取点还有64点的盈余。

当然，按照上述分析，我们完全可以将滤波器长度设置为128*2，即256，此时则刚好将延时和数据点最大偏移用完，即滤波数据起点为第1点，终点为最后的512点。但是，滤波器越长，计算量越大。matlab看滤波路效果，128阶的够用了。因此就用128阶的。

下面直接上代码了：

C代码：

Lowpass.h

#pragma once
#include"stdint.h"extern void Lowpass_init();
extern uint8_t Lowpass(int16_t* data_frame, int16_t* data_frame_out, int16_t SampleRate);

Lowpass.c

#include"stdio.h"
#include"stdlib.h"
#include"string.h"
#include"stdint.h"
#include "assert.h" #define  POINT_FRAME  (256)  // (SAMPLERATE*T_FRAME*T_FRAME)typedef struct {float DataBuf[2 * POINT_FRAME];float DataAfFir[POINT_FRAME + (POINT_FRAME >> 1)];uint64_t FrameCnt;  // 
}LowpassPra_t;
LowpassPra_t  LowpassPra = { 0 };// FIR 45Hz低通
#define FIR_LEN  (128)
static float fir_coef_b[FIR_LEN + 1] ={0.001364, -0.001009, -0.001358, -0.000845, 0.000328, 0.000870, 0.000046, -0.001164, -0.001094, 0.000425, 0.001572, 0.000723, -0.001273, -0.001857, -0.000006, 0.002128, 0.001681, -0.001116, -0.002802, -0.000929, 0.002459, 0.002985, -0.000451, -0.003734, -0.002413, 0.002331, 0.004545, 0.000925, -0.004396, -0.004473, 0.001453, 0.006163, 0.003177, -0.004447, -0.007040, -0.000497, 0.007527, 0.006437, -0.003443, -0.009946, -0.003896, 0.008190, 0.010827, -0.000795, -0.012942, -0.009279, 0.007504, 0.016601, 0.004484, -0.015728, -0.017795, 0.004292, 0.024643, 0.014802, -0.017997, -0.033204, -0.004871, 0.039100, 0.040318, -0.019479, -0.078933, -0.045026, 0.108777, 0.295845, 0.380006, 0.295845, 0.108777, -0.045026, -0.078933, -0.019479, 0.040318, 0.039100, -0.004871, -0.033204, -0.017997, 0.014802, 0.024643, 0.004292, -0.017795, -0.015728, 0.004484, 0.016601, 0.007504, -0.009279, -0.012942, -0.000795, 0.010827, 0.008190, -0.003896, -0.009946, -0.003443, 0.006437, 0.007527, -0.000497, -0.007040, -0.004447, 0.003177, 0.006163, 0.001453, -0.004473, -0.004396, 0.000925, 0.004545, 0.002331, -0.002413, -0.003734, -0.000451, 0.002985, 0.002459, -0.000929, -0.002802, -0.001116, 0.001681, 0.002128, -0.000006, -0.001857, -0.001273, 0.000723, 0.001572, 0.000425, -0.001094, -0.001164, 0.000046, 0.000870, 0.000328, -0.000845, -0.001358, -0.001009, 0.001364,
};uint8_t fir_filter_zhh(float* sig_in, float* sig_out, uint16_t sig_len)
{assert(sig_len > 1);uint16_t i, j;for (i = 0; i < sig_len; i++) {sig_out[i] = 0;for (j = 0; j < FIR_LEN; j++) {sig_out[i] += sig_in[i - j] * fir_coef_b[j];}}return 0;
}void Lowpass_init() {//memset(LowpassPra.DataBuf, 0, sizeof(LowpassPra.DataBuf));//memset(LowpassPra.DataAfFir, 0, sizeof(LowpassPra.DataAfFir));LowpassPra.FrameCnt = 0;}// 数据按帧传入，同时要传入帧号
uint8_t Lowpass(int16_t* data_frame, int16_t* data_frame_out, int16_t SampleRate)
{int16_t i;// 限制采样率必须是256if (SampleRate != 256) {//printf("SampleRate error!\n");return 1;}LowpassPra.FrameCnt++;  // 函数里面帧号从1开始计数if (LowpassPra.FrameCnt == 1) {memset(LowpassPra.DataBuf, 0 ,sizeof(LowpassPra.DataBuf));for (i = 0; i < POINT_FRAME;i++) {LowpassPra.DataBuf[i + POINT_FRAME] = (float)data_frame[i];}//memset(LowpassPra.DataAfFir, 0, sizeof(LowpassPra.DataAfFir));//memset(LowpassPra.DataTmp, 0, sizeof(LowpassPra.DataTmp));}else {memcpy(&LowpassPra.DataBuf[0], &LowpassPra.DataBuf[POINT_FRAME], POINT_FRAME * sizeof(LowpassPra.DataBuf[0]));  // 左移for (i = 0; i < POINT_FRAME; i++) {LowpassPra.DataBuf[POINT_FRAME + i] = (float)data_frame[i];}int16_t m = fir_filter_zhh(&LowpassPra.DataBuf[(POINT_FRAME >> 1) + (FIR_LEN >> 1)], &LowpassPra.DataAfFir[0], POINT_FRAME);for (i = 0; i < POINT_FRAME; i++) {data_frame_out[i] = (int16_t)LowpassPra.DataAfFir[i];}}  // if (sensoringCtrlPra.rCnt > 1)return  0;
}

测试代码：

main.c

#include<stdio.h>
#include<stdlib.h>
#include<math.h>#include"Lowpass.h"#define SampleRate (256)#define FrameNum (20)// 读matlab转换采样率为SampleRate后的txt数据文件
int16_t data_in[FrameNum * SampleRate] = { 0 };
int16_t data_out[FrameNum * SampleRate] = { 0 };
int16_t data_size = FrameNum * SampleRate;int8_t main()
{int16_t i;uint8_t ret;int16_t read_data_size = 0;FILE* fp;fp = fopen("Sign_input.txt", "r");if (fp == NULL){return 0;}for (i = 0; i < data_size; i++){ret = fscanf(fp, "%d", data_in + i); // 1:读文件成功，则返回成功读取的项数；2:读文件失败，则返回EOF。if (ret == 1)read_data_size++;}fclose(fp);if ((fp = fopen("data_in.txt", "w")) == NULL){printf("Cannot open the file...");exit(1);}for (i = 0; i < FrameNum * SampleRate; i++){fprintf(fp, "%d\n", data_in[i]);}fclose(fp);Lowpass_init();for (i = 0; i < FrameNum; i++) {ret = Lowpass(data_in + i * SampleRate, data_out + i * SampleRate - (SampleRate >> 1), SampleRate);//ret = Lowpass(data_in + i * SampleRate, data_out + i * SampleRate, SampleRate);if (ret) {return 1;}}if ((fp = fopen("data_out.txt", "w")) == NULL){printf("Cannot open the file...");exit(1);}for (i = 0; i < FrameNum * SampleRate; i++){fprintf(fp, "%d\n", data_out[i]);}fclose(fp);return 0;
}

附带对数据或者说调试的matlab代码：

fir_lowpass_45_50_order128.m

function Hd = fir_lowpass_45_50_order128
%FIR_LOWPASS_45_50_ORDER128 Returns a discrete-time filter object.% MATLAB Code
% Generated by MATLAB(R) 9.5 and Signal Processing Toolbox 8.1.
% Generated on: 11-Sep-2024 11:13:54% Equiripple Lowpass filter designed using the FIRPM function.% All frequency values are in Hz.
Fs = 250;  % Sampling FrequencyN     = 128;  % Order
Fpass = 45;   % Passband Frequency
Fstop = 50;   % Stopband Frequency
Wpass = 1;    % Passband Weight
Wstop = 1;    % Stopband Weight
dens  = 20;   % Density Factor% Calculate the coefficients using the FIRPM function.
b  = firpm(N, [0 Fpass Fstop Fs/2]/(Fs/2), [1 1 0 0], [Wpass Wstop], ...{dens});
Hd = dfilt.dffir(b);% [EOF]

test_vs.m

%%% 和vs对数据程序close all,clear,clc% fs = 200;
% f1 = 3;f2 = 40;
% t = 0:1/fs:1-1/fs;
% x = sin(2*pi*t*f1)+0.25*sin(2*pi*t*f2);load('..\data_in.txt');
load('..\data_out.txt');figure
plot(data_in)
hold on
plot(data_out)fid=fopen('..\data_in.txt'); %D:\zhh\work\VS projects\filter_int_v3
x=fscanf(fid,'%d');
fclose(fid);filename = 'data.txt';
% dlmwrite(filename, x);
fid = fopen(filename, 'w');
for i=1:length(x)fprintf(fid, '%.6f, ', x(i));
end
fclose(fid);%Hd = fir_lowhpass_40_50_equiripple;
Hd = fir_lowpass_45_50_order128;% 直接matlab滤波
[b, a] = tf(Hd);
% b=[1,2,1,3,1 ];
% a=[1,1,1,1,1 ];
y1 = iir_filter_zhh(b,a,x);
figure
plot(x);hold on
plot(y1);y2 = flipud(  filter( b,a,flipud(y1) )  );
figure
plot(x);hold on
plot(y2);filename = 'data_fir.txt';
fid = fopen(filename, 'w');
for i=1:length(x)fprintf(fid, '%.6f\n', y2(i));
end
fclose(fid);zhh = 1;