diff --git a/DStarRX.cpp b/DStarRX.cpp index 103176b..c55edd5 100644 --- a/DStarRX.cpp +++ b/DStarRX.cpp @@ -241,6 +241,10 @@ 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,8 +261,16 @@ m_pathMemory2(), m_pathMemory3(), m_fecOutput(), m_rssiAccum(0U), -m_rssiCount(0U) +m_rssiCount(0U), +m_dcFilter(), +m_dcState() { + ::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() @@ -273,13 +285,24 @@ void CDStarRX::reset() m_rssiCount = 0U; } -void CDStarRX::samples(const q15_t* samples, const uint16_t* rssi, uint8_t length) -{ +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 = samples[i] < 0; + bool bit = (q31Samples[i] - dcLevel) < 0; if (bit != m_prev) { if (m_pll < (PLLMAX / 2U)) diff --git a/DStarRX.h b/DStarRX.h index d636cde..49dab0b 100644 --- a/DStarRX.h +++ b/DStarRX.h @@ -32,7 +32,7 @@ class CDStarRX { public: CDStarRX(); - void samples(const q15_t* samples, const uint16_t* rssi, uint8_t length); + void samples(q15_t* samples, const uint16_t* rssi, uint8_t length); void reset(); @@ -53,6 +53,8 @@ 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 8f290e1..0188d62 100644 --- a/IO.cpp +++ b/IO.cpp @@ -36,10 +36,6 @@ const uint16_t GAUSSIAN_0_5_FILTER_LEN = 12U; static q15_t BOXCAR_FILTER[] = {12000, 12000, 12000, 12000, 12000, 0}; const uint16_t BOXCAR_FILTER_LEN = 6U; -// 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 - const uint16_t DC_OFFSET = 2048U; CIO::CIO() : @@ -66,14 +62,11 @@ m_detect(false), m_adcOverflow(0U), m_dacOverflow(0U), m_watchdog(0U), -m_lockout(false), -m_dcFilter(), -m_dcState() +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_rrcFilter.numTaps = RRC_0_2_FILTER_LEN; m_rrcFilter.pState = m_rrcState; @@ -87,11 +80,6 @@ m_dcState() m_boxcarFilter.pState = m_boxcarState; m_boxcarFilter.pCoeffs = BOXCAR_FILTER; - m_dcFilter.numStages = DC_FILTER_STAGES; - m_dcFilter.pState = m_dcState; - m_dcFilter.pCoeffs = DC_FILTER; - m_dcFilter.postShift = 0; - initInt(); } @@ -168,22 +156,6 @@ void CIO::process() if (m_lockout) return; - - q31_t dcLevel = 0; - q31_t dcVals[20]; - q31_t q31Samples[20U]; - - ::arm_q15_to_q31(samples, q31Samples, RX_BLOCK_SIZE); - ::arm_biquad_cascade_df1_q31(&m_dcFilter, q31Samples, dcVals, RX_BLOCK_SIZE); - - for (uint8_t i = 0U; i < RX_BLOCK_SIZE; i++) - dcLevel += dcVals[i]; - dcLevel /= RX_BLOCK_SIZE; - - q15_t offset = q15_t(dcLevel >> 16); - - for (uint8_t i = 0U; i < RX_BLOCK_SIZE; i++) - samples[i] -= offset; if (m_modemState == STATE_IDLE) { if (m_dstarEnable) { diff --git a/IO.h b/IO.h index 07e6d2d..80a6d28 100644 --- a/IO.h +++ b/IO.h @@ -60,14 +60,12 @@ private: CSampleRB m_txBuffer; CRSSIRB m_rssiBuffer; - arm_fir_instance_q15 m_rrcFilter; - arm_fir_instance_q15 m_gaussianFilter; - arm_fir_instance_q15 m_boxcarFilter; - q15_t m_rrcState[70U]; // NoTaps + BlockSize - 1, 42 + 20 - 1 plus some spare - q15_t m_gaussianState[40U]; // NoTaps + BlockSize - 1, 12 + 20 - 1 plus some spare - q15_t m_boxcarState[30U]; // NoTaps + BlockSize - 1, 6 + 20 - 1 plus some spare - 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; + q15_t m_rrcState[70U]; // NoTaps + BlockSize - 1, 42 + 20 - 1 plus some spare + q15_t m_gaussianState[40U]; // NoTaps + BlockSize - 1, 12 + 20 - 1 plus some spare + q15_t m_boxcarState[30U]; // NoTaps + BlockSize - 1, 6 + 20 - 1 plus some spare bool m_pttInvert; q15_t m_rxLevel; diff --git a/P25RX.cpp b/P25RX.cpp index 9228c5b..6c1d0af 100644 --- a/P25RX.cpp +++ b/P25RX.cpp @@ -38,6 +38,10 @@ 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(), @@ -60,8 +64,16 @@ m_threshold(), m_thresholdVal(0), m_averagePtr(NOAVEPTR), m_rssiAccum(0U), -m_rssiCount(0U) +m_rssiCount(0U), +m_dcFilter(), +m_dcState() { + ::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() @@ -86,10 +98,23 @@ void CP25RX::reset() m_rssiCount = 0U; } -void CP25RX::samples(const q15_t* samples, uint16_t* rssi, uint8_t length) +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]; + q15_t sample = samples[i] - offset; m_rssiAccum += rssi[i]; m_rssiCount++; diff --git a/P25RX.h b/P25RX.h index 0bdf3c5..16f6cf3 100644 --- a/P25RX.h +++ b/P25RX.h @@ -32,7 +32,7 @@ class CP25RX { public: CP25RX(); - void samples(const q15_t* samples, uint16_t* rssi, uint8_t length); + void samples(q15_t* samples, uint16_t* rssi, uint8_t length); void reset(); @@ -59,6 +59,8 @@ 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 88c6f83..f3be78c 100644 --- a/YSFRX.cpp +++ b/YSFRX.cpp @@ -38,6 +38,10 @@ 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(), @@ -58,8 +62,16 @@ m_threshold(), m_thresholdVal(0), m_averagePtr(NOAVEPTR), m_rssiAccum(0U), -m_rssiCount(0U) +m_rssiCount(0U), +m_dcFilter(), +m_dcState() { + ::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() @@ -82,10 +94,23 @@ void CYSFRX::reset() m_rssiCount = 0U; } -void CYSFRX::samples(const q15_t* samples, uint16_t* rssi, uint8_t length) +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]; + q15_t sample = samples[i] - offset; m_rssiAccum += rssi[i]; m_rssiCount++; diff --git a/YSFRX.h b/YSFRX.h index 556280a..f00d6c4 100644 --- a/YSFRX.h +++ b/YSFRX.h @@ -31,7 +31,7 @@ class CYSFRX { public: CYSFRX(); - void samples(const q15_t* samples, uint16_t* rssi, uint8_t length); + void samples(q15_t* samples, uint16_t* rssi, uint8_t length); void reset(); @@ -56,6 +56,8 @@ 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);