Revision 34:c864a0c67dbf, committed 2021-08-04

Comitter:

AndyA

Date:

Wed Aug 04 16:43:45 2021 +0000

Parent:

32:bfb385d35097

Child:

35:7ecf25d9c414

Commit message:

Paranoia commit - added low pass filter creation library.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter/LowPassFilter.cpp	Wed Aug 04 16:43:45 2021 +0000
@@ -0,0 +1,99 @@
+
+/*    public:
+        LowPassFilter(int order, float frequency, float sampleRate);
+        float addPoint(float point);
+    private:
+        float *m_inValues;
+        float *m_outValues;
+        float *m_inScale;
+        float *m_outScale;
+        int m_order;
+        float m_gain;
+        bool m_enable;
+    }*/
+
+#include "LowPassFilter.h"
+#include "makeFilter.h"
+
+LowPassFilter::LowPassFilter(int order, float frequency, float sampleRate)
+{
+//    m_first = true;
+    m_enable = false;
+    if (order <= 0)
+        return;
+
+    if (order > MakeFilterName::MAXORDER)
+        order = MakeFilterName::MAXORDER;
+
+    m_order = order;
+    m_inValues = (float*)malloc((m_order+1)*sizeof(float));
+    if (!m_inValues)
+        return;
+
+    m_outValues = (float*)malloc((m_order+1)*sizeof(float));
+    if (!m_outValues) {
+        free(m_inValues);
+        return;
+    }
+    m_inScale = (float*)malloc((m_order+1)*sizeof(float));
+    if (!m_inScale) {
+        free(m_inValues);
+        free(m_outValues);
+        return;
+    }
+    m_outScale = (float*)malloc((m_order+1)*sizeof(float));
+    if (!m_outValues) {
+        free(m_inValues);
+        free(m_outValues);
+        free(m_inScale);
+        return;
+    }
+
+    MakeFilter *newFilter = new MakeFilter(order, frequency, sampleRate);
+    if (newFilter) {
+        m_enable = true;
+        m_gain = newFilter->getGain();
+        for (int i=0; i<(m_order+1); i++) {
+            m_inScale[i] = newFilter->getXCoeff()[i];
+            m_outScale[i] = newFilter->getYCoeff()[i];
+            m_inValues[i] = 0;
+            m_outValues[i] = 0;
+        }
+        delete newFilter;
+    } else {
+        free(m_inValues);
+        free(m_outValues);
+        free(m_inScale);
+        free(m_outScale);
+    }
+}
+
+LowPassFilter::~LowPassFilter()
+{
+    if (m_enable) {
+        free(m_inValues);
+        free(m_outValues);
+        free(m_inScale);
+        free(m_outScale);
+    }
+}
+
+float LowPassFilter::addPoint(float input)
+{
+    if (!m_enable)
+        return input;
+
+    for (int i = 0; i < m_order; i++) {
+        m_inValues[i] = m_inValues[i + 1];
+        m_outValues[i] = m_outValues[i + 1];
+    }
+    m_inValues[m_order] = input / m_gain;
+    m_outValues[m_order] = 0;
+    for (int i = 0; i < m_order + 1; i++) {
+        m_outValues[m_order] += m_inValues[i] * m_inScale[i];
+    }
+    for (int i = 0; i < m_order; i++) {
+        m_outValues[m_order] += m_outValues[i] * m_outScale[i];
+    }
+    return m_outValues[m_order];
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter/LowPassFilter.h	Wed Aug 04 16:43:45 2021 +0000
@@ -0,0 +1,17 @@
+#pragma once
+
+class LowPassFilter {
+    public:
+        LowPassFilter(int order, float frequency, float sampleRate);
+        ~LowPassFilter();
+        float addPoint(float point);
+    private:
+        float *m_inValues;
+        float *m_outValues;
+        float *m_inScale;
+        float *m_outScale;
+        int m_order;
+        float m_gain;
+        bool m_enable;
+//        bool m_first;
+    };
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter/makeFilter.cpp	Wed Aug 04 16:43:45 2021 +0000
@@ -0,0 +1,150 @@
+/**
+******************************************************************************
+* @file    makeFilter.cpp
+* @author  Hemant Kapoor
+* @date    18-July-2019
+* @brief   Make filter class
+******************************************************************************
+**/
+#include <math.h>
+#include "./makeFilter.h"
+
+#define PI               3.14159265358979f
+using namespace MakeFilterName;
+
+MakeFilter::MakeFilter(int ord, float freq, float sampleFreq)
+{
+    m_order = ord;
+    m_rawAlpha1 = freq / sampleFreq;
+    m_poleMask = 0xffffffff;
+    m_rawAlpha2 = m_rawAlpha1;
+    compute_s();
+    normalize();
+    compute_z();
+    expandpoly();   
+}
+
+
+void MakeFilter::compute_s() 
+{ /* compute S-plane poles for prototype LP filter */
+    m_numPoles = 0;
+    int i;
+    for (i = 0; i < 2 * m_order; i++) 
+    {
+        double img;
+        if(m_order == 1)
+        {
+            img = (i  * PI) / m_order;
+        }
+        else
+        {
+            img = ((i + 0.5) * PI) / m_order;
+        }
+        //complexStruct s = {exp(0.0),exp(img)};
+        complexStruct s = {0.0,img};
+        complexStruct expS = s.Exp();
+        choosepole(expS);
+    }
+}
+
+void MakeFilter::choosepole(complexStruct z) 
+{
+    if (z.Real < 0.0) 
+    {
+        if ((m_poleMask & 0x01) == 0x01)
+            m_spoles[m_numPoles++] = z;
+        m_poleMask >>= 1;
+    }
+}
+
+void MakeFilter::normalize() 
+{
+    int i;
+    m_warpedAlpha1 = tan(PI * m_rawAlpha1) / PI;
+    m_warpedAlpha2 = tan(PI * m_rawAlpha2) / PI;
+    complexStruct w1 = {PI * 2 * m_warpedAlpha1, 0};
+    //complexStruct w2 = {PI * 2 * m_warpedAlpha2, 0};
+    for (i = 0; i < m_numPoles; i++)
+    {
+        m_spoles[i] = m_spoles[i] * w1;
+    }
+}
+
+void MakeFilter::compute_z() /* given S-plane poles, compute Z-plane poles */
+{
+    int i;
+    for (i = 0; i < m_numPoles; i++) 
+    { /* use bilinear transform */
+        complexStruct top, bot;
+        top = m_ctwo + m_spoles[i];
+        bot = m_ctwo - m_spoles[i];
+        m_zPoles[i] = top / bot;
+        m_zZeros[i] = m_cmone;
+    }
+}
+
+
+void MakeFilter::expandpoly() /* given Z-plane poles & zeros, compute top & bot polynomials in Z, and then recurrence relation */
+{
+    complexStruct topcoeffs[MAXPOLES + 1] = {0};
+    complexStruct botcoeffs[MAXPOLES + 1] = {0};
+    complexStruct zfc;
+    int i;
+
+    expand(m_zZeros, topcoeffs);
+    expand(m_zPoles, botcoeffs);
+    m_dcGain = evaluate(topcoeffs, botcoeffs, m_numPoles, m_cone);
+    complexStruct st{0.0, PI * (m_rawAlpha1 + m_rawAlpha2)}; /* "jwT" for centre freq. */
+    zfc = st.Exp();
+    m_fcGain = evaluate(topcoeffs, botcoeffs, m_numPoles, zfc);
+    m_hfGain = evaluate(topcoeffs, botcoeffs, m_numPoles, m_cmone);
+    for (i = 0; i <= m_numPoles; i++) 
+    {
+        m_xcoeffs[i] = topcoeffs[i].Real / botcoeffs[m_numPoles].Real;
+        m_ycoeffs[i] = -(botcoeffs[i].Real / botcoeffs[m_numPoles].Real);
+    }
+}
+
+void MakeFilter::expand(complexStruct pz[], complexStruct coeffs[]) 
+{ /* compute product of poles or zeros as a polynomial of z */
+    int i;
+    coeffs[0] = m_cone;
+    for (i = 0; i < m_numPoles; i++)
+        coeffs[i + 1] = m_czero;
+    for (i = 0; i < m_numPoles; i++)
+        multin(pz[i], coeffs);
+    #ifdef NEVER
+    /* check computed coeffs of z^k are all real */
+    for (i = 0; i < m_numPoles + 1; i++) 
+    {
+        if (Math.Abs(coeffs[i].Imaginary) > 1e-10) 
+        {
+            MessageBox.Show("mkfilter: coeff of z^" + i.ToString() + " is not real; poles are not complex conjugates\n");
+        }
+    }
+    #endif
+}
+
+complexStruct MakeFilter::evaluate(complexStruct topco[], complexStruct botco[], int np, complexStruct z) 
+{ /* evaluate response, substituting for z */
+        return (eval(topco, np, z) / eval(botco, np, z));
+}
+
+complexStruct MakeFilter::eval(complexStruct coeffs[], int np, complexStruct z)
+{ /* evaluate polynomial in z, substituting for z */
+    complexStruct sum;
+    int i;
+    sum = m_czero;
+    for (i = np; i >= 0; i--)
+        sum = (sum * z) + coeffs[i];
+    return sum;
+}
+
+void MakeFilter::multin(complexStruct w, complexStruct coeffs[]) 
+{ /* multiply factor (z-w) into coeffs */
+    complexStruct nw; int i;
+    nw = w.Negate();
+    for (i = m_numPoles; i >= 1; i--)
+        coeffs[i] = (nw * coeffs[i]) + coeffs[i - 1];
+    coeffs[0] = nw * coeffs[0];
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter/makeFilter.h	Wed Aug 04 16:43:45 2021 +0000
@@ -0,0 +1,127 @@
+/**
+******************************************************************************
+* @file    makeFilter.cpp
+* @author  Hemant Kapoor
+* @date    18-July-2019
+* @brief   Make filter class
+******************************************************************************
+**/
+
+#pragma once
+#define _USE_MATH_DEFINES
+#include "math.h"
+#include "mbed.h"
+
+namespace MakeFilterName
+{
+    const double FPKEXP709 = 0x2be5dc9b7fdd422d;
+    struct complexStruct
+    {
+        double Real;
+        double Img;
+        //complexStruct(double r,double  i):Real(r),Img(i){}
+        complexStruct operator +(complexStruct number2)const { complexStruct x = {(Real + number2.Real),(Img + number2.Img)}; return x; }  
+        complexStruct operator -(complexStruct number2)const { complexStruct x = {(Real - number2.Real),(Img - number2.Img)}; return x; }  
+        complexStruct operator *(complexStruct number2)const { complexStruct x = {(Real * number2.Real - Img * number2.Img),(Real * number2.Img + Img * number2.Real)}; return x; } 
+        complexStruct operator /(complexStruct number2)const { 
+                                                                                                                        complexStruct x = {
+                                                                                                                                                                ((Real*number2.Real + Img*number2.Img)/(number2.Real*number2.Real + number2.Img*number2.Img)),
+                                                                                                                                                                ((Img*number2.Real - Real*number2.Img)/(number2.Real*number2.Real + number2.Img*number2.Img))}; 
+                                                                                                                                                                return x;}
+        
+                                                                                                                                                                
+                                                                                                                                                                
+     complexStruct Exp() const {
+   double sinval, cosval, expval, exparg;
+   complexStruct w;
+   
+   sinval = sin(Img);                /* sine of imaginary part of argument */
+   cosval = cos(Img);                /* cosine of imaginary part of argument */
+   
+   if (Real <= 709.0) {               /* exp(Real(z)) is finite */
+      expval = exp(Real);             /* evaluate exponential */
+      w.Real = cosval*expval;           /* real result = cos(Imag(z))*exp(Real(z)) */
+      w.Img = sinval*expval;           /* imag result = sin(Imag(z))*exp(Real(z)) */
+   }
+   
+   else if (fpclassify(Real) < FP_ZERO) {      /* Real(z) = +INF or a NaN */
+      w.Real = cosval*Real;             /* deserved invalid may occur */
+      w.Img = sinval*Real;             /* deserved invalid may occur */
+   }
+   
+   else if (Real > 1460.0) {          /* probable overflow case */
+      w.Real = scalbn(cosval,2100);
+      w.Img  = scalbn(sinval,2100);
+   }
+   else {           /* exp(Real(z)) overflows but product with sin or cos may not */
+      w.Real = cosval*FPKEXP709;   /* initialize real result */
+      w.Img = sinval*FPKEXP709;   /* initialize imag result */
+      exparg = Real - 709.0;          /* initialize reduced exponent argument */
+      while (exparg > 709.0) {        /* exponential reduction loop */
+         w.Real *= FPKEXP709;
+         w.Img *= FPKEXP709;
+         exparg -= 709.0;
+      }
+      expval = exp(exparg);           /* final exponential value */
+      w.Real *= expval;                 /* final multiplication steps */
+      w.Img *= expval;
+   }
+   return w;                          /* done */            
+        }           //{complexStruct x = {exp(Real),exp(Img)}; return x; }
+        
+
+
+  complexStruct Negate() const {complexStruct x = {-Real,-Img}; return x; }
+    };
+    
+    const int MAXORDER = 10;
+    const int MAXPOLES = 2 * MAXORDER;
+
+    const complexStruct m_cmone = {-1, 0.0};
+    const complexStruct m_czero = {0.0, 0.0};
+    const complexStruct m_cone = {1.0, 0.0};
+    const complexStruct m_ctwo = {2.0, 0.0};
+    const complexStruct m_chalf = {0.5, 0.0};
+    
+};
+
+
+class MakeFilter
+{
+public:     
+    MakeFilter(int ord, float freq, float sampleFreq);
+//    ~MakeFilter();
+    double getGain() const{ return m_dcGain.Real; }
+    double* getXCoeff() { return m_xcoeffs; }
+    double* getYCoeff() { return m_ycoeffs; }
+
+    
+private:
+    int m_order, m_numPoles;
+    double m_rawAlpha1, m_rawAlpha2;
+    MakeFilterName::complexStruct m_dcGain, m_fcGain, m_hfGain;
+    uint32_t m_opts;   /* option flag bits */
+
+    double m_warpedAlpha1, m_warpedAlpha2, m_chebrip;
+    uint32_t m_poleMask;
+    bool m_optsok;  
+
+
+    MakeFilterName::complexStruct m_spoles[MakeFilterName::MAXPOLES];
+    MakeFilterName::complexStruct m_zPoles[MakeFilterName::MAXPOLES];
+    MakeFilterName::complexStruct m_zZeros[MakeFilterName::MAXPOLES];
+
+    double m_xcoeffs[MakeFilterName::MAXPOLES + 1];
+    double m_ycoeffs[MakeFilterName::MAXPOLES + 1];
+    
+    void compute_s();
+    void normalize();
+    void compute_z();
+    void expandpoly();
+    void choosepole(MakeFilterName::complexStruct z);
+    void expand(MakeFilterName::complexStruct pz[], MakeFilterName::complexStruct coeffs[]);
+    void multin(MakeFilterName::complexStruct w, MakeFilterName::complexStruct coeffs[]);
+    MakeFilterName::complexStruct evaluate(MakeFilterName::complexStruct topco[], MakeFilterName::complexStruct botco[], int np, MakeFilterName::complexStruct z) ;
+    MakeFilterName::complexStruct eval(MakeFilterName::complexStruct coeffs[], int np, MakeFilterName::complexStruct z);
+};
+

--- a/main.cpp	Tue Aug 03 14:33:40 2021 +0000
+++ b/main.cpp	Wed Aug 04 16:43:45 2021 +0000
@@ -30,6 +30,27 @@
     Set the IPv4 address to use for the ethernet interface.
     All 3 values must be set for a static address to be used otherwise DHCP will be used.
 
+
+FilterOrder=n
+FilterFreq=m.mm
+FilterRate=r.rr
+Low pass filter settings for channels that have it enabled.
+Filter is of order n with a cut off at m Hz assuming input data rate is at r Hz
+Filter order must be set to enable filters.
+Frequency default is 10Hz
+Rate default is 100Hz
+
+FilterXY=1
+FilterZ=1
+FilterRoll=1
+FilterPitch=1
+FilterYaw=1
+Enable channels to low pass filter. All filters use the settings given above.
+A value of 1 enables the filter. A value of 0 or skipping the line disables the filter.
+
+NOTE-The filter will add latency so a filtered channel will be delayed relative to an unfiltered one.
+
+
 All settings are case sensitive.
 Do NOT include spaces in the options lines.
 All options default to a value of 0 is omitted from the file.
@@ -146,6 +167,14 @@
     char IPAddress[32];
     char Gateway[32];
     char Subnet[32];
+    int FilterOrder;
+    float FilterFreq;
+    float FilterRate;
+    bool FilterXY;
+    bool FilterZ;
+    bool FilterRoll;
+    bool FilterPitch;
+    bool FilterYaw;
 } UserSettings_t;
 
 UserSettings_t UserSettings;
@@ -562,10 +591,27 @@
 
 void readSettingsFile()
 {
+
+    UserSettings.FIZmode = formatPreston;
+    UserSettings.SerialOutMode = mode_VIPS;
+    UserSettings.UDPPort = 0;
+    UserSettings.IPAddress[0] = 0;
+    UserSettings.Gateway[0] = 0;
+    UserSettings.Subnet[0] = 0;
+    UserSettings.FilterOrder = 0;
+    UserSettings.FilterFreq = 10;
+    UserSettings.FilterRate = 100;
+    UserSettings.FilterXY = false;
+    UserSettings.FilterZ = false;
+    UserSettings.FilterRoll = false;
+    UserSettings.FilterPitch = false;
+    UserSettings.FilterYaw = false;
+
     LocalFileSystem localFS("local");
     FILE *LSFile= fopen("/local/settings.txt","r");
     char lineBuffer[64];
     int valueIn;
+    float floatIn;
     if (LSFile) {
         while (!feof(LSFile)) {
             if (fgets(lineBuffer, 64, LSFile)) {
@@ -590,6 +636,38 @@
                 if (sscanf(lineBuffer,"Gateway=%s",UserSettings.Gateway) == 1) {
                     pc.printf("Got gateway value from file of %s\r\n",UserSettings.Gateway);
                 }
+                if (sscanf(lineBuffer,"FilterOrder=%d",&valueIn) == 1) {
+                    pc.printf("Got FilterOrder value from file of %d\r\n",valueIn);
+                    UserSettings.FilterOrder = valueIn;
+                }
+                if (sscanf(lineBuffer,"FilterFreq=%f",&floatIn) == 1) {
+                    pc.printf("Got FilterFreq value from file of %.2f\r\n",floatIn);
+                    UserSettings.FilterFreq = floatIn;
+                }
+                if (sscanf(lineBuffer,"FilterRate=%f",&floatIn) == 1) {
+                    pc.printf("Got FilterRate value from file of %.2f\r\n",floatIn);
+                    UserSettings.FilterRate = floatIn;
+                }
+                if (sscanf(lineBuffer,"FilterXY=%d",&valueIn) == 1) {
+                    pc.printf("Got FilterXY value from file of %d\r\n",valueIn);
+                    UserSettings.FilterXY = (valueIn==1);
+                }
+                if (sscanf(lineBuffer,"FilterZ=%d",&valueIn) == 1) {
+                    pc.printf("Got FilterZ value from file of %d\r\n",valueIn);
+                    UserSettings.FilterZ = (valueIn==1);
+                }
+                if (sscanf(lineBuffer,"FilterRoll=%d",&valueIn) == 1) {
+                    pc.printf("Got FilterRoll value from file of %d\r\n",valueIn);
+                    UserSettings.FilterRoll = (valueIn==1);
+                }
+                if (sscanf(lineBuffer,"FilterPitch=%d",&valueIn) == 1) {
+                    pc.printf("Got FilterPitch value from file of %d\r\n",valueIn);
+                    UserSettings.FilterPitch = (valueIn==1);
+                }
+                if (sscanf(lineBuffer,"FilterYaw=%d",&valueIn) == 1) {
+                    pc.printf("Got FilterYaw value from file of %d\r\n",valueIn);
+                    UserSettings.FilterYaw = (valueIn==1);
+                }
             }
         }
         fclose(LSFile);
@@ -678,13 +756,6 @@
     pc.printf("\r\n\r\nStartup - v0.9\r\n");
     setRED();
 
-    UserSettings.FIZmode = formatPreston;
-    UserSettings.SerialOutMode = mode_VIPS;
-    UserSettings.UDPPort = 0;
-    UserSettings.IPAddress[0] = 0;
-    UserSettings.Gateway[0] = 0;
-    UserSettings.Subnet[0] = 0;
-
     readSettingsFile();
 
     switch(UserSettings.FIZmode) {