Revision 35:7ecf25d9c414, committed 2021-08-05

Comitter:

AndyA

Date:

Thu Aug 05 13:44:45 2021 +0000

Parent:

34:c864a0c67dbf

Child:

36:fff2a71bbf86

Child:

37:fc1104eeb6ca

Commit message:

filter working

Changed in this revision

--- a/Filter/LowPassFilter.cpp	Wed Aug 04 16:43:45 2021 +0000
+++ b/Filter/LowPassFilter.cpp	Thu Aug 05 13:44:45 2021 +0000
@@ -1,59 +1,73 @@
 
-/*    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;
-    }*/
+/*
+//To use ARM DSP blocks
+
+    CMSIS_Stages = order/2 (order must be even)
+
+    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;
+    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];
+
+    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);
+
+    arm_biquad_cascade_df1_f32(&filterStructure, float* input, float* output, numberOfPoints);
+
+*/
 
 #include "LowPassFilter.h"
 #include "makeFilter.h"
+#include "mbed.h"
+#include "BufferedSerial.h"
+extern BufferedSerial pc;
 
-LowPassFilter::LowPassFilter(int order, float frequency, float sampleRate)
+LowPassFilter::LowPassFilter()
 {
-//    m_first = true;
+    m_inValues = NULL;
+    m_outValues = NULL;
+    m_inScale = NULL;
+    m_outValues = NULL;
     m_enable = false;
-    if (order <= 0)
-        return;
+    m_order = 0;
+    m_lastIn = 0;
+}
 
+bool LowPassFilter::makeFilter(int order, float frequency, float sampleRate)
+{
     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;
+    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;
+        }
     }
-    m_inScale = (float*)malloc((m_order+1)*sizeof(float));
-    if (!m_inScale) {
-        free(m_inValues);
-        free(m_outValues);
-        return;
+    if (order==0) { // order 0 - bypass
+        m_enable = false;
+        m_order = 0;
+        return true;
     }
-    m_outScale = (float*)malloc((m_order+1)*sizeof(float));
-    if (!m_outValues) {
-        free(m_inValues);
-        free(m_outValues);
-        free(m_inScale);
-        return;
-    }
+    // have correct size buffers. order != 0
 
     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++) {
+        for (int i=0; i<(order+1); i++) {
             m_inScale[i] = newFilter->getXCoeff()[i];
             m_outScale[i] = newFilter->getYCoeff()[i];
             m_inValues[i] = 0;
@@ -61,27 +75,66 @@
         }
         delete newFilter;
     } else {
+        freeBuffers();
+        return false;
+    }
+
+    m_enable = true;
+    m_order = order;
+    return true;
+}
+
+void LowPassFilter::freeBuffers()
+{
+    if (m_inValues) {
         free(m_inValues);
+        m_inValues = NULL;
+    }
+    if (m_outValues) {
         free(m_outValues);
+        m_outValues = NULL;
+    }
+    if (m_inScale) {
         free(m_inScale);
+        m_inScale = NULL;
+    }
+    if (m_outScale) {
         free(m_outScale);
+        m_outScale = NULL;
     }
+    m_order = 0;
+    m_enable = false;
+}
+
+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));
+    if (!m_outScale) {
+        freeBuffers();
+        return false;
+    }
+    return true;
+}
+
+LowPassFilter::LowPassFilter(int order, float frequency, float sampleRate)
+{
+    makeFilter(order, frequency, sampleRate);
 }
 
 LowPassFilter::~LowPassFilter()
 {
-    if (m_enable) {
-        free(m_inValues);
-        free(m_outValues);
-        free(m_inScale);
-        free(m_outScale);
-    }
+    freeBuffers();
 }
 
 float LowPassFilter::addPoint(float input)
 {
-    if (!m_enable)
+    if (!m_enable) {
+        m_lastIn = input;
         return input;
+    }
 
     for (int i = 0; i < m_order; i++) {
         m_inValues[i] = m_inValues[i + 1];
@@ -89,11 +142,16 @@
     }
     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];
     }
+    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	Wed Aug 04 16:43:45 2021 +0000
+++ b/Filter/LowPassFilter.h	Thu Aug 05 13:44:45 2021 +0000
@@ -1,17 +1,32 @@
 #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
+class LowPassFilter
+{
+public:
+    LowPassFilter();
+    LowPassFilter(int order, float frequency, float sampleRate);
+    ~LowPassFilter();
+    float addPoint(float point);
+    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();
+    bool createBuffers(int order);
+
+    float *m_inValues;
+    float *m_outValues;
+    float *m_inScale;
+    float *m_outScale;
+    int m_order;
+    float m_gain;
+    bool m_enable;
+    float m_lastIn;
+};

--- a/LTCApp.h	Wed Aug 04 16:43:45 2021 +0000
+++ b/LTCApp.h	Thu Aug 05 13:44:45 2021 +0000
@@ -27,4 +27,21 @@
 
 extern void vipsBypassRx(char byte);
 
+typedef struct UserSettings_s {
+    int FIZmode;
+    int SerialOutMode;
+    int UDPPort;
+    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;
+
 #endif

--- a/VIPSSerialProtocol.cpp	Wed Aug 04 16:43:45 2021 +0000
+++ b/VIPSSerialProtocol.cpp	Thu Aug 05 13:44:45 2021 +0000
@@ -402,13 +402,17 @@
 
 void VIPSSerial::smoothOutputPacket(position *posPtr)
 {
+    xFilter.addPoint(posPtr->X);
+    yFilter.addPoint(posPtr->Y);
+    zFilter.addPoint(posPtr->Height);
+
     if (hyperSmoothEnabled) {
         if (!SmoothRunning) {
             XSmoothTotal = posPtr->X * (SmoothBy - 1);
             YSmoothTotal = posPtr->Y * (SmoothBy - 1);
             ZSmoothTotal = posPtr->Height * (SmoothBy - 1);
             SmoothRunning = true;
-            pc.write("Seeded Filter\r\n",11);            
+            pc.write("Seeded Filter\r\n",11);
         }
         //smooth the KF_X and KF_Y positions
         XSmoothTotal += posPtr->X;
@@ -424,5 +428,54 @@
         ZSmoothTotal -= posPtr->Height;
     } else {
         SmoothRunning = false;
+ //       pc.printf("filterX = %f\r\n",xFilter.lastValue());
+        posPtr->X = xFilter.lastValue();
+        posPtr->Y = yFilter.lastValue();
+        posPtr->Height = zFilter.lastValue();
     }
+    posPtr->roll = rollFilter.addPoint(posPtr->roll);
+    posPtr->pitch = pitchFilter.addPoint(posPtr->pitch);
+    posPtr->yaw = yawFilter.addPoint(posPtr->yaw);
 }
+
+bool VIPSSerial::setFilters(struct UserSettings_s *settings)
+{
+    if (settings->FilterXY) {
+        if (!xFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
+            return false;
+        if (!yFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
+            return false;
+    } else {
+        xFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
+        yFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
+    }
+
+    if (settings->FilterZ) {
+        if (!zFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
+            return false;
+    } else {
+        zFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
+    }
+
+    if (settings->FilterRoll) {
+        if (!rollFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
+            return false;
+    } else {
+        rollFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
+    }
+
+    if (settings->FilterPitch) {
+        if (!pitchFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
+            return false;
+    } else {
+        pitchFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
+    }
+
+    if (settings->FilterYaw) {
+        if (!yawFilter.makeFilter(settings->FilterOrder,settings->FilterFreq,settings->FilterRate))
+            return false;
+    } else {
+        yawFilter.makeFilter(0,settings->FilterFreq,settings->FilterRate);
+    }
+    return true;
+}
\ No newline at end of file

--- a/VIPSSerialProtocol.h	Wed Aug 04 16:43:45 2021 +0000
+++ b/VIPSSerialProtocol.h	Thu Aug 05 13:44:45 2021 +0000
@@ -4,33 +4,62 @@
 #include "mbed.h"
 #include "position.h"
 #include "BufferedSerial.h"
+#include "LTCApp.h"
+#include "LowPassFilter.h"
 
 extern const char* VIPSStatusMessages[];
+struct UserSettings_s;
 
-class VIPSSerial { 
+class VIPSSerial
+{
 
-public:    
+public:
 
     VIPSSerial(const PinName Tx, const PinName Rx);
     void run(void);
+    bool setFilters(struct UserSettings_s *settings);
 
     // send all position outputs rather than just when requested.
     void sendAllUpdated(bool enable);
 
     // send a position output for the requested time. Times are based on the global TimeSinceLastFrame timer.
     position* sendPositionForTime(uint32_t timeValue);
-    position* getWaitingPostion() {position *ptr = outputPtr; outputPtr=NULL; return ptr;}
+    position* getWaitingPostion()
+    {
+        position *ptr = outputPtr;
+        outputPtr=NULL;
+        return ptr;
+    }
 
     static void getCRC(void *data, int len, void *checksum);
 
 //  void setOutMask(uint32_t outputMask) {_outputMask = outputMask;};
 
-    bool EnableSmoothing(bool enabled) { hyperSmoothEnabled = enabled; return hyperSmoothEnabled;};
-    bool SmoothingEnabled(void) { return hyperSmoothEnabled;};
+    bool EnableSmoothing(bool enabled)
+    {
+        hyperSmoothEnabled = enabled;
+        return hyperSmoothEnabled;
+    };
+    bool SmoothingEnabled(void)
+    {
+        return hyperSmoothEnabled;
+    };
 
-    int GetSmoothLevel(void) { return SmoothBy; };
-    bool SetSmoothLevel (const int newSmooth) { if (newSmooth == SmoothBy) return false; SmoothBy = newSmooth; SmoothRunning = false; return true; };
-    void EnableBypass(bool enable) { BypassMode = enable;};
+    int GetSmoothLevel(void)
+    {
+        return SmoothBy;
+    };
+    bool SetSmoothLevel (const int newSmooth)
+    {
+        if (newSmooth == SmoothBy) return false;
+        SmoothBy = newSmooth;
+        SmoothRunning = false;
+        return true;
+    };
+    void EnableBypass(bool enable)
+    {
+        BypassMode = enable;
+    };
     void bypassTx(char byte);
     void sendQueued(void);
 
@@ -55,8 +84,8 @@
     bool checkNewPacketRC(unsigned char* data);
     RawSerial _port;
     unsigned char messageInBuffer[128];
-    unsigned char messageOutBuffer[16];    
-    #define posHistoryLen 3
+    unsigned char messageOutBuffer[16];
+#define posHistoryLen 3
     struct posAndTime_s lastPositions[posHistoryLen];
     int nextPosition;
     struct posAndTime_s lastPos; // the most recent position received
@@ -76,14 +105,18 @@
 
     int queueLen;
     int SmoothBy;
-    // total as a float we would start to see rounding errors at valuses of ~20m 
+    // total as a float we would start to see rounding errors at valuses of ~20m
     double XSmoothTotal;
     double YSmoothTotal;
     double ZSmoothTotal;
     bool SmoothRunning;
     bool BypassMode;
-
-
+    LowPassFilter yFilter;
+    LowPassFilter xFilter;
+    LowPassFilter zFilter;
+    LowPassFilter rollFilter;
+    LowPassFilter pitchFilter;
+    LowPassFilter yawFilter;
 };
 
 #endif

--- a/main.cpp	Wed Aug 04 16:43:45 2021 +0000
+++ b/main.cpp	Thu Aug 05 13:44:45 2021 +0000
@@ -160,23 +160,6 @@
 enum SerialOutput {mode_VIPS, mode_FreeD};
 enum FIZFormats {formatPreston, formatFujiPassive, formatFujiActive, formatCanon};
 
-typedef struct UserSettings_s {
-    int FIZmode;
-    int SerialOutMode;
-    int UDPPort;
-    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;
 
 struct outputFormat_s {
@@ -753,7 +736,7 @@
 int main()
 {
     pc.baud(115200);
-    pc.printf("\r\n\r\nStartup - v0.9\r\n");
+    pc.printf("\r\n\r\nStartup - v0.10\r\n");
     setRED();
 
     readSettingsFile();
@@ -792,6 +775,9 @@
 
     LTCInput.enable(true);
 
+    if (!VIPS.setFilters(&UserSettings))
+        pc.puts("Failed to create VIPS filters\r\n");
+    
     VIPS.run();
 
     pc.printf("System init complete\r\n");