I messed up the merge, so pushing it over to another repo so I don't lose it. Will tidy up and remove later
Dependencies: BufferedSerial FatFileSystemCpp mbed
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 }
Generated on Thu Dec 15 2022 06:07:04 by 1.7.2