LowPassFilter.cpp

00001 
00002 /*
00003 //To use ARM DSP blocks
00004 
00005 // include https://os.mbed.com/users/mbed_official/code/mbed-dsp/ in project
00006 
00007 
00008     #include "arm_math.h"
00009     int CMSIS_Stages = order/2  // (order must be even)
00010     float m_forwardFilterStateArray[4*CMSIS_Stages];
00011     float m_filterCoeff[CMSIS_Stages*4+1];
00012     arm_biquad_casd_df1_inst_f32 filterStructure;
00013 
00014   Filter init for order 2:
00015     m_filterCoeff[0] = xval[0] / gain;
00016     m_filterCoeff[1] = xval[1] / gain;
00017     m_filterCoeff[2] = xval[2] / gain;
00018     m_filterCoeff[3] = yVal[1];
00019     m_filterCoeff[4] = yVal[0];
00020     memset(forwardFilterStateArray, 0, 4 * CMSIS_Stages * sizeof(float32_t));
00021     arm_biquad_cascade_df1_init_f32(&filterStructure, CMSIS_Stages, m_filterCoeff, forwardFilterStateArray);
00022 
00023   To filter data
00024     arm_biquad_cascade_df1_f32(&filterStructure, float* input, float* output, numberOfPoints);
00025 
00026 // see https://www.keil.com/pack/doc/CMSIS/DSP/html/group__BiquadCascadeDF1.html for details of how these work.
00027 
00028 
00029 */
00030 
00031 #include "LowPassFilter.h"
00032 #include "makeFilter.h"
00033 #include "mbed.h"
00034 #include "BufferedSerial.h"
00035 extern BufferedSerial pc;
00036 
00037 LowPassFilter::LowPassFilter()
00038 {
00039     m_inValues = NULL;
00040     m_outValues = NULL;
00041     m_inScale = NULL;
00042     m_outValues = NULL;
00043     m_enable = false;
00044     m_order = 0;
00045     m_lastIn = 0;
00046     m_lastOut =&m_lastIn;
00047 }
00048 
00049 bool LowPassFilter::makeFilter(int order, float frequency, float sampleRate)
00050 {
00051     if (order > MakeFilterName::MAXORDER)
00052         order = MakeFilterName::MAXORDER;
00053 
00054     if (order != m_order) { // change buffer sizes
00055         freeBuffers();      // clear existing buffers
00056         if (order) {        // if need new buffers
00057             if (!createBuffers(order)) {   // create buffers and check for errors
00058                 return false;              // failed to create buffers.
00059             }
00060         }
00061     }
00062 
00063     // buffers are now correct size for order
00064 
00065     if (order==0) { // order 0 - bypass the filter
00066         m_enable = false;
00067         m_order = 0;
00068         m_lastOut =&m_lastIn;
00069         return true;
00070     }
00071 
00072     // have correct size buffers. order != 0
00073 
00074     // calculate filter perameters and copy to local buffers.
00075     MakeFilter *newFilter = new MakeFilter(order, frequency, sampleRate);
00076     if (newFilter) {
00077         double gain = newFilter->getGain();
00078         for (int i=0; i<(order+1); i++) {
00079             m_inScale[i] = newFilter->getXCoeff()[i] / gain;
00080             m_outScale[i] = newFilter->getYCoeff()[i];
00081             m_inValues[i] = 0;
00082             m_outValues[i] = 0;
00083         }
00084         delete newFilter;
00085     } else {            // failed to alocate memory needed to calculate filter values.
00086         freeBuffers();  // free memory.
00087         return false;
00088     }
00089 
00090     m_enable = true;
00091     m_order = order;
00092     m_lastOut = &m_outValues[m_order];
00093     return true;
00094 }
00095 
00096 // frees dynamically alocated buffers and sets filter to bypass mode.
00097 void LowPassFilter::freeBuffers()
00098 {
00099     if (m_inValues) {
00100         free(m_inValues);
00101         m_inValues = NULL;
00102     }
00103     if (m_outValues) {
00104         free(m_outValues);
00105         m_outValues = NULL;
00106     }
00107     if (m_inScale) {
00108         free(m_inScale);
00109         m_inScale = NULL;
00110     }
00111     if (m_outScale) {
00112         free(m_outScale);
00113         m_outScale = NULL;
00114     }
00115     m_order = 0;
00116     m_enable = false;
00117     m_lastOut =&m_lastIn;
00118 }
00119 
00120 bool LowPassFilter::createBuffers(int order)
00121 {
00122     m_inValues = (float*)malloc((order+1)*sizeof(float));
00123     m_outValues = (float*)malloc((order+1)*sizeof(float));
00124     m_inScale = (float*)malloc((order+1)*sizeof(float));
00125     m_outScale = (float*)malloc((order+1)*sizeof(float));
00126     if (!m_outScale) {
00127         freeBuffers();
00128         return false;
00129     }
00130     return true;
00131 }
00132 
00133 LowPassFilter::LowPassFilter(int order, float frequency, float sampleRate)
00134 {
00135     makeFilter(order, frequency, sampleRate);
00136 }
00137 
00138 LowPassFilter::~LowPassFilter()
00139 {
00140     freeBuffers();
00141 }
00142 
00143 float LowPassFilter::addPoint(float input)
00144 {
00145     m_lastIn = input;
00146     if (!m_enable) {
00147         return input;
00148     }
00149 
00150     for (int i = 0; i < m_order; i++) {
00151         m_inValues[i] = m_inValues[i + 1];
00152         m_outValues[i] = m_outValues[i + 1];
00153     }
00154     m_inValues[m_order] = input;
00155     m_outValues[m_order] = 0;
00156 
00157     for (int i = 0; i < m_order + 1; i++) {
00158         m_outValues[m_order] += m_inValues[i] * m_inScale[i];
00159     }
00160     for (int i = 0; i < m_order; i++) {
00161         m_outValues[m_order] += m_outValues[i] * m_outScale[i];
00162     }
00163     return m_outValues[m_order];
00164 }