diff --git a/FM.cpp b/FM.cpp index efe3e05..57afcf0 100644 --- a/FM.cpp +++ b/FM.cpp @@ -68,6 +68,12 @@ m_needReverse(false), m_filterStage1( 724, 1448, 724, 32768, -37895, 21352),//3rd order Cheby Filter 300 to 2700Hz, 0.2dB passband ripple, sampling rate 24kHz m_filterStage2(32768, 0,-32768, 32768, -50339, 19052), m_filterStage3(32768, -65536, 32768, 32768, -64075, 31460), +m_dsFilterStage1(m_filterStage1),//same coefficients, separate state +m_dsFilterStage2(m_filterStage2), +m_dsFilterStage3(m_filterStage3), +m_usFilterStage1(m_filterStage1),//same coefficients, separate state +m_usFilterStage2(m_filterStage2), +m_usFilterStage3(m_filterStage3), m_blanking(), m_accessMode(1U), m_linkMode(false), @@ -122,6 +128,7 @@ void CFM::repeaterSamples(bool cos, q15_t* samples, const uint16_t* rssi, uint8_ q15_t currentExtSample; bool inputExt = m_inputExtRB.getSample(currentExtSample);//always consume the external input data so it does not overflow + currentExtSample = m_usFilterStage3.filter(m_usFilterStage2.filter(m_usFilterStage1.filter(currentExtSample))); inputExt = inputExt && m_extEnabled; switch (m_accessMode) { @@ -203,8 +210,10 @@ void CFM::repeaterSamples(bool cos, q15_t* samples, const uint16_t* rssi, uint8_ if (m_duplex) { if (m_state == FS_RELAYING_RF || m_state == FS_KERCHUNK_RF || m_state == FS_RELAYING_EXT || m_state == FS_KERCHUNK_EXT) { currentSample = m_blanking.process(currentSample); - if (m_extEnabled && (m_state == FS_RELAYING_RF || m_state == FS_KERCHUNK_RF)) - m_downSampler.addSample(currentSample); + if (m_extEnabled && (m_state == FS_RELAYING_RF || m_state == FS_KERCHUNK_RF)) { + q15_t dsSample = m_dsFilterStage3.filter(m_dsFilterStage2.filter(m_dsFilterStage1.filter(currentSample))); + m_downSampler.addSample(dsSample); + } currentSample *= currentBoost; } else { @@ -214,9 +223,11 @@ void CFM::repeaterSamples(bool cos, q15_t* samples, const uint16_t* rssi, uint8_ if (m_state == FS_RELAYING_EXT || m_state == FS_KERCHUNK_EXT) { currentSample *= currentBoost; } else { - if (m_extEnabled && (m_state == FS_RELAYING_RF || m_state == FS_KERCHUNK_RF)) - m_downSampler.addSample(currentSample); - continue; + if (m_extEnabled && (m_state == FS_RELAYING_RF || m_state == FS_KERCHUNK_RF)) { + q15_t dsSample = m_dsFilterStage3.filter(m_dsFilterStage2.filter(m_dsFilterStage1.filter(currentSample))); + m_downSampler.addSample(dsSample); + } + continue; } } @@ -263,6 +274,7 @@ void CFM::linkSamples(bool cos, q15_t* samples, uint8_t length) // few samples past that point q15_t currentExtSample = 0; bool inputExt = m_inputExtRB.getSample(currentExtSample);//always consume the external input data so it does not overflow + currentExtSample = m_usFilterStage3.filter(m_usFilterStage2.filter(m_usFilterStage1.filter(currentExtSample))); inputExt = inputExt && m_extEnabled; switch (m_accessMode) { @@ -332,7 +344,8 @@ void CFM::linkSamples(bool cos, q15_t* samples, uint8_t length) if (m_rfSignal && m_extEnabled) { q15_t currentSample = m_blanking.process(currentRFSample); - m_downSampler.addSample(currentSample); + q15_t dsSample = m_dsFilterStage3.filter(m_dsFilterStage2.filter(m_dsFilterStage1.filter(currentSample))); + m_downSampler.addSample(dsSample); } if (!m_extSignal) @@ -412,6 +425,12 @@ void CFM::reset() m_inputExtRB.reset(); m_downSampler.reset(); + m_dsFilterStage1.reset(); + m_dsFilterStage2.reset(); + m_dsFilterStage3.reset(); + m_usFilterStage1.reset(); + m_usFilterStage2.reset(); + m_usFilterStage3.reset(); m_squelch.reset(); m_needReverse = false; diff --git a/FM.h b/FM.h index a1a198f..29df859 100644 --- a/FM.h +++ b/FM.h @@ -80,6 +80,12 @@ private: CFMDirectFormI m_filterStage1; CFMDirectFormI m_filterStage2; CFMDirectFormI m_filterStage3; + CFMDirectFormI m_dsFilterStage1; // anti-alias filter for the downsampler, own state + CFMDirectFormI m_dsFilterStage2; + CFMDirectFormI m_dsFilterStage3; + CFMDirectFormI m_usFilterStage1; // reconstruction filter for the upsampler, own state + CFMDirectFormI m_usFilterStage2; + CFMDirectFormI m_usFilterStage3; CFMBlanking m_blanking; uint8_t m_accessMode; bool m_linkMode; diff --git a/FMDownSampler.cpp b/FMDownSampler.cpp index 47fb120..04b2c96 100644 --- a/FMDownSampler.cpp +++ b/FMDownSampler.cpp @@ -34,6 +34,7 @@ m_sampleIndex(0U) void CFMDownSampler::addSample(q15_t sample) { + sample = __SSAT(sample, 12);//clamp before packing to 12 bits uint32_t usample = uint32_t(int32_t(sample) + 2048); //only take one of three samples switch(m_sampleIndex){ @@ -74,6 +75,8 @@ uint16_t CFMDownSampler::getData() void CFMDownSampler::reset() { m_sampleIndex = 0U; + m_samplePack = 0U; + m_ringBuffer.reset(); } #endif