Filter: added a notch filter

maths from Leonard

libraries/Filter/NotchFilter.cpp

@@ -0,0 +1,142 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "NotchFilter.h"
+
+/*
+  initialise filter
+ */
+template <class T>
+void NotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB)
+{
+    float omega = 2.0 * M_PI * center_freq_hz / sample_freq_hz;
+    float octaves = log2f(center_freq_hz  / (center_freq_hz - bandwidth_hz/2)) * 2;
+    float A = powf(10, -attenuation_dB/40);
+    float Q = sqrtf(powf(2, octaves)) / (powf(2,octaves) - 1);
+    float alpha = sinf(omega) / (2 * Q/A);
+    b0 =  1.0 + alpha*A;
+    b1 = -2.0 * cosf(omega);
+    b2 =  1.0 - alpha*A;
+    a0_inv =  1.0/(1.0 + alpha/A);
+    a1 = -2.0 * cosf(omega);
+    a2 =  1.0 - alpha/A;
+    initialised = true;
+}
+
+/*
+  apply a new input sample, returning new output
+ */
+template <class T>
+T NotchFilter<T>::apply(const T &sample)
+{
+    if (!initialised) {
+        // if we have not been initialised when return the input
+        // sample as output
+        return sample;
+    }
+    ntchsig2 = ntchsig1;
+    ntchsig1 = ntchsig;
+    ntchsig = sample;
+    T output = (ntchsig*b0 + ntchsig1*b1 + ntchsig2*b2 - signal1*a1 - signal2*a2) * a0_inv;
+    signal2 = signal1;
+    signal1 = output;
+    return output;
+}
+
+// table of user settable parameters
+const AP_Param::GroupInfo NotchFilterVector3fParam::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: Enable
+    // @Description: Enable notch filter
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO_FLAGS("ENABLE", 1, NotchFilterVector3fParam, enable, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: FREQ
+    // @DisplayName: Frequency
+    // @Description: Notch center frequency in Hz
+    // @Range: 10 200
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("FREQ", 2, NotchFilterVector3fParam, center_freq_hz, 80),
+
+    // @Param: BW
+    // @DisplayName: Bandwidth
+    // @Description: Notch bandwidth in Hz
+    // @Range: 5 50
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("BW", 3, NotchFilterVector3fParam, bandwidth_hz, 20),
+
+    // @Param: ATT
+    // @DisplayName: Attenuation
+    // @Description: Notch attenuation in dB
+    // @Range: 5 30
+    // @Units: dB
+    // @User: Advanced
+    AP_GROUPINFO("ATT", 4, NotchFilterVector3fParam, attenuation_dB, 15),
+    
+    AP_GROUPEND
+};
+
+/*
+  a notch filter with enable and filter parameters - constructor
+ */
+NotchFilterVector3fParam::NotchFilterVector3fParam(void)
+{
+    AP_Param::setup_object_defaults(this, var_info);    
+}
+
+/*
+  initialise filter
+ */
+void NotchFilterVector3fParam::init(float _sample_freq_hz)
+{
+    filter.init(_sample_freq_hz, center_freq_hz, bandwidth_hz, attenuation_dB);
+
+    sample_freq_hz = _sample_freq_hz;
+    last_center_freq = center_freq_hz;
+    last_bandwidth = bandwidth_hz;
+    last_attenuation = attenuation_dB;
+}
+
+/*
+  apply a filter sample
+ */
+Vector3f NotchFilterVector3fParam::apply(const Vector3f &sample)
+{
+    if (!enable) {
+        // when not enabled it is a simple pass-through
+        return sample;
+    }
+
+    // check for changed parameters
+    if (!is_equal(center_freq_hz.get(), last_center_freq) ||
+        !is_equal(bandwidth_hz.get(), last_bandwidth) ||
+        !is_equal(attenuation_dB.get(), last_attenuation)) {
+        if (!is_zero(sample_freq_hz)) {
+            init(sample_freq_hz);
+        }
+    }
+    
+    return filter.apply(sample);
+}
+
+/* 
+   instantiate template classes
+ */
+template class NotchFilter<float>;
+template class NotchFilter<Vector3f>;

libraries/Filter/NotchFilter.h

@@ -0,0 +1,70 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+/*
+  notch filter with settable sample rate, center frequency, bandwidth and attenuation
+
+  Design by Leonard Hall
+ */
+
+#include <AP_Math/AP_Math.h>
+#include <cmath>
+#include <inttypes.h>
+#include <AP_Param/AP_Param.h>
+
+
+template <class T>
+class NotchFilter {
+public:
+    // set parameters
+    void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB);
+    T apply(const T &sample);
+
+private:
+    bool initialised;
+    float b0, b1, b2, a1, a2, a0_inv;
+    T ntchsig, ntchsig1, ntchsig2, signal2, signal1;
+};
+
+/*
+  a notch filter with enable and filter parameters
+ */
+class NotchFilterVector3fParam {
+public:
+    NotchFilterVector3fParam(void);
+    void init(float sample_freq_hz);
+    Vector3f apply(const Vector3f &sample);
+
+    static const struct AP_Param::GroupInfo var_info[];
+    
+private:
+    AP_Int8 enable;
+    AP_Float center_freq_hz;
+    AP_Float bandwidth_hz;
+    AP_Float attenuation_dB;
+
+    float sample_freq_hz;
+
+    float last_center_freq;
+    float last_bandwidth;
+    float last_attenuation;
+    
+    NotchFilter<Vector3f> filter;
+};
+
+typedef NotchFilter<float> NotchFilterFloat;
+typedef NotchFilter<Vector3f> NotchFilterVector3f;
+