mirror of https://github.com/g4klx/MMDVM.git
Jonathan Naylor
GitHub
commit
e721dedf1e
42
FM.cpp
42
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;
|
||||
}
|
||||
|
|
|
|||
9
FM.h
9
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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
@ -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()
|
||||
Loading…
Reference in New Issue