mathlib: LowPassFilter2p and NotchFilter add magnitude response getter

src/lib/mathlib/math/filter/LowPassFilter2p.cpp

@@ -41,6 +41,7 @@ namespace math
 void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
 {
 	_cutoff_freq = cutoff_freq;
+	_sample_freq = sample_freq;
 
 	// reset delay elements on filter change
 	_delay_element_1 = 0.0f;

src/lib/mathlib/math/filter/LowPassFilter2p.hpp

@@ -81,22 +81,25 @@ public:
 	// Return the cutoff frequency
 	float get_cutoff_freq() const { return _cutoff_freq; }
 
+	float getMagnitudeResponse(float frequency) const;
+
 	// Reset the filter state to this value
 	float reset(float sample);
 
 protected:
 
-	float _cutoff_freq{0.0f};
+	float _cutoff_freq{0.f};
+	float _sample_freq{0.f};
 
-	float _a1{0.0f};
-	float _a2{0.0f};
+	float _a1{0.f};
+	float _a2{0.f};
 
-	float _b0{0.0f};
-	float _b1{0.0f};
-	float _b2{0.0f};
+	float _b0{0.f};
+	float _b1{0.f};
+	float _b2{0.f};
 
-	float _delay_element_1{0.0f};	// buffered sample -1
-	float _delay_element_2{0.0f};	// buffered sample -2
+	float _delay_element_1{0.f};	// buffered sample -1
+	float _delay_element_2{0.f};	// buffered sample -2
 };
 
 } // namespace math

src/lib/mathlib/math/filter/NotchFilter.hpp

@@ -123,6 +123,18 @@ public:
 		b[2] = _b2;
 	}
 
+	float getMagnitudeResponse(float frequency) const
+	{
+		float w = 2.f * M_PI_F * frequency / _sample_freq;
+
+		float numerator = _b0 * _b0 + _b1 * _b1 + _b2 * _b2
+				  + 2.f * (_b0 * _b1 + _b1 * _b2) * cosf(w) + 2.f * _b0 * _b2 * cosf(2.f * w);
+
+		float denominator = 1.f + _a1 * _a1 + _a2 * _a2 + 2.f * (_a1 + _a1 * _a2) * cosf(w) + 2.f * _a2 * cosf(2.f * w);
+
+		return sqrtf(numerator / denominator);
+	}
+
 	/**
 	 * Bypasses the filter update to directly set different filter coefficients.
 	 * Note: the filtered frequency and quality factor saved on the filter lose their
@@ -144,6 +156,7 @@ public:
 protected:
 	float _notch_freq{};
 	float _bandwidth{};
+	float _sample_freq{};
 
 	// All the coefficients are normalized by a0, so a0 becomes 1 here
 	float _a1{};
@@ -170,6 +183,7 @@ void NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float ba
 {
 	_notch_freq = notch_freq;
 	_bandwidth = bandwidth;
+	_sample_freq = sample_freq;
 
 	if (notch_freq <= 0.f) {
 		// no filtering