2021-06-21 01:22:59 +00:00
|
|
|
// Copyright 2015-2021 Rob Riggs <rob@mobilinkd.com>
|
2018-07-30 02:34:53 +00:00
|
|
|
// All rights reserved.
|
|
|
|
|
2021-01-03 03:59:59 +00:00
|
|
|
#pragma once
|
2018-07-30 02:34:53 +00:00
|
|
|
|
|
|
|
#include <AudioLevel.hpp>
|
|
|
|
|
|
|
|
#include "arm_math.h"
|
|
|
|
|
2021-07-01 01:16:15 +00:00
|
|
|
#include <array>
|
2018-07-30 02:34:53 +00:00
|
|
|
#include <cstdlib>
|
|
|
|
|
|
|
|
namespace mobilinkd { namespace tnc {
|
|
|
|
|
|
|
|
template <size_t N>
|
|
|
|
struct TFirCoefficients {
|
|
|
|
float taps[N];
|
|
|
|
};
|
|
|
|
|
|
|
|
struct FirCoefficients {
|
|
|
|
size_t size;
|
|
|
|
const float* taps;
|
|
|
|
|
|
|
|
template <size_t N>
|
|
|
|
FirCoefficients(const TFirCoefficients<N>& c)
|
|
|
|
: size(N), taps(c.taps)
|
|
|
|
{}
|
|
|
|
};
|
|
|
|
|
|
|
|
template <size_t BLOCK_SIZE, size_t FILTER_SIZE>
|
|
|
|
struct FirFilter {
|
|
|
|
float filter_state[BLOCK_SIZE + FILTER_SIZE - 1];
|
2021-07-01 01:16:15 +00:00
|
|
|
float filter_output[BLOCK_SIZE];
|
2018-07-30 02:34:53 +00:00
|
|
|
arm_fir_instance_f32 instance;
|
|
|
|
|
|
|
|
FirFilter()
|
|
|
|
{}
|
|
|
|
|
2021-12-28 23:39:21 +00:00
|
|
|
FirFilter(std::array<float, FILTER_SIZE> const& taps)
|
2018-07-30 02:34:53 +00:00
|
|
|
{
|
2021-07-01 01:16:15 +00:00
|
|
|
init(taps);
|
2018-07-30 02:34:53 +00:00
|
|
|
}
|
|
|
|
|
2021-07-01 01:16:15 +00:00
|
|
|
FirFilter(const float* taps)
|
2018-07-30 02:34:53 +00:00
|
|
|
{
|
2021-07-01 01:16:15 +00:00
|
|
|
init(taps);
|
2018-07-30 02:34:53 +00:00
|
|
|
}
|
|
|
|
|
2021-12-28 23:39:21 +00:00
|
|
|
void init(std::array<float, FILTER_SIZE> const& taps)
|
2018-07-30 02:34:53 +00:00
|
|
|
{
|
2021-07-01 01:16:15 +00:00
|
|
|
arm_fir_init_f32(&instance, FILTER_SIZE, (float*)taps.data(),
|
2018-07-30 02:34:53 +00:00
|
|
|
filter_state, BLOCK_SIZE);
|
|
|
|
}
|
|
|
|
|
2021-07-01 01:16:15 +00:00
|
|
|
void init(const float* taps)
|
2018-07-30 02:34:53 +00:00
|
|
|
{
|
2021-07-01 01:16:15 +00:00
|
|
|
arm_fir_init_f32(&instance, FILTER_SIZE, (float*)taps,
|
|
|
|
filter_state, BLOCK_SIZE);
|
2018-07-30 02:34:53 +00:00
|
|
|
}
|
|
|
|
|
2021-07-01 01:16:15 +00:00
|
|
|
float* operator()(float* input)
|
2018-07-30 02:34:53 +00:00
|
|
|
{
|
2021-07-01 01:16:15 +00:00
|
|
|
arm_fir_f32(&instance, input, filter_output, BLOCK_SIZE);
|
|
|
|
return filter_output;
|
2018-07-30 02:34:53 +00:00
|
|
|
}
|
2019-06-23 02:53:34 +00:00
|
|
|
|
|
|
|
float operator()(float input) // __attribute__((section(".bss2")))
|
|
|
|
{
|
2021-07-01 01:16:15 +00:00
|
|
|
arm_fir_f32(&instance, &input, filter_output, 1);
|
|
|
|
return filter_output[0];
|
2019-06-23 02:53:34 +00:00
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
template <size_t BLOCK_SIZE, size_t FILTER_SIZE>
|
|
|
|
struct Q15FirFilter {
|
2021-01-03 03:59:59 +00:00
|
|
|
const q15_t* filter_taps = nullptr;
|
2019-06-23 02:53:34 +00:00
|
|
|
q15_t filter_state[BLOCK_SIZE + FILTER_SIZE - 1];
|
|
|
|
q15_t filter_output[BLOCK_SIZE];
|
2021-01-03 03:59:59 +00:00
|
|
|
q15_t vgnd_ = 0;
|
|
|
|
q15_t i_vgnd_ = 0;
|
|
|
|
arm_fir_instance_q15 instance;
|
2019-06-23 02:53:34 +00:00
|
|
|
|
|
|
|
Q15FirFilter()
|
|
|
|
{}
|
|
|
|
|
|
|
|
Q15FirFilter(const q15_t* taps)
|
|
|
|
: filter_taps(taps)
|
|
|
|
{
|
|
|
|
init(taps);
|
|
|
|
}
|
|
|
|
|
|
|
|
void init(const q15_t* taps)
|
|
|
|
{
|
|
|
|
vgnd_ = audio::virtual_ground;
|
|
|
|
filter_taps = taps;
|
2021-01-03 03:59:59 +00:00
|
|
|
arm_fir_init_q15(&instance, FILTER_SIZE,
|
|
|
|
const_cast<q15_t*>(filter_taps),
|
2019-06-23 02:53:34 +00:00
|
|
|
filter_state, BLOCK_SIZE);
|
|
|
|
}
|
|
|
|
|
2021-01-03 03:59:59 +00:00
|
|
|
q15_t* filter(const q15_t* input)
|
2019-06-23 02:53:34 +00:00
|
|
|
{
|
2021-01-03 03:59:59 +00:00
|
|
|
arm_fir_fast_q15(&instance, const_cast<q15_t*>(input), filter_output, BLOCK_SIZE);
|
2019-06-23 02:53:34 +00:00
|
|
|
return filter_output;
|
|
|
|
}
|
2018-07-30 02:34:53 +00:00
|
|
|
};
|
|
|
|
|
|
|
|
}} // mobilinkd::tnc
|