diff --git a/FM.cpp b/FM.cpp index d94c5a4..019e27e 100644 --- a/FM.cpp +++ b/FM.cpp @@ -20,18 +20,7 @@ #include "Globals.h" #include "FM.h" -q15_t FILTER_COEFFS[] = { - -630, -842, -846, -634, -312, -53, -14, -251, -683, -1113, -1322, -1179, -718, -147, 234, 172, - -399, -1298, -2124, -2402, -1783, -201, 2051, 4399, 6169, 6827, 6169, 4399, 2051, -201, -1783, -2402, - -2124, -1298, -399, 172, 234, -147, -718, -1179, -1322, -1113, -683, -251, -14, -53, -312, -634, - -846, -842, -630}; - -const uint16_t FILTER_COEFFS_LEN = 51U; - - CFM::CFM() : -m_filterBuffer(NULL), -m_filterPosition(0U), m_callsign(), m_rfAck(), m_ctcssRX(), @@ -46,9 +35,11 @@ m_holdoffTimer(), m_kerchunkTimer(), m_ackMinTimer(), m_ackDelayTimer(), -m_hangTimer() +m_hangTimer(), +m_filterStage1( 724, 1448, 724, 32768, -37895, 21352), +m_filterStage2(32768, 0,-32768, 32768, -50339, 19052), +m_filterStage3(32768, -65536, 32768, 32768, -64075, 31460) { - m_filterBuffer = new q15_t[FILTER_COEFFS_LEN]; } void CFM::samples(q15_t* samples, uint8_t length) @@ -98,7 +89,7 @@ void CFM::samples(q15_t* samples, uint8_t length) if (!m_callsign.isRunning() && !m_rfAck.isRunning()) currentSample += m_timeoutTone.getAudio(); - currentSample = filter(currentSample); + currentSample = q15_t(m_filterStage3.filter(m_filterStage2.filter(m_filterStage1.filter(currentSample)))); currentSample += m_ctcssTX.getAudio(); @@ -394,26 +385,3 @@ void CFM::beginRelaying() m_timeoutTimer.start(); m_ackMinTimer.start(); } - -q15_t CFM::filter(q15_t sample) -{ - q15_t output = 0; - - m_filterBuffer[m_filterPosition] = sample; - - uint8_t iTaps = 0U; - - for (int8_t i = m_filterPosition; i >= 0; i--) { - q31_t temp = FILTER_COEFFS[iTaps++] * m_filterBuffer[i]; - output += q15_t(__SSAT((temp >> 15), 16)); - } - - for (int8_t i = FILTER_COEFFS_LEN - 1; i >= m_filterPosition; i--) { - q31_t temp = FILTER_COEFFS[iTaps++] * m_filterBuffer[i]; - output += q15_t(__SSAT((temp >> 15), 16)); - } - - m_filterPosition = (m_filterPosition + 1U) % FILTER_COEFFS_LEN; - - return output; -} diff --git a/FM.h b/FM.h index d2f9419..77a3d81 100644 --- a/FM.h +++ b/FM.h @@ -26,6 +26,7 @@ #include "FMTimeout.h" #include "FMKeyer.h" #include "FMTimer.h" +#include "FMDirectForm1.h" enum FM_STATE { FS_LISTENING, @@ -37,6 +38,9 @@ enum FM_STATE { FS_HANG }; + + + class CFM { public: CFM(); @@ -52,8 +56,6 @@ public: uint8_t setMisc(uint16_t timeout, uint8_t timeoutLevel, uint8_t ctcssFrequency, uint8_t ctcssThreshold, uint8_t ctcssLevel, uint8_t kerchunkTime, uint8_t hangTime); private: - q15_t* m_filterBuffer; - uint8_t m_filterPosition; CFMKeyer m_callsign; CFMKeyer m_rfAck; CFMCTCSSRX m_ctcssRX; @@ -69,6 +71,9 @@ private: CFMTimer m_ackMinTimer; CFMTimer m_ackDelayTimer; CFMTimer m_hangTimer; + CFMDirectFormI m_filterStage1; + CFMDirectFormI m_filterStage2; + CFMDirectFormI m_filterStage3; void stateMachine(bool validSignal, uint8_t length); void listeningState(bool validSignal); diff --git a/FMCTCSSRX.cpp b/FMCTCSSRX.cpp index 5c36f66..6240f34 100644 --- a/FMCTCSSRX.cpp +++ b/FMCTCSSRX.cpp @@ -111,6 +111,8 @@ CTCSSState CFMCTCSSRX::process(q15_t sample) { m_result = m_result & (~CTS_READY); + q31_t samp = q31_t(sample); + q31_t q2 = m_q1; m_q1 = m_q0; diff --git a/FMDirectForm1.h b/FMDirectForm1.h new file mode 100644 index 0000000..4b90245 --- /dev/null +++ b/FMDirectForm1.h @@ -0,0 +1,111 @@ + +/******************************************************************************* +This header file has been taken from: +"A Collection of Useful C++ Classes for Digital Signal Processing" +By Vinnie Falco +Bernd Porr adapted it for Linux and turned it into a filter using +fixed point arithmetic. +-------------------------------------------------------------------------------- +License: MIT License (http://www.opensource.org/licenses/mit-license.php) +Copyright (c) 2009 by Vinnie Falco +Copyright (C) 2013-2017, Bernd Porr, mail@berndporr.me.uk +Copyright (C) 2020 , Mario Molitor , mario_molitor@web.de +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*******************************************************************************/ + +// based on https://raw.githubusercontent.com/berndporr/iir_fixed_point/master/DirectFormI.h + +#include "Globals.h" + +#ifndef DIRECTFORMI_H_ +#define DIRECTFORMI_H_ +class CFMDirectFormI +{ +public: + + // convenience function which takes the a0 argument but ignores it! + CFMDirectFormI(const q31_t b0, const q31_t b1, const q31_t b2, + const q31_t, const q31_t a1, const q31_t a2) + { + // FIR coefficients + c_b0 = b0; + c_b1 = b1; + c_b2 = b2; + // IIR coefficients + c_a1 = a1; + c_a2 = a2; + reset(); + } + + CFMDirectFormI(const CFMDirectFormI &my) + { + // delay line + m_x2 = my.m_x2; // x[n-2] + m_y2 = my.m_y2; // y[n-2] + m_x1 = my.m_x1; // x[n-1] + m_y1 = my.m_y1; // y[n-1] + + // coefficients + c_b0 = my.c_b0; + c_b1 = my.c_b1; + c_b2 = my.c_b2; // FIR + c_a1 = my.c_a1; + c_a2 = my.c_a2; // IIR + } + + void reset () + { + m_x1 = 0; + m_x2 = 0; + m_y1 = 0; + m_y2 = 0; + } + + // filtering operation: one sample in and one out + inline q15_t filter(const q15_t in) + { + // calculate the output + register q31_t out_upscaled = c_b0 * in //F4FXL puting stauration here made everything quiet, not sure why + + c_b1 * m_x1 + + c_b2 * m_x2 + - c_a1 * m_y1 + - c_a2 * m_y2; + + q15_t out = __SSAT(out_upscaled >> 15, 15); + + // update the delay lines + m_x2 = m_x1; + m_y2 = m_y1; + m_x1 = in; + m_y1 = out; + + return out; + } + +private: + // delay line + q31_t m_x2; // x[n-2] + q31_t m_y2; // y[n-2] + q31_t m_x1; // x[n-1] + q31_t m_y1; // y[n-1] + + // coefficients + q31_t c_b0,c_b1,c_b2; // FIR + q31_t c_a1,c_a2; // IIR +}; + +#endif /* DIRECTFORMI_H */ \ No newline at end of file diff --git a/FMGenerateFilterCoefficients.py b/FMGenerateFilterCoefficients.py new file mode 100644 index 0000000..84ea62b --- /dev/null +++ b/FMGenerateFilterCoefficients.py @@ -0,0 +1,45 @@ +# based on https://github.com/berndporr/iir_fixed_point/blob/master/gen_coeff.py + +import numpy as np +import scipy.signal as signal +import pylab as pl + +# Calculate the coefficients for a pure fixed point +# integer filter + +# sampling rate +fs = 24000 + +# cutoffs +f1 = 300 +f2 = 2700 +# ripple +rp = 0.2 + +# scaling factor in bits, do not change ! +q = 15 +# scaling factor as facor... +scaling_factor = 2**q + +# let's generate a sequence of 2nd order IIR filters +#sos = signal.butter(2,[f1/fs*2,f2/fs*2],'pass',output='sos') +sos = signal.cheby1(3,rp,[f1/fs*2,f2/fs*2],'bandpass', output='sos') + +sos = np.round((sos) * scaling_factor) + +# print coefficients +for biquad in sos: + for coeff in biquad: + print(int(coeff),",",sep="",end="") + #print((coeff),",",sep="",end="") + print("") + +# plot the frequency response +b,a = signal.sos2tf(sos) +w,h = signal.freqz(b,a) +pl.plot(w/np.pi/2*fs,20*np.log(np.abs(h))) +pl.xlabel('frequency/Hz'); +pl.ylabel('gain/dB'); +pl.ylim(top=1,bottom=-20); +pl.xlim(left=250, right=12000); +pl.show() \ No newline at end of file