From 9398d03e0bb73dd611ccd5f9c4ffa6eb9a0479f8 Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Fri, 18 Aug 2017 10:48:23 +0100 Subject: [PATCH] Optimised the DC offset processing. --- DStarRX.cpp | 29 ++--------------------------- DStarRX.h | 2 -- IO.cpp | 41 +++++++++++++++++++++++++++++++++++------ IO.h | 3 +++ P25RX.cpp | 29 ++--------------------------- P25RX.h | 2 -- YSFRX.cpp | 29 ++--------------------------- YSFRX.h | 2 -- 8 files changed, 44 insertions(+), 93 deletions(-) diff --git a/DStarRX.cpp b/DStarRX.cpp index 519c23a..b7fea0c 100644 --- a/DStarRX.cpp +++ b/DStarRX.cpp @@ -233,10 +233,6 @@ const uint16_t CCITT_TABLE[] = { const uint16_t NOENDPTR = 9999U; -// Generated using [b, a] = butter(1, 0.001) in MATLAB -static q31_t DC_FILTER[] = {3367972, 0, 3367972, 0, 2140747704, 0}; // {b0, 0, b1, b2, -a1, -a2} -const uint32_t DC_FILTER_STAGES = 1U; // One Biquad stage - CDStarRX::CDStarRX() : m_rxState(DSRXS_NONE), m_bitBuffer(), @@ -261,16 +257,8 @@ m_pathMemory2(), m_pathMemory3(), m_fecOutput(), m_rssiAccum(0U), -m_rssiCount(0U), -m_dcFilter(), -m_dcState() +m_rssiCount(0U) { - ::memset(m_dcState, 0x00U, 4U * sizeof(q31_t)); - - m_dcFilter.numStages = DC_FILTER_STAGES; - m_dcFilter.pState = m_dcState; - m_dcFilter.pCoeffs = DC_FILTER; - m_dcFilter.postShift = 0; } void CDStarRX::reset() @@ -293,24 +281,11 @@ void CDStarRX::reset() void CDStarRX::samples(q15_t* samples, const uint16_t* rssi, uint8_t length) { - q31_t dcLevel = 0; - q31_t dcVals[20U]; - q31_t q31Samples[20U]; - - ::arm_q15_to_q31(samples, q31Samples, length); - ::arm_biquad_cascade_df1_q31(&m_dcFilter, q31Samples, dcVals, length); - - for (uint8_t i = 0U; i < length; i++) - dcLevel += dcVals[i]; - dcLevel /= length; - - q15_t offset = q15_t(__SSAT((dcLevel >> 16), 16)); - for (uint16_t i = 0U; i < length; i++) { m_rssiAccum += rssi[i]; m_rssiCount++; - q15_t sample = samples[i] - offset; + q15_t sample = samples[i]; m_bitBuffer[m_bitPtr] <<= 1; if (sample < 0) diff --git a/DStarRX.h b/DStarRX.h index ea473bf..4223974 100644 --- a/DStarRX.h +++ b/DStarRX.h @@ -61,8 +61,6 @@ private: uint8_t m_fecOutput[42U]; uint32_t m_rssiAccum; uint16_t m_rssiCount; - arm_biquad_casd_df1_inst_q31 m_dcFilter; - q31_t m_dcState[4]; void processNone(q15_t sample); void processHeader(q15_t sample); diff --git a/IO.cpp b/IO.cpp index 1a65a8e..8b77f85 100644 --- a/IO.cpp +++ b/IO.cpp @@ -22,6 +22,10 @@ #include "Globals.h" #include "IO.h" +// Generated using [b, a] = butter(1, 0.001) in MATLAB +static q31_t DC_FILTER[] = {3367972, 0, 3367972, 0, 2140747704, 0}; // {b0, 0, b1, b2, -a1, -a2} +const uint32_t DC_FILTER_STAGES = 1U; // One Biquad stage + // One symbol boxcar filter static q15_t BOXCAR_FILTER[] = {12000, 12000, 12000, 12000, 12000, 0}; const uint16_t BOXCAR_FILTER_LEN = 6U; @@ -33,6 +37,8 @@ m_started(false), m_rxBuffer(RX_RINGBUFFER_SIZE), m_txBuffer(TX_RINGBUFFER_SIZE), m_rssiBuffer(RX_RINGBUFFER_SIZE), +m_dcFilter(), +m_dcState(), m_boxcarFilter(), m_boxcarState(), m_pttInvert(false), @@ -51,6 +57,12 @@ m_watchdog(0U), m_lockout(false) { ::memset(m_boxcarState, 0x00U, 30U * sizeof(q15_t)); + ::memset(m_dcState, 0x00U, 4U * sizeof(q31_t)); + + m_dcFilter.numStages = DC_FILTER_STAGES; + m_dcFilter.pState = m_dcState; + m_dcFilter.pCoeffs = DC_FILTER; + m_dcFilter.postShift = 0; m_boxcarFilter.numTaps = BOXCAR_FILTER_LEN; m_boxcarFilter.pState = m_boxcarState; @@ -136,12 +148,29 @@ void CIO::process() q15_t vals[RX_BLOCK_SIZE]; ::arm_fir_fast_q15(&m_boxcarFilter, samples, vals, RX_BLOCK_SIZE); + q31_t dcLevel = 0; + q31_t dcValues[RX_BLOCK_SIZE]; + q31_t q31Vals[RX_BLOCK_SIZE]; + + ::arm_q15_to_q31(vals, q31Vals, RX_BLOCK_SIZE); + ::arm_biquad_cascade_df1_q31(&m_dcFilter, q31Vals, dcValues, RX_BLOCK_SIZE); + + for (uint8_t i = 0U; i < RX_BLOCK_SIZE; i++) + dcLevel += dcValues[i]; + dcLevel /= RX_BLOCK_SIZE; + + q15_t offset = q15_t(__SSAT((dcLevel >> 16), 16));; + + q15_t dcVals[RX_BLOCK_SIZE]; + for (uint8_t i = 0U; i < RX_BLOCK_SIZE; i++) + dcVals[i] = vals[i] - offset; + if (m_modemState == STATE_IDLE) { if (m_dstarEnable) - dstarRX.samples(vals, rssi, RX_BLOCK_SIZE); + dstarRX.samples(dcVals, rssi, RX_BLOCK_SIZE); if (m_p25Enable) - p25RX.samples(vals, rssi, RX_BLOCK_SIZE); + p25RX.samples(dcVals, rssi, RX_BLOCK_SIZE); if (m_dmrEnable) { if (m_duplex) @@ -151,10 +180,10 @@ void CIO::process() } if (m_ysfEnable) - ysfRX.samples(vals, rssi, RX_BLOCK_SIZE); + ysfRX.samples(dcVals, rssi, RX_BLOCK_SIZE); } else if (m_modemState == STATE_DSTAR) { if (m_dstarEnable) - dstarRX.samples(vals, rssi, RX_BLOCK_SIZE); + dstarRX.samples(dcVals, rssi, RX_BLOCK_SIZE); } else if (m_modemState == STATE_DMR) { if (m_dmrEnable) { if (m_duplex) { @@ -169,10 +198,10 @@ void CIO::process() } } else if (m_modemState == STATE_YSF) { if (m_ysfEnable) - ysfRX.samples(vals, rssi, RX_BLOCK_SIZE); + ysfRX.samples(dcVals, rssi, RX_BLOCK_SIZE); } else if (m_modemState == STATE_P25) { if (m_p25Enable) - p25RX.samples(vals, rssi, RX_BLOCK_SIZE); + p25RX.samples(dcVals, rssi, RX_BLOCK_SIZE); } else if (m_modemState == STATE_DSTARCAL) { calDStarRX.samples(vals, RX_BLOCK_SIZE); } else if (m_modemState == STATE_RSSICAL) { diff --git a/IO.h b/IO.h index 8498b75..4c0c727 100644 --- a/IO.h +++ b/IO.h @@ -60,6 +60,9 @@ private: CSampleRB m_txBuffer; CRSSIRB m_rssiBuffer; + arm_biquad_casd_df1_inst_q31 m_dcFilter; + q31_t m_dcState[4]; + arm_fir_instance_q15 m_boxcarFilter; q15_t m_boxcarState[30U]; // NoTaps + BlockSize - 1, 6 + 20 - 1 plus some spare diff --git a/P25RX.cpp b/P25RX.cpp index 6c1d0af..6b4cfd4 100644 --- a/P25RX.cpp +++ b/P25RX.cpp @@ -38,10 +38,6 @@ const uint16_t NOENDPTR = 9999U; const unsigned int MAX_SYNC_FRAMES = 4U + 1U; -// Generated using [b, a] = butter(1, 0.001) in MATLAB -static q31_t DC_FILTER[] = {3367972, 0, 3367972, 0, 2140747704, 0}; // {b0, 0, b1, b2, -a1, -a2} -const uint32_t DC_FILTER_STAGES = 1U; // One Biquad stage - CP25RX::CP25RX() : m_state(P25RXS_NONE), m_bitBuffer(), @@ -64,16 +60,8 @@ m_threshold(), m_thresholdVal(0), m_averagePtr(NOAVEPTR), m_rssiAccum(0U), -m_rssiCount(0U), -m_dcFilter(), -m_dcState() +m_rssiCount(0U) { - ::memset(m_dcState, 0x00U, 4U * sizeof(q31_t)); - - m_dcFilter.numStages = DC_FILTER_STAGES; - m_dcFilter.pState = m_dcState; - m_dcFilter.pCoeffs = DC_FILTER; - m_dcFilter.postShift = 0; } void CP25RX::reset() @@ -100,21 +88,8 @@ void CP25RX::reset() void CP25RX::samples(q15_t* samples, uint16_t* rssi, uint8_t length) { - q31_t dcLevel = 0; - q31_t dcVals[20U]; - q31_t q31Samples[20U]; - - ::arm_q15_to_q31(samples, q31Samples, length); - ::arm_biquad_cascade_df1_q31(&m_dcFilter, q31Samples, dcVals, length); - - for (uint8_t i = 0U; i < length; i++) - dcLevel += dcVals[i]; - dcLevel /= length; - - q15_t offset = q15_t(__SSAT((dcLevel >> 16), 16));; - for (uint8_t i = 0U; i < length; i++) { - q15_t sample = samples[i] - offset; + q15_t sample = samples[i]; m_rssiAccum += rssi[i]; m_rssiCount++; diff --git a/P25RX.h b/P25RX.h index 16f6cf3..48af4f9 100644 --- a/P25RX.h +++ b/P25RX.h @@ -59,8 +59,6 @@ private: uint8_t m_averagePtr; uint32_t m_rssiAccum; uint16_t m_rssiCount; - arm_biquad_casd_df1_inst_q31 m_dcFilter; - q31_t m_dcState[4]; void processNone(q15_t sample); void processHdr(q15_t sample); diff --git a/YSFRX.cpp b/YSFRX.cpp index f3be78c..3b156af 100644 --- a/YSFRX.cpp +++ b/YSFRX.cpp @@ -38,10 +38,6 @@ const uint16_t NOENDPTR = 9999U; const unsigned int MAX_SYNC_FRAMES = 4U + 1U; -// Generated using [b, a] = butter(1, 0.001) in MATLAB -static q31_t DC_FILTER[] = {3367972, 0, 3367972, 0, 2140747704, 0}; // {b0, 0, b1, b2, -a1, -a2} -const uint32_t DC_FILTER_STAGES = 1U; // One Biquad stage - CYSFRX::CYSFRX() : m_state(YSFRXS_NONE), m_bitBuffer(), @@ -62,16 +58,8 @@ m_threshold(), m_thresholdVal(0), m_averagePtr(NOAVEPTR), m_rssiAccum(0U), -m_rssiCount(0U), -m_dcFilter(), -m_dcState() +m_rssiCount(0U) { - ::memset(m_dcState, 0x00U, 4U * sizeof(q31_t)); - - m_dcFilter.numStages = DC_FILTER_STAGES; - m_dcFilter.pState = m_dcState; - m_dcFilter.pCoeffs = DC_FILTER; - m_dcFilter.postShift = 0; } void CYSFRX::reset() @@ -96,21 +84,8 @@ void CYSFRX::reset() void CYSFRX::samples(q15_t* samples, uint16_t* rssi, uint8_t length) { - q31_t dcLevel = 0; - q31_t dcVals[20U]; - q31_t q31Samples[20U]; - - ::arm_q15_to_q31(samples, q31Samples, length); - ::arm_biquad_cascade_df1_q31(&m_dcFilter, q31Samples, dcVals, length); - - for (uint8_t i = 0U; i < length; i++) - dcLevel += dcVals[i]; - dcLevel /= length; - - q15_t offset = q15_t(__SSAT((dcLevel >> 16), 16));; - for (uint8_t i = 0U; i < length; i++) { - q15_t sample = samples[i] - offset; + q15_t sample = samples[i]; m_rssiAccum += rssi[i]; m_rssiCount++; diff --git a/YSFRX.h b/YSFRX.h index f00d6c4..a4aea4a 100644 --- a/YSFRX.h +++ b/YSFRX.h @@ -56,8 +56,6 @@ private: uint8_t m_averagePtr; uint32_t m_rssiAccum; uint16_t m_rssiCount; - arm_biquad_casd_df1_inst_q31 m_dcFilter; - q31_t m_dcState[4]; void processNone(q15_t sample); void processData(q15_t sample);