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 1a65a8e..9df584c 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), @@ -42,6 +48,7 @@ m_dstarTXLevel(128 * 128), m_dmrTXLevel(128 * 128), m_ysfTXLevel(128 * 128), m_p25TXLevel(128 * 128), +m_txDCOffset(DC_OFFSET), m_ledCount(0U), m_ledValue(true), m_detect(false), @@ -51,6 +58,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 +149,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 +181,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 +199,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) { @@ -217,7 +247,7 @@ void CIO::write(MMDVM_STATE mode, q15_t* samples, uint16_t length, const uint8_t for (uint16_t i = 0U; i < length; i++) { q31_t res1 = samples[i] * txLevel; q15_t res2 = q15_t(__SSAT((res1 >> 15), 16)); - uint16_t res3 = uint16_t(res2 + DC_OFFSET); + uint16_t res3 = uint16_t(res2 + m_txDCOffset); // Detect DAC overflow if (res3 > 4095U) @@ -258,7 +288,7 @@ void CIO::setMode() #endif } -void CIO::setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rxLevel, uint8_t cwIdTXLevel, uint8_t dstarTXLevel, uint8_t dmrTXLevel, uint8_t ysfTXLevel, uint8_t p25TXLevel) +void CIO::setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rxLevel, uint8_t cwIdTXLevel, uint8_t dstarTXLevel, uint8_t dmrTXLevel, uint8_t ysfTXLevel, uint8_t p25TXLevel, int16_t txDCOffset) { m_pttInvert = pttInvert; @@ -268,6 +298,8 @@ void CIO::setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rx m_dmrTXLevel = q15_t(dmrTXLevel * 128); m_ysfTXLevel = q15_t(ysfTXLevel * 128); m_p25TXLevel = q15_t(p25TXLevel * 128); + + m_txDCOffset = DC_OFFSET + txDCOffset; if (rxInvert) m_rxLevel = -m_rxLevel; @@ -304,6 +336,11 @@ void CIO::resetWatchdog() m_watchdog = 0U; } +uint32_t CIO::getWatchdog() +{ + return m_watchdog; +} + bool CIO::hasLockout() const { return m_lockout; diff --git a/IO.h b/IO.h index 8498b75..1dba4a4 100644 --- a/IO.h +++ b/IO.h @@ -42,7 +42,7 @@ public: void interrupt(); - void setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rxLevel, uint8_t cwIdTXLevel, uint8_t dstarTXLevel, uint8_t dmrTXLevel, uint8_t ysfTXLevel, uint8_t p25TXLevel); + void setParameters(bool rxInvert, bool txInvert, bool pttInvert, uint8_t rxLevel, uint8_t cwIdTXLevel, uint8_t dstarTXLevel, uint8_t dmrTXLevel, uint8_t ysfTXLevel, uint8_t p25TXLevel, int16_t txDCOffset); void getOverflow(bool& adcOverflow, bool& dacOverflow); @@ -52,6 +52,7 @@ public: bool hasLockout() const; void resetWatchdog(); + uint32_t getWatchdog(); private: bool m_started; @@ -60,6 +61,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 @@ -71,6 +75,8 @@ private: q15_t m_ysfTXLevel; q15_t m_p25TXLevel; + uint16_t m_txDCOffset; + uint32_t m_ledCount; bool m_ledValue; diff --git a/IOSTM.cpp b/IOSTM.cpp index 56826bb..f02c347 100644 --- a/IOSTM.cpp +++ b/IOSTM.cpp @@ -355,7 +355,7 @@ void CIO::initInt() { GPIO_InitTypeDef GPIO_InitStruct; GPIO_StructInit(&GPIO_InitStruct); - GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStruct.GPIO_Speed = GPIO_Fast_Speed; GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_DOWN; 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/SerialPort.cpp b/SerialPort.cpp index 3ee2f21..c012ab0 100644 --- a/SerialPort.cpp +++ b/SerialPort.cpp @@ -221,7 +221,7 @@ void CSerialPort::getVersion() uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length) { - if (length < 13U) + if (length < 14U) return 4U; bool rxInvert = (data[0U] & 0x01U) == 0x01U; @@ -268,6 +268,8 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length) uint8_t ysfTXLevel = data[11U]; uint8_t p25TXLevel = data[12U]; + int16_t txDCOffset = int16_t(data[13U]) - 128; + m_modemState = modemState; m_dstarEnable = dstarEnable; @@ -289,7 +291,7 @@ uint8_t CSerialPort::setConfig(const uint8_t* data, uint8_t length) ysfTX.setLoDev(ysfLoDev); - io.setParameters(rxInvert, txInvert, pttInvert, rxLevel, cwIdTXLevel, dstarTXLevel, dmrTXLevel, ysfTXLevel, p25TXLevel); + io.setParameters(rxInvert, txInvert, pttInvert, rxLevel, cwIdTXLevel, dstarTXLevel, dmrTXLevel, ysfTXLevel, p25TXLevel, txDCOffset); io.start(); @@ -420,6 +422,9 @@ void CSerialPort::process() m_buffer[0U] = c; m_ptr = 1U; m_len = 0U; + } else { + m_ptr = 0U; + m_len = 0U; } } else if (m_ptr == 1U) { // Handle the frame length @@ -645,9 +650,9 @@ void CSerialPort::process() #if defined(SERIAL_REPEATER) case MMDVM_SERIAL: { - for (uint8_t i = 3U; i < m_len; i++) - m_repeat.put(m_buffer[i]); - } + for (uint8_t i = 3U; i < m_len; i++) + m_repeat.put(m_buffer[i]); + } break; #endif @@ -663,23 +668,28 @@ void CSerialPort::process() } } + if (io.getWatchdog() >= 48000U) { + m_ptr = 0U; + m_len = 0U; + } + #if defined(SERIAL_REPEATER) - // Write any outgoing serial data - uint16_t space = m_repeat.getData(); - if (space > 0U) { - int avail = availableForWriteInt(3U); - if (avail < space) - space = avail; + // Write any outgoing serial data + uint16_t space = m_repeat.getData(); + if (space > 0U) { + int avail = availableForWriteInt(3U); + if (avail < space) + space = avail; - for (uint16_t i = 0U; i < space; i++) { - uint8_t c = m_repeat.get(); - writeInt(3U, &c, 1U); - } - } + for (uint16_t i = 0U; i < space; i++) { + uint8_t c = m_repeat.get(); + writeInt(3U, &c, 1U); + } + } - // Read any incoming serial data - while (availableInt(3U)) - readInt(3U); + // Read any incoming serial data + while (availableInt(3U)) + readInt(3U); #endif } diff --git a/SerialSTM.cpp b/SerialSTM.cpp index e2b128e..263c034 100644 --- a/SerialSTM.cpp +++ b/SerialSTM.cpp @@ -184,7 +184,7 @@ void InitUSART1(int speed) GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; // Tx | Rx - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Speed = GPIO_Fast_Speed; GPIO_Init(GPIOA, &GPIO_InitStructure); // Configure USART baud rate @@ -375,7 +375,7 @@ void InitUSART2(int speed) GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3; // Tx | Rx - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Speed = GPIO_Fast_Speed; GPIO_Init(GPIOA, &GPIO_InitStructure); // Configure USART baud rate @@ -566,7 +566,7 @@ void InitUSART3(int speed) GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11; // Tx | Rx - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Speed = GPIO_Fast_Speed; GPIO_Init(GPIOC, &GPIO_InitStructure); // Configure USART baud rate @@ -758,7 +758,7 @@ void InitUART5(int speed) GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12; // Tx - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Speed = GPIO_Fast_Speed; GPIO_Init(GPIOC, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2; // Rx 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);