diff --git a/DMRDMORX.cpp b/DMRDMORX.cpp index 2c3bdef..69956b2 100644 --- a/DMRDMORX.cpp +++ b/DMRDMORX.cpp @@ -88,7 +88,7 @@ bool CDMRDMORX::processSample(q15_t sample, uint16_t rssi) m_rssi[m_dataPtr] = rssi; m_bitBuffer[m_bitPtr] <<= 1; - if (sample < 0) + if (sample < m_dc_level) m_bitBuffer[m_bitPtr] |= 0x01U; if (m_state == DMORXS_NONE) { diff --git a/DMRSlotRX.cpp b/DMRSlotRX.cpp index 0d34560..86c273c 100644 --- a/DMRSlotRX.cpp +++ b/DMRSlotRX.cpp @@ -104,7 +104,7 @@ bool CDMRSlotRX::processSample(q15_t sample, uint16_t rssi) m_rssi[m_dataPtr] = rssi; m_bitBuffer[m_bitPtr] <<= 1; - if (sample < 0) + if (sample < m_dc_level) m_bitBuffer[m_bitPtr] |= 0x01U; if (m_state == DMRRXS_NONE) { diff --git a/DStarRX.cpp b/DStarRX.cpp index 6c4f342..60ede7f 100644 --- a/DStarRX.cpp +++ b/DStarRX.cpp @@ -34,10 +34,6 @@ const unsigned int SYNC_POS = 21U * DSTAR_DATA_LENGTH_BITS; const unsigned int SYNC_SCAN_START = SYNC_POS - 3U; const unsigned int SYNC_SCAN_END = SYNC_POS + 3U; -// 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 - // D-Star bit order version of 0x55 0x55 0x6E 0x0A const uint32_t FRAME_SYNC_DATA = 0x00557650U; const uint32_t FRAME_SYNC_MASK = 0x00FFFFFFU; @@ -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() @@ -286,24 +274,12 @@ void CDStarRX::reset() } void CDStarRX::samples(const q15_t* samples, const uint16_t* rssi, uint8_t length) -{ - q31_t dc_level = 0; - q31_t dcVals[20]; - q31_t intSamp[20]; - - ::arm_q15_to_q31((q15_t*)samples, intSamp, length); - ::arm_biquad_cascade_df1_q31(&m_dcFilter, intSamp, dcVals, length); - - for (uint8_t i = 0U; i < length; i++) - dc_level += dcVals[i]; - - dc_level /= length; - +{ for (uint16_t i = 0U; i < length; i++) { m_rssiAccum += rssi[i]; m_rssiCount++; - bool bit = samples[i] < (q15_t) (dc_level >> 16); + bool bit = samples[i] < m_dc_level; if (bit != m_prev) { if (m_pll < (PLLMAX / 2U)) diff --git a/DStarRX.h b/DStarRX.h index 157fb41..d636cde 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/Globals.h b/Globals.h index 5edd5e3..27f62e4 100644 --- a/Globals.h +++ b/Globals.h @@ -93,6 +93,8 @@ extern bool m_duplex; extern bool m_tx; extern bool m_dcd; +extern q15_t m_dc_level; + extern CSerialPort serial; extern CIO io; diff --git a/IO.cpp b/IO.cpp index 35d4a7a..222b876 100644 --- a/IO.cpp +++ b/IO.cpp @@ -36,6 +36,10 @@ const uint16_t GAUSSIAN_0_5_FILTER_LEN = 12U; static q15_t BOXCAR_FILTER[] = {3000, 3000, 3000, 3000, 3000, 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() : @@ -62,11 +66,14 @@ m_detect(false), m_adcOverflow(0U), m_dacOverflow(0U), m_watchdog(0U), -m_lockout(false) +m_lockout(false), +m_dcFilter(), +m_dcState() { ::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; @@ -79,6 +86,11 @@ m_lockout(false) m_boxcarFilter.numTaps = BOXCAR_FILTER_LEN; 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(); } @@ -156,6 +168,20 @@ void CIO::process() if (m_lockout) return; + + q31_t dc_level = 0; + q31_t dcVals[20]; + q31_t intSamp[20]; + + ::arm_q15_to_q31((q15_t*)samples, intSamp, RX_BLOCK_SIZE); + ::arm_biquad_cascade_df1_q31(&m_dcFilter, intSamp, dcVals, RX_BLOCK_SIZE); + + for (uint8_t i = 0U; i < RX_BLOCK_SIZE; i++) + dc_level += dcVals[i]; + + dc_level /= RX_BLOCK_SIZE; + + m_dc_level = (q15_t) (dc_level >> 16); if (m_modemState == STATE_IDLE) { if (m_dstarEnable) { diff --git a/IO.h b/IO.h index 80a6d28..aa718a5 100644 --- a/IO.h +++ b/IO.h @@ -66,6 +66,8 @@ private: 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]; bool m_pttInvert; q15_t m_rxLevel; diff --git a/MMDVM.cpp b/MMDVM.cpp index 6e2be23..91cf1f3 100644 --- a/MMDVM.cpp +++ b/MMDVM.cpp @@ -36,6 +36,8 @@ bool m_duplex = true; bool m_tx = false; bool m_dcd = false; +q15_t m_dc_level = 0; + CDStarRX dstarRX; CDStarTX dstarTX; diff --git a/MMDVM.ino b/MMDVM.ino index 0aaedb4..7f83abd 100644 --- a/MMDVM.ino +++ b/MMDVM.ino @@ -33,6 +33,8 @@ bool m_duplex = true; bool m_tx = false; bool m_dcd = false; +q15_t m_dc_level = 0; + CDStarRX dstarRX; CDStarTX dstarTX; diff --git a/P25RX.cpp b/P25RX.cpp index 9228c5b..479b25c 100644 --- a/P25RX.cpp +++ b/P25RX.cpp @@ -95,7 +95,7 @@ void CP25RX::samples(const q15_t* samples, uint16_t* rssi, uint8_t length) m_rssiCount++; m_bitBuffer[m_bitPtr] <<= 1; - if (sample < 0) + if (sample < m_dc_level) m_bitBuffer[m_bitPtr] |= 0x01U; m_buffer[m_dataPtr] = sample; diff --git a/YSFRX.cpp b/YSFRX.cpp index 88c6f83..91af557 100644 --- a/YSFRX.cpp +++ b/YSFRX.cpp @@ -91,7 +91,7 @@ void CYSFRX::samples(const q15_t* samples, uint16_t* rssi, uint8_t length) m_rssiCount++; m_bitBuffer[m_bitPtr] <<= 1; - if (sample < 0) + if (sample < m_dc_level) m_bitBuffer[m_bitPtr] |= 0x01U; m_buffer[m_dataPtr] = sample;