diff --git a/DStarRX.cpp b/DStarRX.cpp index 4f65f6f..a6a4e34 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() @@ -281,24 +269,13 @@ void CDStarRX::reset() m_rssiCount = 0U; } -void CDStarRX::samples(q15_t* samples, const uint16_t* rssi, uint8_t length) +void CDStarRX::samples(const 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..d636cde 100644 --- a/DStarRX.h +++ b/DStarRX.h @@ -32,7 +32,7 @@ class CDStarRX { public: CDStarRX(); - void samples(q15_t* samples, const uint16_t* rssi, uint8_t length); + void samples(const q15_t* samples, const uint16_t* rssi, uint8_t length); void reset(); @@ -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..f711172 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 dcValues[RX_BLOCK_SIZE]; + q31_t q31Samples[RX_BLOCK_SIZE]; + + ::arm_q15_to_q31(samples, q31Samples, RX_BLOCK_SIZE); + ::arm_biquad_cascade_df1_q31(&m_dcFilter, q31Samples, 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 dcSamples[RX_BLOCK_SIZE]; + for (uint8_t i = 0U; i < RX_BLOCK_SIZE; 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..9228c5b 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() @@ -98,23 +86,10 @@ void CP25RX::reset() m_rssiCount = 0U; } -void CP25RX::samples(q15_t* samples, uint16_t* rssi, uint8_t length) +void CP25RX::samples(const 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..0bdf3c5 100644 --- a/P25RX.h +++ b/P25RX.h @@ -32,7 +32,7 @@ class CP25RX { public: CP25RX(); - void samples(q15_t* samples, uint16_t* rssi, uint8_t length); + void samples(const q15_t* samples, uint16_t* rssi, uint8_t length); void reset(); @@ -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..88c6f83 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() @@ -94,23 +82,10 @@ void CYSFRX::reset() m_rssiCount = 0U; } -void CYSFRX::samples(q15_t* samples, uint16_t* rssi, uint8_t length) +void CYSFRX::samples(const 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..556280a 100644 --- a/YSFRX.h +++ b/YSFRX.h @@ -31,7 +31,7 @@ class CYSFRX { public: CYSFRX(); - void samples(q15_t* samples, uint16_t* rssi, uint8_t length); + void samples(const q15_t* samples, uint16_t* rssi, uint8_t length); void reset(); @@ -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);