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 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);