|
|
|
|
|
|
|
|
|
|
|
|
|
|
#include "Arduino.h" |
|
|
#include "Arduino.h" |
|
|
#include <arm_math.h> |
|
|
#include <arm_math.h> |
|
|
|
|
|
#include <type_traits> |
|
|
|
|
|
|
|
|
enum class BiquadType { |
|
|
enum class BiquadType { |
|
|
LOW_PASS, HIGH_PASS, BAND_PASS, NOTCH, ALL_PASS, PEAKING, LOW_SHELF, HIGH_SHELF |
|
|
LOW_PASS, HIGH_PASS, BAND_PASS, NOTCH, ALL_PASS, PEAKING, LOW_SHELF, HIGH_SHELF |
|
|
|
|
|
|
|
|
template <typename T> |
|
|
template <typename T> |
|
|
void getCoefficients(T* coeffs, BiquadType type, double dbGain, double freq, double srate, double bandwidthOrQOrS, bool isBandwidthOrS=false){ |
|
|
void getCoefficients(T* coeffs, BiquadType type, double dbGain, double freq, double srate, double bandwidthOrQOrS, bool isBandwidthOrS=false){ |
|
|
const double A =(type == BiquadType::PEAKING || type == BiquadType::LOW_SHELF || type == BiquadType::HIGH_SHELF) ? pow(10., dbGain / 40.) : pow(10, dbGain / 20); |
|
|
const double A =(type == BiquadType::PEAKING || type == BiquadType::LOW_SHELF || type == BiquadType::HIGH_SHELF) ? pow(10., dbGain / 40.) : pow(10, dbGain / 20); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
const double omega = 2 * M_PI * freq / srate; |
|
|
const double omega = 2 * M_PI * freq / srate; |
|
|
const double sn = sin(omega); |
|
|
const double sn = sin(omega); |
|
|
const double cs = cos(omega); |
|
|
const double cs = cos(omega); |
|
|
|
|
|
|
|
|
break; |
|
|
break; |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
*coeffs++=(T)(b0 * a0Inv); |
|
|
|
|
|
*coeffs++=(T)(b1 * a0Inv); |
|
|
|
|
|
*coeffs++=(T)(b2 * a0Inv); |
|
|
|
|
|
*coeffs++=(T)(-a1 * a0Inv); |
|
|
|
|
|
*coeffs=(T)(-a2 * a0Inv); |
|
|
|
|
|
|
|
|
using writable_type = typename std::remove_const<T>::type; |
|
|
|
|
|
writable_type* coeffs_ptr = const_cast<writable_type*>(coeffs); |
|
|
|
|
|
*coeffs_ptr++ = (T)(b0 * a0Inv); |
|
|
|
|
|
*coeffs_ptr++ = (T)(b1 * a0Inv); |
|
|
|
|
|
*coeffs_ptr++ = (T)(b2 * a0Inv); |
|
|
|
|
|
*coeffs_ptr++ = (T)(-a1 * a0Inv); |
|
|
|
|
|
*coeffs_ptr = (T)(-a2 * a0Inv); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
template <typename T, typename BIQUAD, typename BTYPE> |
|
|
template <typename T, typename BIQUAD, typename BTYPE> |
|
|
|
|
|
|
|
|
*pDst++=(T)yn; |
|
|
*pDst++=(T)yn; |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
template <typename B> |
|
|
template <typename B> |
|
|
void preload(const B* S, double val=0.){ |
|
|
void preload(const B* S, double val=0.){ |
|
|
|
|
|
|
|
|
*(S->pState) = (*(S->pCoeffs+1) + *(S->pCoeffs+3)) * val + *(S->pState+1); |
|
|
*(S->pState) = (*(S->pCoeffs+1) + *(S->pCoeffs+3)) * val + *(S->pState+1); |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
#endif |