| #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 |