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
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");