![]()
#include "FIRfilterPtByPt.h"
/**
* \brief Function used for initialization FIR filter structure used for filtering
* \param[in] signalBufferLength Length of cyclic buffer storing signal
* \param[in] signalCyclicBuffer[] Cyclic buffer storing signal
* \param[in] coeffLength Number of filter coefficients
* \param[in] coefficients[] Filter coefficients
* \return FIRfilterObject FIR filter structure
*/
FIRfilterObject CreateFIRfilterObject(uint16_t signalBufferLength, float signalCyclicBuffer[], uint16_t coeffLength, float coefficients[])
{
FIRfilterObject _FIRfilterObject;
_FIRfilterObject.coefficients = &coefficients[0];
_FIRfilterObject.coefficientsLength = coeffLength;
_FIRfilterObject.signalBuffer = &signalCyclicBuffer[0];
_FIRfilterObject.signalBufferHead = 0;
_FIRfilterObject.signalBufferLength = signalBufferLength;
_FIRfilterObject.signalBufferMask = signalBufferLength - 1;
//======= Zero out signalBuffer values =======//
uint16_t i;
uint16_t counterBy8 = _FIRfilterObject.signalBufferLength >> 3; // Division by 8
uint16_t remainder = _FIRfilterObject.signalBufferLength - ((_FIRfilterObject.signalBufferLength >> 3) << 3); // % 8
float* signalBufferPtr = &_FIRfilterObject.signalBuffer[0];
for(i=0; i<counterBy8; i++)
{
*signalBufferPtr++ = 0.0f;
*signalBufferPtr++ = 0.0f;
*signalBufferPtr++ = 0.0f;
*signalBufferPtr++ = 0.0f;
*signalBufferPtr++ = 0.0f;
*signalBufferPtr++ = 0.0f;
*signalBufferPtr++ = 0.0f;
*signalBufferPtr++ = 0.0f;
}
for(i=0; i<remainder; i++)
{
*signalBufferPtr++ = 0.0f;
}
//===================================//
return _FIRfilterObject;
}
/**
* \brief Function filtering input signal point-by-point
* \param[in] filterObject Pointer to FIR filter structure
* \param[in] signal One sample of input signal
* \param[out] result Result of filtering
*/
void FIRfilterPtByPt(FIRfilterObject* filterObject, float signal, float* result)
{
*result = 0;
filterObject->signalBuffer[filterObject->signalBufferHead] = signal;
float* coefficientsPtr;
coefficientsPtr = &filterObject->coefficients[0];
float* signalPtr;
signalPtr = &filterObject->signalBuffer[filterObject->signalBufferHead];
int16_t i;
int16_t counterBy8;
int16_t remainder;
if( filterObject->signalBufferHead >= filterObject->coefficientsLength)
{
counterBy8 = filterObject->coefficientsLength >> 3; // filterObject->coefficientsLength / 8
remainder = filterObject->coefficientsLength - ((filterObject->coefficientsLength >> 3) << 3); // filterObject->coefficientsLength % 8
for(i=0; i<counterBy8; i++)
{
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
}
for(i=0; i<remainder; i++)
{
*result += *signalPtr-- * *coefficientsPtr++;
}
}
else
{
counterBy8 = filterObject->signalBufferHead >> 3; // filterObject->signalBufferHead / 8
remainder = filterObject->signalBufferHead - ((filterObject->signalBufferHead >> 3) << 3); // filterObject->signalBufferHead % 8
for(i=0; i<counterBy8; i++)
{
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
}
for(i=0; i<remainder; i++)
{
*result += *signalPtr-- * *coefficientsPtr++;
}
*result += filterObject->signalBuffer[0] * *coefficientsPtr++;
signalPtr = &filterObject->signalBuffer[filterObject->signalBufferLength-1];
counterBy8 = filterObject->coefficientsLength - filterObject->signalBufferHead - 1;
remainder = counterBy8 - ((counterBy8 >> 3) << 3); // counterBy8 % 8
counterBy8 = counterBy8 >> 3; // counterBy8 / 8
for(i=0; i<counterBy8; i++)
{
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
*result += *signalPtr-- * *coefficientsPtr++;
}
for(i=0; i<remainder; i++)
{
*result += *signalPtr-- * *coefficientsPtr++;
}
}
filterObject->signalBufferHead = (filterObject->signalBufferHead + 1) & filterObject->signalBufferMask;
}
#include "IIRfilterPtByPt.h"
/**
* \brief Function used for initialization IIR 2nd order filter structure used for filtering
* \param[in] forwardCoeff[3] Filter forward coefficients
* \param[in] reverseCoeff[3] Filter reverse coefficients
* \return IIR2ndOrderFilterObj IIR 2nd order filter structure
*/
IIR2ndOrderFilterObj IIR_2ndOrderSignalFilteringPtByPtInitialize(float forwardCoeff[3], float reverseCoeff[3])
{
IIR2ndOrderFilterObj IIR2ndOrderFilterObj_;
IIR2ndOrderFilterObj_.forwardCoeff = forwardCoeff;
IIR2ndOrderFilterObj_.reverseCoeff = reverseCoeff;
return IIR2ndOrderFilterObj_;
}
/**
* \brief Function used for initialization IIR 3rd order filter structure used for filtering
* \param[in] forwardCoeff[4] Filter forward coefficients
* \param[in] reverseCoeff[4] Filter reverse coefficients
* \return IIR3rdOrderFilterObj IIR 3rd order filter structure
*/
IIR3rdOrderFilterObj IIR_3rdOrderSignalFilteringPtByPtInitialize(float forwardCoeff[4], float reverseCoeff[4])
{
IIR3rdOrderFilterObj IIR3rdOrderFilterObj_;
IIR3rdOrderFilterObj_.forwardCoeff = forwardCoeff;
IIR3rdOrderFilterObj_.reverseCoeff = reverseCoeff;
return IIR3rdOrderFilterObj_;
}
/**
* \brief Function used for initialization IIR 4th order filter structure used for filtering
* \param[in] forwardCoeff[5] Filter forward coefficients
* \param[in] reverseCoeff[5] Filter reverse coefficients
* \return IIR4thOrderFilterObj IIR 4th order filter structure
*/
IIR4thOrderFilterObj IIR_4thOrderSignalFilteringPtByPtInitialize(float forwardCoeff[5], float reverseCoeff[5])
{
IIR4thOrderFilterObj IIR4thOrderFilterObj_;
IIR4thOrderFilterObj_.forwardCoeff = forwardCoeff;
IIR4thOrderFilterObj_.reverseCoeff = reverseCoeff;
return IIR4thOrderFilterObj_;
}
/**
* \brief Function for recursive Infinite Impulse Response 2nd order filtering of input signal point-by-point
* \warning This IIR filter implemantation can be used only when first reverse coefficient value == 1
* \param[in] filterObject Pointer to structure of 2nd order IIR filter
* \param[in] signal Input signal last sample
* \param[out] result Output filtered signal sample
*/
void IIR_2ndOrderSignalFilteringPtByPt(IIR2ndOrderFilterObj* filterObject, float signal, float* result)
{
static float as;
filterObject->stateBuffer[0] += signal * filterObject->forwardCoeff[0];
filterObject->stateBuffer[1] += signal * filterObject->forwardCoeff[1];
filterObject->stateBuffer[2] += signal * filterObject->forwardCoeff[2];
as = -filterObject->stateBuffer[0];
filterObject->stateBuffer[1] += as * filterObject->reverseCoeff[1];
filterObject->stateBuffer[2] += as * filterObject->reverseCoeff[2];
*result = filterObject->stateBuffer[0];
filterObject->stateBuffer[0] = filterObject->stateBuffer[1];
filterObject->stateBuffer[1] = filterObject->stateBuffer[2];
filterObject->stateBuffer[2] = 0;
}
/**
* \brief Function for recursive Infinite Impulse Response 3rd order filtering of input signal point-by-point
* \warning This IIR filter implemantation can be used only when first reverse coefficient value == 1
* \param[in] filterObject Pointer to structure of 3rd order IIR filter
* \param[in] signal Input signal last sample
* \param[out] result Output filtered signal sample
*/
void IIR_3rdOrderSignalFilteringPtByPt(IIR3rdOrderFilterObj* filterObject, float signal, float* result)
{
static float as;
filterObject->stateBuffer[0] += signal * filterObject->forwardCoeff[0];
filterObject->stateBuffer[1] += signal * filterObject->forwardCoeff[1];
filterObject->stateBuffer[2] += signal * filterObject->forwardCoeff[2];
filterObject->stateBuffer[3] += signal * filterObject->forwardCoeff[3];
as = -filterObject->stateBuffer[0];
filterObject->stateBuffer[1] += as * filterObject->reverseCoeff[1];
filterObject->stateBuffer[2] += as * filterObject->reverseCoeff[2];
filterObject->stateBuffer[3] += as * filterObject->reverseCoeff[3];
*result = filterObject->stateBuffer[0];
filterObject->stateBuffer[0] = filterObject->stateBuffer[1];
filterObject->stateBuffer[1] = filterObject->stateBuffer[2];
filterObject->stateBuffer[2] = filterObject->stateBuffer[3];
filterObject->stateBuffer[3] = 0;
}
/**
* \brief Function for recursive Infinite Impulse Response 4th order filtering of input signal point-by-point
* \warning This IIR filter implemantation can be used only when first reverse coefficient value == 1
* \param[in] filterObject Pointer to structure of 4th order IIR filter
* \param[in] signal Input signal last sample
* \param[out] result Output filtered signal sample
*/
void IIR_4thOrderSignalFilteringPtByPt(IIR4thOrderFilterObj* filterObject, float signal, float* result)
{
static float as;
filterObject->stateBuffer[0] += signal * filterObject->forwardCoeff[0];
filterObject->stateBuffer[1] += signal * filterObject->forwardCoeff[1];
filterObject->stateBuffer[2] += signal * filterObject->forwardCoeff[2];
filterObject->stateBuffer[3] += signal * filterObject->forwardCoeff[3];
filterObject->stateBuffer[4] += signal * filterObject->forwardCoeff[4];
as = -filterObject->stateBuffer[0];
filterObject->stateBuffer[1] += as * filterObject->reverseCoeff[1];
filterObject->stateBuffer[2] += as * filterObject->reverseCoeff[2];
filterObject->stateBuffer[3] += as * filterObject->reverseCoeff[3];
filterObject->stateBuffer[4] += as * filterObject->reverseCoeff[4];
*result = filterObject->stateBuffer[0];
filterObject->stateBuffer[0] = filterObject->stateBuffer[1];
filterObject->stateBuffer[1] = filterObject->stateBuffer[2];
filterObject->stateBuffer[2] = filterObject->stateBuffer[3];
filterObject->stateBuffer[3] = filterObject->stateBuffer[4];
filterObject->stateBuffer[4] = 0;
}
|
楼主
标题置顶
标题高亮
