hpm6750/controller_yy_app/matrix/flt_butter.c

#include "flt_butter.h"

#include <float.h>
#include <math.h>
#include <stdint.h>

int butter3_filter_init(LpfButter3 *butter, const float b[4],
                        const float a[4])
{

    for (uint8_t i = 0; i < 4; i++)
    {
        butter->B[i] = b[i];
        butter->A[i] = a[i];
        butter->X[i] = butter->Y[i] = 0.0f;
    }

    return 0;
}

int butter3_filter_reset(float sample, LpfButter3 *butter)
{

    for (uint8_t i = 0; i < 4; i++)
    {
        butter->X[i] = butter->Y[i] = sample;
    }

    return 0;
}

float butter3_filter_apply(float in, LpfButter3 *butter)
{
    float out;
    butter->X[3] = in;
    /* a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb)
                         - a(2)*y(n-1) - ... - a(na+1)*y(n-na)  */
    butter->Y[3] = butter->B[0] * butter->X[3] + butter->B[1] * butter->X[2] +
                   butter->B[2] * butter->X[1] + butter->B[3] * butter->X[0] -
                   butter->A[1] * butter->Y[2] - butter->A[2] * butter->Y[1] -
                   butter->A[3] * butter->Y[0];

    /* we assume a(1)=1 */
    out = butter->Y[3];

    /* move X and Y */
    butter->X[0] = butter->X[1];
    butter->X[1] = butter->X[2];
    butter->X[2] = butter->X[3];
    butter->Y[0] = butter->Y[1];
    butter->Y[1] = butter->Y[2];
    butter->Y[2] = butter->Y[3];

    return out;
}

void butter2_filter_disable(LpfButter2 *lp)
{
    lp->_sample_freq = 0.f;
    lp->_cutoff_freq = 0.f;

    lp->_delay_element1 = 0.0f;
    lp->_delay_element2 = 0.0f;

    lp->_b0 = 1.f;
    lp->_b1 = 0.f;
    lp->_b2 = 0.f;

    lp->_a1 = 0.f;
    lp->_a2 = 0.f;
}

int butter2_filter_set_cutoff_freq(LpfButter2 *lp, float sample_freq,
                                   float cutoff_freq)
{
    if ((sample_freq <= 0.f) || (cutoff_freq <= 0.f) ||
        (cutoff_freq >= sample_freq / 2) || !isfinite(sample_freq) ||
        !isfinite(cutoff_freq))
    {
        butter2_filter_disable(lp);
        return -1;
    }

    // reset delay elements on filter change
    lp->_delay_element1 = 0;
    lp->_delay_element2 = 0;

    cutoff_freq = cutoff_freq >= 1.f ? cutoff_freq : 1.f;
    cutoff_freq =
        cutoff_freq <= (sample_freq / 2) ? cutoff_freq : (sample_freq / 2);
    // TODO: min based on actual numerical limit

    lp->_sample_freq = sample_freq;
    lp->_cutoff_freq = cutoff_freq;

    const float fr = lp->_sample_freq / lp->_cutoff_freq;
    const float ohm = tanf(M_PI / fr);
    const float c = 1.f + 2.f * cosf(M_PI / 4.f) * ohm + ohm * ohm;

    lp->_b0 = ohm * ohm / c;
    lp->_b1 = 2.f * lp->_b0;
    lp->_b2 = lp->_b0;

    lp->_a1 = 2.f * (ohm * ohm - 1.f) / c;
    lp->_a2 = (1.f - 2.f * cosf(M_PI / 4.f) * ohm + ohm * ohm) / c;

    if (!isfinite(lp->_b0) || !isfinite(lp->_b1) || !isfinite(lp->_b2) ||
        !isfinite(lp->_a1) || !isfinite(lp->_a2))
    {
        butter2_filter_disable(lp);
        return -1;
    }

    return 0;
}

int butter2_filter_init(LpfButter2 *butter, float sample_freq,
                        float cuttoff_freq)
{
    return butter2_filter_set_cutoff_freq(butter, sample_freq, cuttoff_freq);
}

float butter2_filter_apply(LpfButter2 *lp, float sample)
{
    float delay_element_0 =
        sample - lp->_delay_element1 * lp->_a1 - lp->_delay_element2 * lp->_a2;

    const float output = delay_element_0 * lp->_b0 +
                         lp->_delay_element1 * lp->_b1 +
                         lp->_delay_element2 * lp->_b2;
    lp->_delay_element2 = lp->_delay_element1;
    lp->_delay_element1 = delay_element_0;
    return output;
}

float butter2_filter_reset(LpfButter2 *lp, float sample)
{
    const float input = isfinite(sample) ? sample : 0.0f;

    if (fabsf(1 + lp->_a1 + lp->_a2) > FLT_EPSILON)
    {
        lp->_delay_element1 = lp->_delay_element2 = input / (1 + lp->_a1 + lp->_a2);

        if (!isfinite(lp->_delay_element1) || !isfinite(lp->_delay_element2))
        {
            lp->_delay_element1 = lp->_delay_element2 = input;
        }
    }
    else
    {
        lp->_delay_element1 = lp->_delay_element2 = input;
    }

    return butter2_filter_apply(lp, input);
}