From 74682e7ac0900bb735749d23ffbe4e7a35c4756d Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Fri, 18 Aug 2017 10:27:23 +0100 Subject: [PATCH] Optimise the DC offset code. --- DStarRX.cpp | 27 ++------------------------- DStarRX.h | 2 -- IO.cpp | 40 +++++++++++++++++++++++++++++++++++----- IO.h | 3 +++ P25RX.cpp | 29 ++--------------------------- P25RX.h | 2 -- YSFRX.cpp | 29 ++--------------------------- YSFRX.h | 2 -- 8 files changed, 44 insertions(+), 90 deletions(-) diff --git a/DStarRX.cpp b/DStarRX.cpp index 4f65f6f..6321dff 100644 --- a/DStarRX.cpp +++ b/DStarRX.cpp @@ -237,10 +237,6 @@ const uint16_t CCITT_TABLE[] = { 0xf78fU, 0xe606U, 0xd49dU, 0xc514U, 0xb1abU, 0xa022U, 0x92b9U, 0x8330U, 0x7bc7U, 0x6a4eU, 0x58d5U, 0x495cU, 0x3de3U, 0x2c6aU, 0x1ef1U, 0x0f78U}; -// 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_pll(0U), m_prev(false), @@ -257,16 +253,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() @@ -283,22 +271,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; - for (uint16_t i = 0U; i < length; i++) { m_rssiAccum += rssi[i]; m_rssiCount++; - bool bit = (q31Samples[i] - dcLevel) < 0; + bool bit = samples[i] < 0; if (bit != m_prev) { if (m_pll < (PLLMAX / 2U)) diff --git a/DStarRX.h b/DStarRX.h index 49dab0b..48bb1a9 100644 --- a/DStarRX.h +++ b/DStarRX.h @@ -53,8 +53,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(bool bit); void processHeader(bool bit); diff --git a/IO.cpp b/IO.cpp index 0188d62..626f072 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 + // Generated using rcosdesign(0.2, 8, 5, 'sqrt') in MATLAB static q15_t RRC_0_2_FILTER[] = {401, 104, -340, -731, -847, -553, 112, 909, 1472, 1450, 683, -675, -2144, -3040, -2706, -770, 2667, 6995, 11237, 14331, 15464, 14331, 11237, 6995, 2667, -770, -2706, -3040, -2144, -675, 683, 1450, 1472, 909, 112, @@ -43,6 +47,8 @@ m_started(false), m_rxBuffer(RX_RINGBUFFER_SIZE), m_txBuffer(TX_RINGBUFFER_SIZE), m_rssiBuffer(RX_RINGBUFFER_SIZE), +m_dcFilter(), +m_dcState(), m_rrcFilter(), m_gaussianFilter(), m_boxcarFilter(), @@ -67,6 +73,12 @@ m_lockout(false) ::memset(m_rrcState, 0x00U, 70U * sizeof(q15_t)); ::memset(m_gaussianState, 0x00U, 40U * sizeof(q15_t)); ::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_rrcFilter.numTaps = RRC_0_2_FILTER_LEN; m_rrcFilter.pState = m_rrcState; @@ -157,21 +169,39 @@ void CIO::process() if (m_lockout) return; + 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));; + + q15_t dcSamples[RX_BLOCK_SIZE]; + for (uint8_t i = 0U; i < length; i++) + dcSamples[i] = samples[i] - offset; + if (m_modemState == STATE_IDLE) { if (m_dstarEnable) { q15_t GMSKVals[RX_BLOCK_SIZE]; - ::arm_fir_fast_q15(&m_gaussianFilter, samples, GMSKVals, RX_BLOCK_SIZE); + ::arm_fir_fast_q15(&m_gaussianFilter, dcSamples, GMSKVals, RX_BLOCK_SIZE); dstarRX.samples(GMSKVals, rssi, RX_BLOCK_SIZE); } if (m_p25Enable) { q15_t P25Vals[RX_BLOCK_SIZE]; - ::arm_fir_fast_q15(&m_boxcarFilter, samples, P25Vals, RX_BLOCK_SIZE); + ::arm_fir_fast_q15(&m_boxcarFilter, dcSamples, P25Vals, RX_BLOCK_SIZE); p25RX.samples(P25Vals, rssi, RX_BLOCK_SIZE); } + // XXX YSF should use dcSamples, but DMR not if (m_dmrEnable || m_ysfEnable) { q15_t C4FSKVals[RX_BLOCK_SIZE]; ::arm_fir_fast_q15(&m_rrcFilter, samples, C4FSKVals, RX_BLOCK_SIZE); @@ -189,7 +219,7 @@ void CIO::process() } else if (m_modemState == STATE_DSTAR) { if (m_dstarEnable) { q15_t GMSKVals[RX_BLOCK_SIZE]; - ::arm_fir_fast_q15(&m_gaussianFilter, samples, GMSKVals, RX_BLOCK_SIZE); + ::arm_fir_fast_q15(&m_gaussianFilter, dcSamples, GMSKVals, RX_BLOCK_SIZE); dstarRX.samples(GMSKVals, rssi, RX_BLOCK_SIZE); } @@ -211,14 +241,14 @@ void CIO::process() } else if (m_modemState == STATE_YSF) { if (m_ysfEnable) { q15_t C4FSKVals[RX_BLOCK_SIZE]; - ::arm_fir_fast_q15(&m_rrcFilter, samples, C4FSKVals, RX_BLOCK_SIZE); + ::arm_fir_fast_q15(&m_rrcFilter, dcSamples, C4FSKVals, RX_BLOCK_SIZE); ysfRX.samples(C4FSKVals, rssi, RX_BLOCK_SIZE); } } else if (m_modemState == STATE_P25) { if (m_p25Enable) { q15_t P25Vals[RX_BLOCK_SIZE]; - ::arm_fir_fast_q15(&m_boxcarFilter, samples, P25Vals, RX_BLOCK_SIZE); + ::arm_fir_fast_q15(&m_boxcarFilter, dcSamples, P25Vals, RX_BLOCK_SIZE); p25RX.samples(P25Vals, rssi, RX_BLOCK_SIZE); } diff --git a/IO.h b/IO.h index 80a6d28..5421881 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_rrcFilter; arm_fir_instance_q15 m_gaussianFilter; arm_fir_instance_q15 m_boxcarFilter; 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);