From 1bb474dfeff61499fac0acc6b78e2edab1ec76d9 Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Mon, 6 Feb 2017 21:02:45 +0000 Subject: [PATCH] Add new interpolation for most modes and start positioning for P25 and YSF. --- DMRDMORX.cpp | 72 ++++++++++++++++----------------- DMRDefines.h | 24 +++++------ DMRIdleRX.cpp | 28 +++++++++---- DMRSlotRX.cpp | 35 +++++++++++----- P25Defines.h | 2 + P25RX.cpp | 108 ++++++++++++++++++++++++++++++-------------------- P25RX.h | 4 +- YSFDefines.h | 3 ++ YSFRX.cpp | 71 +++++++++++++++++++++------------ YSFRX.h | 1 + 10 files changed, 212 insertions(+), 136 deletions(-) diff --git a/DMRDMORX.cpp b/DMRDMORX.cpp index bf62eee..9541249 100644 --- a/DMRDMORX.cpp +++ b/DMRDMORX.cpp @@ -240,16 +240,13 @@ bool CDMRDMORX::processSample(q15_t sample, uint16_t rssi) void CDMRDMORX::correlateSync(bool first) { - uint8_t errs1 = countBits32((m_bitBuffer[m_bitPtr] & DMR_SYNC_SYMBOLS_MASK) ^ DMR_S2_DATA_SYNC_SYMBOLS); - uint8_t errs2 = countBits32((m_bitBuffer[m_bitPtr] & DMR_SYNC_SYMBOLS_MASK) ^ DMR_MS_DATA_SYNC_SYMBOLS); + uint8_t errs = countBits32((m_bitBuffer[m_bitPtr] & DMR_SYNC_SYMBOLS_MASK) ^ DMR_MS_DATA_SYNC_SYMBOLS); // The voice sync is the complement of the data sync - bool data1 = (errs1 <= MAX_SYNC_SYMBOLS_ERRS); - bool data2 = (errs2 <= MAX_SYNC_SYMBOLS_ERRS); - bool voice1 = (errs1 >= (DMR_SYNC_LENGTH_SYMBOLS - MAX_SYNC_SYMBOLS_ERRS)); - bool voice2 = (errs2 >= (DMR_SYNC_LENGTH_SYMBOLS - MAX_SYNC_SYMBOLS_ERRS)); + bool data = (errs <= MAX_SYNC_SYMBOLS_ERRS); + bool voice = (errs >= (DMR_SYNC_LENGTH_SYMBOLS - MAX_SYNC_SYMBOLS_ERRS)); - if (data1 || data2 || voice1 || voice2) { + if (data || voice) { uint16_t ptr = m_dataPtr + DMO_BUFFER_LENGTH_SAMPLES - DMR_SYNC_LENGTH_SAMPLES + DMR_RADIO_SYMBOL_LENGTH; if (ptr >= DMO_BUFFER_LENGTH_SAMPLES) ptr -= DMO_BUFFER_LENGTH_SAMPLES; @@ -258,23 +255,34 @@ void CDMRDMORX::correlateSync(bool first) q15_t min = 16000; q15_t max = -16000; - uint32_t mask = 0x00800000U; - for (uint8_t i = 0U; i < DMR_SYNC_LENGTH_SYMBOLS; i++, mask >>= 1) { - bool b; - if (data1 || voice1) - b = (DMR_S2_DATA_SYNC_SYMBOLS & mask) == mask; + for (uint8_t i = 0U; i < DMR_SYNC_LENGTH_SYMBOLS; i++) { + q15_t val = m_buffer[ptr]; + + if (val > max) + max = val; + if (val < min) + min = val; + + int8_t corrVal; + if (data) + corrVal = DMR_MS_DATA_SYNC_SYMBOLS_VALUES[i]; else - b = (DMR_MS_DATA_SYNC_SYMBOLS & mask) == mask; + corrVal = DMR_MS_VOICE_SYNC_SYMBOLS_VALUES[i]; - if (m_buffer[ptr] > max) - max = m_buffer[ptr]; - if (m_buffer[ptr] < min) - min = m_buffer[ptr]; - - if (data1 || data2) - corr += b ? -m_buffer[ptr] : m_buffer[ptr]; - else // if (voice) - corr += b ? m_buffer[ptr] : -m_buffer[ptr]; + switch (corrVal) { + case +3: + corr -= (val + val + val); + break; + case +1: + corr -= val; + break; + case -1: + corr += val; + break; + default: // -3 + corr += (val + val + val); + break; + } ptr += DMR_RADIO_SYMBOL_LENGTH; if (ptr >= DMO_BUFFER_LENGTH_SAMPLES) @@ -294,15 +302,11 @@ void CDMRDMORX::correlateSync(bool first) samplesToBits(ptr, DMR_SYNC_LENGTH_SYMBOLS, sync, 4U, centre, threshold); - if (data1 || data2) { + if (data) { uint8_t errs = 0U; - for (uint8_t i = 0U; i < DMR_SYNC_BYTES_LENGTH; i++) { - if (data1) - errs += countBits8((sync[i] & DMR_SYNC_BYTES_MASK[i]) ^ DMR_S2_DATA_SYNC_BYTES[i]); - else - errs += countBits8((sync[i] & DMR_SYNC_BYTES_MASK[i]) ^ DMR_MS_DATA_SYNC_BYTES[i]); - } - + for (uint8_t i = 0U; i < DMR_SYNC_BYTES_LENGTH; i++) + errs += countBits8((sync[i] & DMR_SYNC_BYTES_MASK[i]) ^ DMR_MS_DATA_SYNC_BYTES[i]); + if (errs <= MAX_SYNC_BYTES_ERRS) { if (first) { m_threshold[0U] = m_threshold[1U] = m_threshold[2U] = m_threshold[3U] = threshold; @@ -331,12 +335,8 @@ void CDMRDMORX::correlateSync(bool first) } } else { // if (voice1 || voice2) uint8_t errs = 0U; - for (uint8_t i = 0U; i < DMR_SYNC_BYTES_LENGTH; i++) { - if (voice1) - errs += countBits8((sync[i] & DMR_SYNC_BYTES_MASK[i]) ^ DMR_S2_VOICE_SYNC_BYTES[i]); - else - errs += countBits8((sync[i] & DMR_SYNC_BYTES_MASK[i]) ^ DMR_MS_VOICE_SYNC_BYTES[i]); - } + for (uint8_t i = 0U; i < DMR_SYNC_BYTES_LENGTH; i++) + errs += countBits8((sync[i] & DMR_SYNC_BYTES_MASK[i]) ^ DMR_MS_VOICE_SYNC_BYTES[i]); if (errs <= MAX_SYNC_BYTES_ERRS) { if (first) { diff --git a/DMRDefines.h b/DMRDefines.h index 3b2ccba..5335a03 100644 --- a/DMRDefines.h +++ b/DMRDefines.h @@ -61,32 +61,32 @@ const uint8_t DMR_MS_DATA_SYNC_BYTES[] = {0x0DU, 0x5DU, 0x7FU, 0x77U, 0xFDU, 0 const uint8_t DMR_MS_VOICE_SYNC_BYTES[] = {0x07U, 0xF7U, 0xD5U, 0xDDU, 0x57U, 0xDFU, 0xD0U}; const uint8_t DMR_BS_DATA_SYNC_BYTES[] = {0x0DU, 0xFFU, 0x57U, 0xD7U, 0x5DU, 0xF5U, 0xD0U}; const uint8_t DMR_BS_VOICE_SYNC_BYTES[] = {0x07U, 0x55U, 0xFDU, 0x7DU, 0xF7U, 0x5FU, 0x70U}; -const uint8_t DMR_S1_DATA_SYNC_BYTES[] = {0x0FU, 0x7FU, 0xDDU, 0x5DU, 0xDFU, 0xD5U, 0x50U}; -const uint8_t DMR_S1_VOICE_SYNC_BYTES[] = {0x05U, 0xD5U, 0x77U, 0xF7U, 0x75U, 0x7FU, 0xF0U}; -const uint8_t DMR_S2_DATA_SYNC_BYTES[] = {0x0DU, 0x75U, 0x57U, 0xF5U, 0xFFU, 0x7FU, 0x50U}; -const uint8_t DMR_S2_VOICE_SYNC_BYTES[] = {0x07U, 0xDFU, 0xFDU, 0x5FU, 0x55U, 0xD5U, 0xF0U}; const uint8_t DMR_SYNC_BYTES_MASK[] = {0x0FU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xFFU, 0xF0U}; const uint64_t DMR_MS_DATA_SYNC_BITS = 0x0000D5D7F77FD757U; const uint64_t DMR_MS_VOICE_SYNC_BITS = 0x00007F7D5DD57DFDU; const uint64_t DMR_BS_DATA_SYNC_BITS = 0x0000DFF57D75DF5DU; const uint64_t DMR_BS_VOICE_SYNC_BITS = 0x0000755FD7DF75F7U; -const uint64_t DMR_S1_DATA_SYNC_BITS = 0x0000F7FDD5DDFD55U; -const uint64_t DMR_S1_VOICE_SYNC_BITS = 0x00005D577F7757FFU; -const uint64_t DMR_S2_DATA_SYNC_BITS = 0x0000D7557F5FF7F5U; -const uint64_t DMR_S2_VOICE_SYNC_BITS = 0x00007DFFD5F55D5FU; const uint64_t DMR_SYNC_BITS_MASK = 0x0000FFFFFFFFFFFFU; const uint32_t DMR_MS_DATA_SYNC_SYMBOLS = 0x0076286EU; const uint32_t DMR_MS_VOICE_SYNC_SYMBOLS = 0x0089D791U; const uint32_t DMR_BS_DATA_SYNC_SYMBOLS = 0x00439B4DU; const uint32_t DMR_BS_VOICE_SYNC_SYMBOLS = 0x00BC64B2U; -const uint32_t DMR_S1_DATA_SYNC_SYMBOLS = 0x0021751FU; -const uint32_t DMR_S1_VOICE_SYNC_SYMBOLS = 0x00DE8AE0U; -const uint32_t DMR_S2_DATA_SYNC_SYMBOLS = 0x006F8C23U; -const uint32_t DMR_S2_VOICE_SYNC_SYMBOLS = 0x009073DCU; const uint32_t DMR_SYNC_SYMBOLS_MASK = 0x00FFFFFFU; +// D 5 D 7 F 7 7 F D 7 5 7 +// 11 01 01 01 11 01 01 11 11 11 01 11 01 11 11 11 11 01 01 11 01 01 01 11 +// -3 +3 +3 +3 -3 +3 +3 -3 -3 -3 +3 -3 +3 -3 -3 -3 -3 +3 +3 -3 +3 +3 +3 -3 + +const int8_t DMR_MS_DATA_SYNC_SYMBOLS_VALUES[] = {-3, +3, +3, +3, -3, +3, +3, -3, -3, -3, +3, -3, +3, -3, -3, -3, -3, +3, +3, -3, +3, +3, +3, -3}; + +// 7 F 7 D 5 D D 5 7 D F D +// 01 11 11 11 01 11 11 01 01 01 11 01 11 01 01 01 01 11 11 01 11 11 11 01 +// +3 -3 -3 -3 +3 -3 -3 +3 +3 +3 -3 +3 -3 +3 +3 +3 +3 -3 -3 +3 -3 -3 -3 +3 + +const int8_t DMR_MS_VOICE_SYNC_SYMBOLS_VALUES[] = {+3, -3, -3, -3, +3, -3, -3, +3, +3, +3, -3, +3, -3, +3, +3, +3, +3, -3, -3, +3, -3, -3, -3, +3}; + const uint8_t DT_VOICE_PI_HEADER = 0U; const uint8_t DT_VOICE_LC_HEADER = 1U; const uint8_t DT_TERMINATOR_WITH_LC = 2U; diff --git a/DMRIdleRX.cpp b/DMRIdleRX.cpp index b000f9c..c3ac7b3 100644 --- a/DMRIdleRX.cpp +++ b/DMRIdleRX.cpp @@ -84,16 +84,28 @@ void CDMRIdleRX::processSample(q15_t sample) q15_t max = -16000; q15_t min = 16000; - uint32_t mask = 0x00800000U; - for (uint8_t i = 0U; i < DMR_SYNC_LENGTH_SYMBOLS; i++, mask >>= 1) { - bool b = (DMR_MS_DATA_SYNC_SYMBOLS & mask) == mask; + for (uint8_t i = 0U; i < DMR_SYNC_LENGTH_SYMBOLS; i++) { + q15_t val = m_buffer[ptr]; - if (m_buffer[ptr] > max) - max = m_buffer[ptr]; - if (m_buffer[ptr] < min) - min = m_buffer[ptr]; + if (val > max) + max = val; + if (val < min) + min = val; - corr += b ? -m_buffer[ptr] : m_buffer[ptr]; + switch (DMR_MS_DATA_SYNC_SYMBOLS_VALUES[i]) { + case +3: + corr -= (val + val + val); + break; + case +1: + corr -= val; + break; + case -1: + corr += val; + break; + default: // -3 + corr += (val + val + val); + break; + } ptr += DMR_RADIO_SYMBOL_LENGTH; if (ptr >= DMR_FRAME_LENGTH_SAMPLES) diff --git a/DMRSlotRX.cpp b/DMRSlotRX.cpp index 6947020..4986025 100644 --- a/DMRSlotRX.cpp +++ b/DMRSlotRX.cpp @@ -253,19 +253,34 @@ void CDMRSlotRX::correlateSync(bool first) q15_t min = 16000; q15_t max = -16000; - uint32_t mask = 0x00800000U; - for (uint8_t i = 0U; i < DMR_SYNC_LENGTH_SYMBOLS; i++, mask >>= 1) { - bool b = (DMR_MS_DATA_SYNC_SYMBOLS & mask) == mask; + for (uint8_t i = 0U; i < DMR_SYNC_LENGTH_SYMBOLS; i++) { + q15_t val = m_buffer[ptr]; - if (m_buffer[ptr] > max) - max = m_buffer[ptr]; - if (m_buffer[ptr] < min) - min = m_buffer[ptr]; + if (val > max) + max = val; + if (val < min) + min = val; + int8_t corrVal; if (data) - corr += b ? -m_buffer[ptr] : m_buffer[ptr]; - else // if (voice) - corr += b ? m_buffer[ptr] : -m_buffer[ptr]; + corrVal = DMR_MS_DATA_SYNC_SYMBOLS_VALUES[i]; + else + corrVal = DMR_MS_VOICE_SYNC_SYMBOLS_VALUES[i]; + + switch (corrVal) { + case +3: + corr -= (val + val + val); + break; + case +1: + corr -= val; + break; + case -1: + corr += val; + break; + default: // -3 + corr += (val + val + val); + break; + } ptr += DMR_RADIO_SYMBOL_LENGTH; } diff --git a/P25Defines.h b/P25Defines.h index 21c2ff0..9aede9e 100644 --- a/P25Defines.h +++ b/P25Defines.h @@ -60,6 +60,8 @@ const uint64_t P25_SYNC_BITS_MASK = 0x0000FFFFFFFFFFFFU; // 01 01 01 01 01 11 01 01 11 11 01 01 11 11 11 11 01 11 01 11 11 11 11 11 // +3 +3 +3 +3 +3 -3 +3 +3 -3 -3 +3 +3 -3 -3 -3 -3 +3 -3 +3 -3 -3 -3 -3 -3 +const int8_t P25_SYNC_SYMBOLS_VALUES[] = {+3, +3, +3, +3, +3, -3, +3, +3, -3, -3, +3, +3, -3, -3, -3, -3, +3, -3, +3, -3, -3, -3, -3, -3}; + const uint32_t P25_SYNC_SYMBOLS = 0x00FB30A0U; const uint32_t P25_SYNC_SYMBOLS_MASK = 0x00FFFFFFU; diff --git a/P25RX.cpp b/P25RX.cpp index 76e8ad8..49509f5 100644 --- a/P25RX.cpp +++ b/P25RX.cpp @@ -18,7 +18,7 @@ #define WANT_DEBUG -// #define DUMP_SAMPLES +#define DUMP_SAMPLES #include "Config.h" #include "Globals.h" @@ -27,10 +27,12 @@ const q15_t SCALING_FACTOR = 18750; // Q15(0.55) -const uint8_t MAX_SYNC_SYMBOLS_ERRS = 2U; -const uint8_t MAX_SYNC_BYTES_ERRS = 3U; +const uint8_t MAX_SYNC_BIT_START_ERRS = 2U; +const uint8_t MAX_SYNC_BIT_RUN_ERRS = 4U; -const uint8_t BIT_MASK_TABLE[] = {0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U}; +const uint8_t MAX_SYNC_SYMBOLS_ERRS = 2U; + +const uint8_t BIT_MASK_TABLE[] = { 0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U }; #define WRITE_BIT1(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7]) @@ -44,7 +46,9 @@ m_bitBuffer(), m_buffer(), m_bitPtr(0U), m_dataPtr(0U), -m_endPtr(NOENDPTR), +m_hdrStartPtr(NOENDPTR), +m_lduStartPtr(NOENDPTR), +m_lduEndPtr(NOENDPTR), m_minSyncPtr(NOENDPTR), m_maxSyncPtr(NOENDPTR), m_hdrSyncPtr(NOENDPTR), @@ -71,7 +75,9 @@ void CP25RX::reset() m_bitPtr = 0U; m_maxCorr = 0; m_averagePtr = 0U; - m_endPtr = NOENDPTR; + m_hdrStartPtr = NOENDPTR; + m_lduStartPtr = NOENDPTR; + m_lduEndPtr = NOENDPTR; m_hdrSyncPtr = NOENDPTR; m_lduSyncPtr = NOENDPTR; m_minSyncPtr = NOENDPTR; @@ -182,12 +188,8 @@ void CP25RX::processHdr(q15_t sample) if (m_lduSyncPtr >= m_minSyncPtr || m_lduSyncPtr <= m_maxSyncPtr) isSync = true; } - if (isSync) { - DEBUG4("P25RX, sync position in HDR, pos/min/max", m_lduSyncPtr, m_minSyncPtr, m_maxSyncPtr); - uint16_t ptr = m_hdrSyncPtr + P25_LDU_FRAME_LENGTH_SAMPLES - P25_HDR_FRAME_LENGTH_SAMPLES - P25_SYNC_LENGTH_SAMPLES - 1U; - if (ptr >= P25_LDU_FRAME_LENGTH_SAMPLES) - ptr -= P25_LDU_FRAME_LENGTH_SAMPLES; + if (isSync) { m_threshold[m_averagePtr] = m_thresholdBest; m_centre[m_averagePtr] = m_centreBest; @@ -209,9 +211,9 @@ void CP25RX::processHdr(q15_t sample) DEBUG4("P25RX: sync found in Hdr pos/centre/threshold", m_hdrSyncPtr, m_centreVal, m_thresholdVal); uint8_t frame[P25_HDR_FRAME_LENGTH_BYTES + 1U]; - samplesToBits(ptr, P25_HDR_FRAME_LENGTH_SYMBOLS, frame, 8U, m_centreVal, m_thresholdVal); + samplesToBits(m_hdrStartPtr, P25_HDR_FRAME_LENGTH_SYMBOLS, frame, 8U, m_centreVal, m_thresholdVal); #if defined(DUMP_SAMPLES) - writeSync(ptr); + writeSync(m_hdrStartPtr); #endif frame[0U] = 0x01U; @@ -241,11 +243,7 @@ void CP25RX::processLdu(q15_t sample) correlateSync(); } - if (m_dataPtr == m_endPtr) { - uint16_t ptr = m_endPtr + P25_RADIO_SYMBOL_LENGTH + 1U; - if (ptr >= P25_LDU_FRAME_LENGTH_SAMPLES) - ptr -= P25_LDU_FRAME_LENGTH_SAMPLES; - + if (m_dataPtr == m_lduEndPtr) { // Only update the centre and threshold if they are from a good sync if (m_lostCount == MAX_SYNC_FRAMES) { m_threshold[m_averagePtr] = m_thresholdBest; @@ -278,9 +276,9 @@ void CP25RX::processLdu(q15_t sample) } uint8_t frame[P25_LDU_FRAME_LENGTH_BYTES + 3U]; - samplesToBits(ptr, P25_LDU_FRAME_LENGTH_SYMBOLS, frame, 8U, m_centreVal, m_thresholdVal); + samplesToBits(m_lduStartPtr, P25_LDU_FRAME_LENGTH_SYMBOLS, frame, 8U, m_centreVal, m_thresholdVal); #if defined(DUMP_SAMPLES) - writeSync(ptr); + writeSync(m_lduStartPtr); #endif // We've not seen a data sync for too long, signal RXLOST and change to RX_NONE @@ -293,9 +291,9 @@ void CP25RX::processLdu(q15_t sample) serial.writeP25Lost(); - m_state = P25RXS_NONE; - m_endPtr = NOENDPTR; - m_maxCorr = 0; + m_state = P25RXS_NONE; + m_lduEndPtr = NOENDPTR; + m_maxCorr = 0; } else { frame[0U] = m_lostCount == (MAX_SYNC_FRAMES - 1U) ? 0x01U : 0x00U; @@ -317,16 +315,28 @@ bool CP25RX::correlateSync() q15_t max = -16000; q15_t min = 16000; - uint32_t mask = 0x00080000U; - for (uint8_t i = 0U; i < P25_SYNC_LENGTH_SYMBOLS; i++, mask >>= 1) { - bool b = (P25_SYNC_SYMBOLS & mask) == mask; + for (uint8_t i = 0U; i < P25_SYNC_LENGTH_SYMBOLS; i++) { + q15_t val = m_buffer[ptr]; - if (m_buffer[ptr] > max) - max = m_buffer[ptr]; - if (m_buffer[ptr] < min) - min = m_buffer[ptr]; + if (val > max) + max = val; + if (val < min) + min = val; - corr += b ? -m_buffer[ptr] : m_buffer[ptr]; + switch (P25_SYNC_SYMBOLS_VALUES[i]) { + case +3: + corr -= (val + val + val); + break; + case +1: + corr -= val; + break; + case -1: + corr += val; + break; + default: // -3 + corr += (val + val + val); + break; + } ptr += P25_RADIO_SYMBOL_LENGTH; if (ptr >= P25_LDU_FRAME_LENGTH_SAMPLES) @@ -341,35 +351,45 @@ bool CP25RX::correlateSync() uint8_t sync[P25_SYNC_BYTES_LENGTH]; - uint16_t ptr = m_dataPtr + P25_LDU_FRAME_LENGTH_SAMPLES - P25_SYNC_LENGTH_SAMPLES + P25_RADIO_SYMBOL_LENGTH; - if (ptr >= P25_LDU_FRAME_LENGTH_SAMPLES) - ptr -= P25_LDU_FRAME_LENGTH_SAMPLES; + uint16_t startPtr = m_dataPtr + P25_LDU_FRAME_LENGTH_SAMPLES - P25_SYNC_LENGTH_SAMPLES + P25_RADIO_SYMBOL_LENGTH; + if (startPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) + startPtr -= P25_LDU_FRAME_LENGTH_SAMPLES; - if (m_state == P25RXS_NONE) - samplesToBits(ptr, P25_SYNC_LENGTH_SYMBOLS, sync, 0U, centre, threshold); - else - samplesToBits(ptr, P25_SYNC_LENGTH_SYMBOLS, sync, 0U, m_centreVal, m_thresholdVal); + uint8_t maxErrs; + if (m_state == P25RXS_NONE) { + samplesToBits(startPtr, P25_SYNC_LENGTH_SYMBOLS, sync, 0U, centre, threshold); + maxErrs = MAX_SYNC_BIT_START_ERRS; + } else { + samplesToBits(startPtr, P25_SYNC_LENGTH_SYMBOLS, sync, 0U, m_centreVal, m_thresholdVal); + maxErrs = MAX_SYNC_BIT_RUN_ERRS; + } uint8_t errs = 0U; for (uint8_t i = 0U; i < P25_SYNC_BYTES_LENGTH; i++) errs += countBits8(sync[i] ^ P25_SYNC_BYTES[i]); - if (errs <= MAX_SYNC_BYTES_ERRS) { + if (errs <= maxErrs) { m_maxCorr = corr; m_thresholdBest = threshold; m_centreBest = centre; m_lostCount = MAX_SYNC_FRAMES; + m_lduSyncPtr = m_dataPtr; - // This is the position of the end of a normal LDU - m_endPtr = m_dataPtr + P25_LDU_FRAME_LENGTH_SAMPLES - P25_SYNC_LENGTH_SAMPLES - 1U; - if (m_endPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) - m_endPtr -= P25_LDU_FRAME_LENGTH_SAMPLES; + // These are the positions of the start and end of an LDU + m_lduStartPtr = startPtr; + + m_lduEndPtr = m_dataPtr + P25_LDU_FRAME_LENGTH_SAMPLES - P25_SYNC_LENGTH_SAMPLES - 1U; + if (m_lduEndPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) + m_lduEndPtr -= P25_LDU_FRAME_LENGTH_SAMPLES; - // These are the positions of the sync in the following LDU if the HDR is present if (m_state == P25RXS_NONE) { m_hdrSyncPtr = m_dataPtr; + // This is the position of the start of a HDR + m_hdrStartPtr = startPtr; + + // These are the range of positions for a sync for an LDU following a HDR m_minSyncPtr = m_dataPtr + P25_HDR_FRAME_LENGTH_SAMPLES - 1U; if (m_minSyncPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) m_minSyncPtr -= P25_LDU_FRAME_LENGTH_SAMPLES; diff --git a/P25RX.h b/P25RX.h index 408a782..9dd0c08 100644 --- a/P25RX.h +++ b/P25RX.h @@ -42,7 +42,9 @@ private: q15_t m_buffer[P25_LDU_FRAME_LENGTH_SAMPLES]; uint16_t m_bitPtr; uint16_t m_dataPtr; - uint16_t m_endPtr; + uint16_t m_hdrStartPtr; + uint16_t m_lduStartPtr; + uint16_t m_lduEndPtr; uint16_t m_minSyncPtr; uint16_t m_maxSyncPtr; uint16_t m_hdrSyncPtr; diff --git a/YSFDefines.h b/YSFDefines.h index c724451..c05f426 100644 --- a/YSFDefines.h +++ b/YSFDefines.h @@ -44,6 +44,9 @@ const uint64_t YSF_SYNC_BITS_MASK = 0x000000FFFFFFFFFFU; // D 4 7 1 C 9 6 3 4 D // 11 01 01 00 01 11 00 01 11 00 10 01 01 10 00 11 01 00 11 01 // -3 +3 +3 +1 +3 -3 +1 +3 -3 +1 -1 +3 +3 -1 +3 -3 +3 +1 -3 +3 + +const int8_t YSF_SYNC_SYMBOLS_VALUES[] = {-3, +3, +3, +1, +3, -3, +1, +3, -3, +1, -1, +3, +3, -1, +3, -3, +3, +1, -3, +3}; + const uint32_t YSF_SYNC_SYMBOLS = 0x0007B5ADU; const uint32_t YSF_SYNC_SYMBOLS_MASK = 0x000FFFFFU; diff --git a/YSFRX.cpp b/YSFRX.cpp index 6af22cc..e81cfc5 100644 --- a/YSFRX.cpp +++ b/YSFRX.cpp @@ -18,7 +18,7 @@ #define WANT_DEBUG -// #define DUMP_SAMPLES +#define DUMP_SAMPLES #include "Config.h" #include "Globals.h" @@ -27,8 +27,10 @@ const q15_t SCALING_FACTOR = 18750; // Q15(0.55) -const uint8_t MAX_SYNC_SYMBOLS_ERRS = 2U; -const uint8_t MAX_SYNC_BYTES_ERRS = 3U; +const uint8_t MAX_SYNC_BIT_START_ERRS = 2U; +const uint8_t MAX_SYNC_BIT_RUN_ERRS = 4U; + +const uint8_t MAX_SYNC_SYMBOLS_ERRS = 3U; const uint8_t BIT_MASK_TABLE[] = {0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U, 0x02U, 0x01U}; @@ -44,6 +46,7 @@ m_bitBuffer(), m_buffer(), m_bitPtr(0U), m_dataPtr(0U), +m_startPtr(NOENDPTR), m_endPtr(NOENDPTR), m_syncPtr(NOENDPTR), m_minSyncPtr(NOENDPTR), @@ -70,6 +73,7 @@ void CYSFRX::reset() m_bitPtr = 0U; m_maxCorr = 0; m_averagePtr = 0U; + m_startPtr = NOENDPTR; m_endPtr = NOENDPTR; m_syncPtr = NOENDPTR; m_minSyncPtr = NOENDPTR; @@ -168,10 +172,6 @@ void CYSFRX::processData(q15_t sample) } if (m_dataPtr == m_endPtr) { - uint16_t ptr = m_endPtr + YSF_RADIO_SYMBOL_LENGTH + 1U; - if (ptr >= YSF_FRAME_LENGTH_SAMPLES) - ptr -= YSF_FRAME_LENGTH_SAMPLES; - // Only update the centre and threshold if they are from a good sync if (m_lostCount == MAX_SYNC_FRAMES) { m_threshold[m_averagePtr] = m_thresholdBest; @@ -204,9 +204,9 @@ void CYSFRX::processData(q15_t sample) } uint8_t frame[YSF_FRAME_LENGTH_BYTES + 3U]; - samplesToBits(ptr, YSF_FRAME_LENGTH_SYMBOLS, frame, 8U, m_centreVal, m_thresholdVal); + samplesToBits(m_startPtr, YSF_FRAME_LENGTH_SYMBOLS, frame, 8U, m_centreVal, m_thresholdVal); #if defined(DUMP_SAMPLES) - writeSync(ptr); + writeSync(m_startPtr); #endif // We've not seen a data sync for too long, signal RXLOST and change to RX_NONE @@ -244,16 +244,28 @@ bool CYSFRX::correlateSync() q15_t max = -16000; q15_t min = 16000; - uint32_t mask = 0x00080000U; - for (uint8_t i = 0U; i < YSF_SYNC_LENGTH_SYMBOLS; i++, mask >>= 1) { - bool b = (YSF_SYNC_SYMBOLS & mask) == mask; + for (uint8_t i = 0U; i < YSF_SYNC_LENGTH_SYMBOLS; i++) { + q15_t val = m_buffer[ptr]; - if (m_buffer[ptr] > max) - max = m_buffer[ptr]; - if (m_buffer[ptr] < min) - min = m_buffer[ptr]; + if (val > max) + max = val; + if (val < min) + min = val; - corr += b ? -m_buffer[ptr] : m_buffer[ptr]; + switch (YSF_SYNC_SYMBOLS_VALUES[i]) { + case +3: + corr -= (val + val + val); + break; + case +1: + corr -= val; + break; + case -1: + corr += val; + break; + default: // -3 + corr += (val + val + val); + break; + } ptr += YSF_RADIO_SYMBOL_LENGTH; if (ptr >= YSF_FRAME_LENGTH_SAMPLES) @@ -268,26 +280,32 @@ bool CYSFRX::correlateSync() uint8_t sync[YSF_SYNC_BYTES_LENGTH]; - uint16_t ptr = m_dataPtr + YSF_FRAME_LENGTH_SAMPLES - YSF_SYNC_LENGTH_SAMPLES + YSF_RADIO_SYMBOL_LENGTH; - if (ptr >= YSF_FRAME_LENGTH_SAMPLES) - ptr -= YSF_FRAME_LENGTH_SAMPLES; + uint16_t startPtr = m_dataPtr + YSF_FRAME_LENGTH_SAMPLES - YSF_SYNC_LENGTH_SAMPLES + YSF_RADIO_SYMBOL_LENGTH; + if (startPtr >= YSF_FRAME_LENGTH_SAMPLES) + startPtr -= YSF_FRAME_LENGTH_SAMPLES; - if (m_state == YSFRXS_NONE) - samplesToBits(ptr, YSF_SYNC_LENGTH_SYMBOLS, sync, 0U, centre, threshold); - else - samplesToBits(ptr, YSF_SYNC_LENGTH_SYMBOLS, sync, 0U, m_centreVal, m_thresholdVal); + uint8_t maxErrs; + if (m_state == YSFRXS_NONE) { + samplesToBits(startPtr, YSF_SYNC_LENGTH_SYMBOLS, sync, 0U, centre, threshold); + maxErrs = MAX_SYNC_BIT_START_ERRS; + } else { + samplesToBits(startPtr, YSF_SYNC_LENGTH_SYMBOLS, sync, 0U, m_centreVal, m_thresholdVal); + maxErrs = MAX_SYNC_BIT_RUN_ERRS; + } uint8_t errs = 0U; for (uint8_t i = 0U; i < YSF_SYNC_BYTES_LENGTH; i++) errs += countBits8(sync[i] ^ YSF_SYNC_BYTES[i]); - if (errs <= MAX_SYNC_BYTES_ERRS) { + if (errs <= maxErrs) { m_maxCorr = corr; m_thresholdBest = threshold; m_centreBest = centre; m_lostCount = MAX_SYNC_FRAMES; m_syncPtr = m_dataPtr; + m_startPtr = startPtr; + m_endPtr = m_dataPtr + YSF_FRAME_LENGTH_SAMPLES - YSF_SYNC_LENGTH_SAMPLES - 1U; if (m_endPtr >= YSF_FRAME_LENGTH_SAMPLES) m_endPtr -= YSF_FRAME_LENGTH_SAMPLES; @@ -360,6 +378,9 @@ void CYSFRX::writeSync(uint16_t start) q15_t sync[YSF_SYNC_LENGTH_SYMBOLS]; for (uint16_t i = 0U; i < YSF_SYNC_LENGTH_SYMBOLS; i++) { + if (i == (YSF_SYNC_LENGTH_SYMBOLS - 1U)) + DEBUG3("YSFRX: sync/start", m_syncPtr, start); + sync[i] = m_buffer[start]; start += YSF_RADIO_SYMBOL_LENGTH; diff --git a/YSFRX.h b/YSFRX.h index 2087e76..9e5d48d 100644 --- a/YSFRX.h +++ b/YSFRX.h @@ -41,6 +41,7 @@ private: q15_t m_buffer[YSF_FRAME_LENGTH_SAMPLES]; uint16_t m_bitPtr; uint16_t m_dataPtr; + uint16_t m_startPtr; uint16_t m_endPtr; uint16_t m_minSyncPtr; uint16_t m_maxSyncPtr;