Revision 37:fc1104eeb6ca, committed 2021-08-06

Comitter:

AndyA

Date:

Fri Aug 06 09:15:41 2021 +0000

Parent:

35:7ecf25d9c414

Child:

38:b5765588345d

Commit message:

Fixed memory filter allocation issues. Minor tidy up of filter code.

Changed in this revision

--- a/Filter/LowPassFilter.cpp	Thu Aug 05 13:44:45 2021 +0000
+++ b/Filter/LowPassFilter.cpp	Fri Aug 06 09:15:41 2021 +0000
@@ -2,24 +2,29 @@
 /*
 //To use ARM DSP blocks
 
-    CMSIS_Stages = order/2 (order must be even)
+// include https://os.mbed.com/users/mbed_official/code/mbed-dsp/ in project
+
 
-    for order 2:
-   { xval[0] / gain, xval[1] / gain, xval[2] / gain, yVal[1], yVal[0] }
-    m_filterCoeff[0] =  xval[0] / gain;
-    m_filterCoeff[1] =  xval[1] / gain;
+    #include "arm_math.h"
+    int CMSIS_Stages = order/2  // (order must be even)
+    float m_forwardFilterStateArray[4*CMSIS_Stages];
+    float m_filterCoeff[CMSIS_Stages*4+1];
+    arm_biquad_casd_df1_inst_f32 filterStructure;
+
+  Filter init for order 2:
+    m_filterCoeff[0] = xval[0] / gain;
+    m_filterCoeff[1] = xval[1] / gain;
     m_filterCoeff[2] = xval[2] / gain;
     m_filterCoeff[3] = yVal[1];
-    m_filterCoeff[4] =  yVal[0];
-
-    float32_t forwardFilterStateArray[4*CMSIS_Stages];
-    float m_filterCoeff[CMSIS_Stages*4+1];
+    m_filterCoeff[4] = yVal[0];
+    memset(forwardFilterStateArray, 0, 4 * CMSIS_Stages * sizeof(float32_t));
+    arm_biquad_cascade_df1_init_f32(&filterStructure, CMSIS_Stages, m_filterCoeff, forwardFilterStateArray);
 
-    arm_biquad_casd_df1_inst_f32 filterStructure;
-    memset(forwardFilterStateArray, 0, 4 * CMSIS_Stages * sizeof(float32_t));
-    arm_biquad_cascade_df1_init_f32(&filterStructure, CMSIS_Stages, &m_filterCoeff[0], forwardFilterStateArray);
+  To filter data
+    arm_biquad_cascade_df1_f32(&filterStructure, float* input, float* output, numberOfPoints);
 
-    arm_biquad_cascade_df1_f32(&filterStructure, float* input, float* output, numberOfPoints);
+// see https://www.keil.com/pack/doc/CMSIS/DSP/html/group__BiquadCascadeDF1.html for details of how these work.
+
 
 */
 
@@ -38,52 +43,57 @@
     m_enable = false;
     m_order = 0;
     m_lastIn = 0;
+    m_lastOut =&m_lastIn;
 }
 
 bool LowPassFilter::makeFilter(int order, float frequency, float sampleRate)
 {
     if (order > MakeFilterName::MAXORDER)
         order = MakeFilterName::MAXORDER;
-    if (order != m_order) {
-        freeBuffers();
-        if (order==0) { // order 0 - bypass
-            m_enable = false;
-            m_order = 0;
-            return true;
-        }
-        if (!createBuffers(order)) {
-            m_enable = false;
-            m_order = 0;
-            return false;
+
+    if (order != m_order) { // change buffer sizes
+        freeBuffers();      // clear existing buffers
+        if (order) {        // if need new buffers
+            if (!createBuffers(order)) {   // create buffers and check for errors
+                return false;              // failed to create buffers.
+            }
         }
     }
-    if (order==0) { // order 0 - bypass
+
+    // buffers are now correct size for order
+
+    if (order==0) { // order 0 - bypass the filter
         m_enable = false;
         m_order = 0;
+        m_lastOut =&m_lastIn;
         return true;
     }
+
     // have correct size buffers. order != 0
 
+    // calculate filter perameters and copy to local buffers.
     MakeFilter *newFilter = new MakeFilter(order, frequency, sampleRate);
     if (newFilter) {
-        m_gain = newFilter->getGain();
+        double gain = newFilter->getGain();
         for (int i=0; i<(order+1); i++) {
-            m_inScale[i] = newFilter->getXCoeff()[i];
+            m_inScale[i] = newFilter->getXCoeff()[i] / gain;
             m_outScale[i] = newFilter->getYCoeff()[i];
             m_inValues[i] = 0;
             m_outValues[i] = 0;
         }
         delete newFilter;
-    } else {
-        freeBuffers();
+    } else {            // failed to alocate memory needed to calculate filter values.
+        freeBuffers();  // free memory.
         return false;
     }
 
     m_enable = true;
     m_order = order;
+    m_lastOut = &m_outValues[m_order];
     return true;
 }
 
+// frees dynamically alocated buffers and sets filter to bypass mode.
 void LowPassFilter::freeBuffers()
 {
     if (m_inValues) {
@@ -104,14 +114,15 @@
     }
     m_order = 0;
     m_enable = false;
+    m_lastOut =&m_lastIn;
 }
 
 bool LowPassFilter::createBuffers(int order)
 {
-    m_inValues = (float*)malloc((m_order+2)*sizeof(float));
-    m_outValues = (float*)malloc((m_order+2)*sizeof(float));
-    m_inScale = (float*)malloc((m_order+2)*sizeof(float));
-    m_outScale = (float*)malloc((m_order+2)*sizeof(float));
+    m_inValues = (float*)malloc((order+1)*sizeof(float));
+    m_outValues = (float*)malloc((order+1)*sizeof(float));
+    m_inScale = (float*)malloc((order+1)*sizeof(float));
+    m_outScale = (float*)malloc((order+1)*sizeof(float));
     if (!m_outScale) {
         freeBuffers();
         return false;
@@ -131,8 +142,8 @@
 
 float LowPassFilter::addPoint(float input)
 {
-    if (!m_enable) {
-        m_lastIn = input;
+    m_lastIn = input;
+    if (m_enable) {
         return input;
     }
 
@@ -140,7 +151,7 @@
         m_inValues[i] = m_inValues[i + 1];
         m_outValues[i] = m_outValues[i + 1];
     }
-    m_inValues[m_order] = input / m_gain;
+    m_inValues[m_order] = input;
     m_outValues[m_order] = 0;
 
     for (int i = 0; i < m_order + 1; i++) {
@@ -149,9 +160,5 @@
     for (int i = 0; i < m_order; i++) {
         m_outValues[m_order] += m_outValues[i] * m_outScale[i];
     }
-    if(isnan(m_outValues[m_order])) {
-        pc.putc('n');
-        return 0;
-    }
     return m_outValues[m_order];
 }
\ No newline at end of file

--- a/Filter/LowPassFilter.h	Thu Aug 05 13:44:45 2021 +0000
+++ b/Filter/LowPassFilter.h	Fri Aug 06 09:15:41 2021 +0000
@@ -1,4 +1,10 @@
 #pragma once
+/*
+* Butterworth low pass filter with aribitaty parameters.
+* Could be made faster by using arm DSP libraries for filters with an even order.
+* Libraries impliment the same basic structure but in a more efficent way.
+*
+*/
 
 class LowPassFilter
 {
@@ -6,16 +12,14 @@
     LowPassFilter();
     LowPassFilter(int order, float frequency, float sampleRate);
     ~LowPassFilter();
+    
     float addPoint(float point);
+    float lastValue() { return *m_lastOut;  };
     bool makeFilter(int order, float frequency, float sampleRate);
     bool isEnabled()
     {
         return m_enable;
     };
-    float lastValue()
-    {
-        return m_enable?m_outValues[m_order]:m_lastIn;
-    };
 private:
 
     void freeBuffers();
@@ -26,7 +30,7 @@
     float *m_inScale;
     float *m_outScale;
     int m_order;
-    float m_gain;
     bool m_enable;
     float m_lastIn;
+    float *m_lastOut;
 };