From f3a3132a6c11ca829567624b9bf680f6aa3db671 Mon Sep 17 00:00:00 2001 From: Jonathan Naylor Date: Tue, 31 Jan 2017 20:36:31 +0000 Subject: [PATCH] Add sync dumping and improve YSF and P25. --- DMRDMORX.cpp | 29 +++++- DMRDMORX.h | 3 +- DMRSlotRX.cpp | 23 +++++ DMRSlotRX.h | 1 + P25RX.cpp | 235 +++++++++++++++++++++++++++++++++---------------- P25RX.h | 14 +-- SerialPort.cpp | 27 +++++- SerialPort.h | 4 +- YSFRX.cpp | 96 ++++++++++++-------- YSFRX.h | 4 +- 10 files changed, 311 insertions(+), 125 deletions(-) diff --git a/DMRDMORX.cpp b/DMRDMORX.cpp index 35028b8..4e8886a 100644 --- a/DMRDMORX.cpp +++ b/DMRDMORX.cpp @@ -18,6 +18,8 @@ #define WANT_DEBUG +#define DUMP_SAMPLES + #include "Config.h" #include "Globals.h" #include "DMRDMORX.h" @@ -127,7 +129,11 @@ bool CDMRDMORX::processSample(q15_t sample, uint16_t rssi) ptr -= DMO_BUFFER_LENGTH_SAMPLES; samplesToBits(ptr, DMR_FRAME_LENGTH_SYMBOLS, frame, 8U, centre, threshold); - +#if defined(DUMP_SAMPLES) + if (m_control == CONTROL_DATA || m_control == CONTROL_VOICE) + writeSync(ptr); +#endif + if (m_control == CONTROL_DATA) { // Data sync uint8_t colorCode; @@ -425,3 +431,24 @@ void CDMRDMORX::writeRSSIData(uint8_t* frame) serial.writeDMRData(true, frame, DMR_FRAME_LENGTH_BYTES + 1U); #endif } + +void CDMRDMORX::writeSync(uint16_t start) +{ + // XXX Check this + start += DMR_AUDIO_LENGTH_SYMBOLS / 2U; + if (start >= DMO_BUFFER_LENGTH_SAMPLES) + start -= DMO_BUFFER_LENGTH_SAMPLES; + + q15_t sync[DMR_SYNC_LENGTH_SYMBOLS]; + + for (uint16_t i = 0U; i < DMR_SYNC_LENGTH_SYMBOLS; i++) { + sync[i] = m_buffer[start]; + + start += DMR_RADIO_SYMBOL_LENGTH; + if (start >= DMO_BUFFER_LENGTH_SAMPLES) + start -= DMO_BUFFER_LENGTH_SAMPLES; + } + + serial.writeSamples(STATE_DMR, sync, DMR_SYNC_LENGTH_SYMBOLS); +} + diff --git a/DMRDMORX.h b/DMRDMORX.h index 21de2cc..f78de80 100644 --- a/DMRDMORX.h +++ b/DMRDMORX.h @@ -64,7 +64,8 @@ private: void correlateSync(bool first); void samplesToBits(uint16_t start, uint8_t count, uint8_t* buffer, uint16_t offset, q15_t centre, q15_t threshold); void writeRSSIData(uint8_t* frame); + void writeSync(uint16_t start); }; #endif - + diff --git a/DMRSlotRX.cpp b/DMRSlotRX.cpp index e8d7052..0bcafd2 100644 --- a/DMRSlotRX.cpp +++ b/DMRSlotRX.cpp @@ -18,6 +18,8 @@ #define WANT_DEBUG +#define DUMP_SAMPLES + #include "Config.h" #include "Globals.h" #include "DMRSlotRX.h" @@ -130,6 +132,10 @@ bool CDMRSlotRX::processSample(q15_t sample, uint16_t rssi) uint16_t ptr = m_endPtr - DMR_FRAME_LENGTH_SAMPLES + DMR_RADIO_SYMBOL_LENGTH + 1U; samplesToBits(ptr, DMR_FRAME_LENGTH_SYMBOLS, frame, 8U, centre, threshold); +#if defined(DUMP_SAMPLES) + if (m_control == CONTROL_DATA || m_control == CONTROL_VOICE) + writeSync(ptr); +#endif if (m_control == CONTROL_DATA) { // Data sync @@ -387,3 +393,20 @@ void CDMRSlotRX::writeRSSIData(uint8_t* frame) serial.writeDMRData(m_slot, frame, DMR_FRAME_LENGTH_BYTES + 1U); #endif } + +void CDMRSlotRX::writeSync(uint16_t start) +{ + // XXX Check this + start += DMR_AUDIO_LENGTH_SYMBOLS / 2U; + + q15_t sync[DMR_SYNC_LENGTH_SYMBOLS]; + + for (uint16_t i = 0U; i < DMR_SYNC_LENGTH_SYMBOLS; i++) { + sync[i] = m_buffer[start]; + + start += DMR_RADIO_SYMBOL_LENGTH; + } + + serial.writeSamples(STATE_DMR, sync, DMR_SYNC_LENGTH_SYMBOLS); +} + diff --git a/DMRSlotRX.h b/DMRSlotRX.h index aab79ff..0207082 100644 --- a/DMRSlotRX.h +++ b/DMRSlotRX.h @@ -67,6 +67,7 @@ private: void correlateSync(bool first); void samplesToBits(uint16_t start, uint8_t count, uint8_t* buffer, uint16_t offset, q15_t centre, q15_t threshold); void writeRSSIData(uint8_t* frame); + void writeSync(uint16_t start); }; #endif diff --git a/P25RX.cpp b/P25RX.cpp index 8022837..7183dbe 100644 --- a/P25RX.cpp +++ b/P25RX.cpp @@ -18,6 +18,8 @@ #define WANT_DEBUG +#define DUMP_SAMPLES + #include "Config.h" #include "Globals.h" #include "P25RX.h" @@ -42,14 +44,13 @@ m_bitBuffer(), m_buffer(), m_bitPtr(0U), m_dataPtr(0U), -m_endLduPtr(NOENDPTR), +m_endPtr(NOENDPTR), m_syncPtr(NOENDPTR), -m_minHdrPtr(NOENDPTR), -m_maxHdrPtr(NOENDPTR), m_minSyncPtr(NOENDPTR), m_maxSyncPtr(NOENDPTR), m_maxCorr(0), m_lostCount(0U), +m_countdown(0U), m_centre(), m_centreVal(0), m_centreBest(0), @@ -64,22 +65,21 @@ m_rssiCount(0U) void CP25RX::reset() { - m_state = P25RXS_NONE; - m_dataPtr = 0U; - m_bitPtr = 0U; - m_maxCorr = 0; - m_averagePtr = 0U; - m_endLduPtr = NOENDPTR; - m_syncPtr = NOENDPTR; - m_minHdrPtr = NOENDPTR; - m_maxHdrPtr = NOENDPTR; - m_minSyncPtr = NOENDPTR; - m_maxSyncPtr = NOENDPTR; - m_centreVal = 0; - m_thresholdVal = 0; - m_lostCount = 0U; - m_rssiAccum = 0U; - m_rssiCount = 0U; + m_state = P25RXS_NONE; + m_dataPtr = 0U; + m_bitPtr = 0U; + m_maxCorr = 0; + m_averagePtr = 0U; + m_endPtr = NOENDPTR; + m_syncPtr = NOENDPTR; + m_minSyncPtr = NOENDPTR; + m_maxSyncPtr = NOENDPTR; + m_centreVal = 0; + m_thresholdVal = 0; + m_lostCount = 0U; + m_countdown = 0U; + m_rssiAccum = 0U; + m_rssiCount = 0U; } void CP25RX::samples(const q15_t* samples, uint16_t* rssi, uint8_t length) @@ -96,10 +96,17 @@ void CP25RX::samples(const q15_t* samples, uint16_t* rssi, uint8_t length) m_buffer[m_dataPtr] = sample; - if (m_state == P25RXS_NONE) + switch (m_state) { + case P25RXS_HDR: + processHdr(sample); + break; + case P25RXS_LDU: + processLdu(sample); + break; + default: processNone(sample); - else - processData(sample); + break; + } m_dataPtr++; if (m_dataPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) @@ -113,32 +120,102 @@ void CP25RX::samples(const q15_t* samples, uint16_t* rssi, uint8_t length) void CP25RX::processNone(q15_t sample) { - bool ret = correlateSync(true); + bool ret = correlateSync(); if (ret) { - m_rssiAccum = 0U; - m_rssiCount = 0U; + // On the first sync, start the countdown to the state change + if (m_countdown == 0U) { + m_rssiAccum = 0U; + m_rssiCount = 0U; - io.setDecode(true); - io.setADCDetection(true); + io.setDecode(true); + io.setADCDetection(true); - // If sync is between the two Hdr ptrs then we have a Hdr - // Send data out and update the m_endLduPtr + m_countdown = 5U; + } } - if (m_dataPtr == m_endLduPtr) { - for (uint8_t i = 0U; i < 16U; i++) { - m_centre[i] = m_centreBest; - m_threshold[i] = m_thresholdBest; + if (m_countdown > 0U) + m_countdown--; + + if (m_countdown == 1U) { + for (uint8_t i = 0U; i < 16U; i++) { + m_centre[i] = m_centreBest; + m_threshold[i] = m_thresholdBest; + } + + m_centreVal = m_centreBest; + m_thresholdVal = m_thresholdBest; + m_averagePtr = 0U; + + // These are the sync positions for the following LDU after a HDR + m_minSyncPtr = m_syncPtr + P25_HDR_FRAME_LENGTH_SAMPLES - 1U; + if (m_minSyncPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) + m_minSyncPtr -= P25_LDU_FRAME_LENGTH_SAMPLES; + + m_maxSyncPtr = m_syncPtr + P25_HDR_FRAME_LENGTH_SAMPLES + 1U; + if (m_maxSyncPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) + m_maxSyncPtr -= P25_LDU_FRAME_LENGTH_SAMPLES; + + m_state = P25RXS_HDR; + m_countdown = 0U; + } +} + +void CP25RX::processHdr(q15_t sample) +{ + if (m_minSyncPtr < m_maxSyncPtr) { + if (m_dataPtr >= m_minSyncPtr && m_dataPtr <= m_maxSyncPtr) + correlateSync(); + } else { + if (m_dataPtr >= m_minSyncPtr || m_dataPtr <= m_maxSyncPtr) + correlateSync(); + } + + if (m_dataPtr == m_maxSyncPtr) { + bool isSync = false; + if (m_minSyncPtr < m_maxSyncPtr) { + if (m_syncPtr >= m_minSyncPtr && m_syncPtr <= m_maxSyncPtr) + isSync = true; + } else { + if (m_syncPtr >= m_minSyncPtr || m_syncPtr <= m_maxSyncPtr) + isSync = true; } - m_centreVal = m_centreBest; - m_thresholdVal = m_thresholdBest; - m_averagePtr = 0U; + DEBUG4("P25RX, sync position in HDR, pos/min/max", m_syncPtr, m_minSyncPtr, m_maxSyncPtr); + if (isSync) { + // XXX this is possibly wrong + uint16_t ptr = m_syncPtr + 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; - DEBUG4("P25RX: sync found in None pos/centre/threshold", m_syncPtr, m_centreVal, m_thresholdVal); + m_threshold[m_averagePtr] = m_thresholdBest; + m_centre[m_averagePtr] = m_centreBest; - uint16_t ptr = m_endLduPtr + P25_RADIO_SYMBOL_LENGTH + 1U; - if (ptr >= P25_LDU_FRAME_LENGTH_SAMPLES) - ptr -= P25_LDU_FRAME_LENGTH_SAMPLES; + m_averagePtr++; + if (m_averagePtr >= 16U) + m_averagePtr = 0U; + + // Find the average centre and threshold values + m_centreVal = 0; + m_thresholdVal = 0; + for (uint8_t i = 0U; i < 16U; i++) { + m_centreVal += m_centre[i]; + m_thresholdVal += m_threshold[i]; + } + m_centreVal >>= 4; + m_thresholdVal >>= 4; + + DEBUG4("P25RX: sync found in Hdr (best) pos/centre/threshold", m_syncPtr, m_centreBest, m_thresholdBest); + DEBUG4("P25RX: sync found in Hdr pos/centre/threshold", m_syncPtr, 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); +#if defined(DUMP_SAMPLES) + writeSync(ptr); +#endif + + frame[0U] = 0x01U; + serial.writeP25Hdr(frame, P25_HDR_FRAME_LENGTH_BYTES + 1U); + } m_minSyncPtr = m_syncPtr + P25_LDU_FRAME_LENGTH_SAMPLES - 1U; if (m_minSyncPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) @@ -148,32 +225,23 @@ void CP25RX::processNone(q15_t sample) if (m_maxSyncPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) m_maxSyncPtr -= P25_LDU_FRAME_LENGTH_SAMPLES; - uint8_t frame[P25_LDU_FRAME_LENGTH_BYTES + 3U]; - samplesToBits(ptr, P25_LDU_FRAME_LENGTH_SYMBOLS, frame, 8U, m_centreVal, m_thresholdVal); - - frame[0U] = 0x01U; - writeRSSILdu(frame); - - // Start the next frame - ::memset(frame, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 3U); - - m_state = P25RXS_DATA; + m_state = P25RXS_LDU; m_maxCorr = 0; } } -void CP25RX::processData(q15_t sample) +void CP25RX::processLdu(q15_t sample) { if (m_minSyncPtr < m_maxSyncPtr) { if (m_dataPtr >= m_minSyncPtr && m_dataPtr <= m_maxSyncPtr) - correlateSync(false); + correlateSync(); } else { if (m_dataPtr >= m_minSyncPtr || m_dataPtr <= m_maxSyncPtr) - correlateSync(false); + correlateSync(); } - if (m_dataPtr == m_endLduPtr) { - uint16_t ptr = m_endLduPtr + P25_RADIO_SYMBOL_LENGTH + 1U; + 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; @@ -196,8 +264,8 @@ void CP25RX::processData(q15_t sample) m_centreVal >>= 4; m_thresholdVal >>= 4; - DEBUG4("P25RX: sync found in Data (best) pos/centre/threshold", m_syncPtr, m_centreBest, m_thresholdBest); - DEBUG4("P25RX: sync found in Data (val) pos/centre/threshold", m_syncPtr, m_centreVal, m_thresholdVal); + DEBUG4("P25RX: sync found in Ldu (best) pos/centre/threshold", m_syncPtr, m_centreBest, m_thresholdBest); + DEBUG4("P25RX: sync found in Ldu pos/centre/threshold", m_syncPtr, m_centreVal, m_thresholdVal); m_minSyncPtr = m_syncPtr + P25_LDU_FRAME_LENGTH_SAMPLES - 1U; if (m_minSyncPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) @@ -210,6 +278,9 @@ void CP25RX::processData(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); +#if defined(DUMP_SAMPLES) + writeSync(ptr); +#endif // We've not seen a data sync for too long, signal RXLOST and change to RX_NONE m_lostCount--; @@ -221,22 +292,20 @@ void CP25RX::processData(q15_t sample) serial.writeP25Lost(); - m_state = P25RXS_NONE; - m_endLduPtr = NOENDPTR; + m_state = P25RXS_NONE; + m_endPtr = NOENDPTR; + m_maxCorr = 0; } else { frame[0U] = m_lostCount == (MAX_SYNC_FRAMES - 1U) ? 0x01U : 0x00U; writeRSSILdu(frame); - // Start the next frame - ::memset(frame, 0x00U, P25_LDU_FRAME_LENGTH_BYTES + 3U); + m_maxCorr = 0; } - - m_maxCorr = 0; } } -bool CP25RX::correlateSync(bool none) +bool CP25RX::correlateSync() { if (countBits32((m_bitBuffer[m_bitPtr] & P25_SYNC_SYMBOLS_MASK) ^ P25_SYNC_SYMBOLS) <= MAX_SYNC_SYMBOLS_ERRS) { uint16_t ptr = m_dataPtr + P25_LDU_FRAME_LENGTH_SAMPLES - P25_SYNC_LENGTH_SAMPLES + P25_RADIO_SYMBOL_LENGTH; @@ -275,7 +344,7 @@ bool CP25RX::correlateSync(bool none) if (ptr >= P25_LDU_FRAME_LENGTH_SAMPLES) ptr -= P25_LDU_FRAME_LENGTH_SAMPLES; - if (none) + 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); @@ -291,18 +360,20 @@ bool CP25RX::correlateSync(bool none) m_lostCount = MAX_SYNC_FRAMES; m_syncPtr = m_dataPtr; - m_endLduPtr = m_dataPtr + P25_LDU_FRAME_LENGTH_SAMPLES - P25_SYNC_LENGTH_SAMPLES - 1U; - if (m_endLduPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) - m_endLduPtr -= P25_LDU_FRAME_LENGTH_SAMPLES; + // 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; - if (none) { - m_minHdrPtr = m_dataPtr + P25_HDR_FRAME_LENGTH_SAMPLES - 1U; - if (m_minHdrPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) - m_minHdrPtr -= 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_minSyncPtr = m_dataPtr + P25_HDR_FRAME_LENGTH_SAMPLES - 1U; + if (m_minSyncPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) + m_minSyncPtr -= P25_LDU_FRAME_LENGTH_SAMPLES; - m_maxHdrPtr = m_dataPtr + P25_HDR_FRAME_LENGTH_SAMPLES + 1U; - if (m_maxHdrPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) - m_maxHdrPtr -= P25_LDU_FRAME_LENGTH_SAMPLES; + m_maxSyncPtr = m_dataPtr + P25_HDR_FRAME_LENGTH_SAMPLES + 1U; + if (m_maxSyncPtr >= P25_LDU_FRAME_LENGTH_SAMPLES) + m_maxSyncPtr -= P25_LDU_FRAME_LENGTH_SAMPLES; } return true; @@ -367,3 +438,17 @@ void CP25RX::writeRSSILdu(uint8_t* ldu) m_rssiCount = 0U; } +void CP25RX::writeSync(uint16_t start) +{ + q15_t sync[P25_SYNC_LENGTH_SYMBOLS]; + + for (uint16_t i = 0U; i < P25_SYNC_LENGTH_SYMBOLS; i++) { + sync[i] = m_buffer[start]; + + start += P25_RADIO_SYMBOL_LENGTH; + if (start >= P25_LDU_FRAME_LENGTH_SAMPLES) + start -= P25_LDU_FRAME_LENGTH_SAMPLES; + } + + serial.writeSamples(STATE_P25, sync, P25_SYNC_LENGTH_SYMBOLS); +} diff --git a/P25RX.h b/P25RX.h index 9a90f57..560a4fd 100644 --- a/P25RX.h +++ b/P25RX.h @@ -24,7 +24,8 @@ enum P25RX_STATE { P25RXS_NONE, - P25RXS_DATA + P25RXS_HDR, + P25RXS_LDU }; class CP25RX { @@ -41,14 +42,13 @@ private: q15_t m_buffer[P25_LDU_FRAME_LENGTH_SAMPLES]; uint16_t m_bitPtr; uint16_t m_dataPtr; - uint16_t m_endLduPtr; - uint16_t m_minHdrPtr; - uint16_t m_maxHdrPtr; + uint16_t m_endPtr; uint16_t m_minSyncPtr; uint16_t m_maxSyncPtr; uint16_t m_syncPtr; q31_t m_maxCorr; uint16_t m_lostCount; + uint8_t m_countdown; q15_t m_centre[16U]; q15_t m_centreVal; q15_t m_centreBest; @@ -60,10 +60,12 @@ private: uint16_t m_rssiCount; void processNone(q15_t sample); - void processData(q15_t sample); - bool correlateSync(bool none); + void processHdr(q15_t sample); + void processLdu(q15_t sample); + bool correlateSync(); void samplesToBits(uint16_t start, uint16_t count, uint8_t* buffer, uint16_t offset, q15_t centre, q15_t threshold); void writeRSSILdu(uint8_t* ldu); + void writeSync(uint16_t start); }; #endif diff --git a/SerialPort.cpp b/SerialPort.cpp index 7972475..141a4aa 100644 --- a/SerialPort.cpp +++ b/SerialPort.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2013,2015,2016 by Jonathan Naylor G4KLX + * Copyright (C) 2013,2015,2016,2017 by Jonathan Naylor G4KLX * Copyright (C) 2016 by Colin Durbridge G4EML * * This program is free software; you can redistribute it and/or modify @@ -62,6 +62,8 @@ const uint8_t MMDVM_NAK = 0x7FU; const uint8_t MMDVM_SERIAL = 0x80U; +const uint8_t MMDVM_SAMPLES = 0xF0U; + const uint8_t MMDVM_DEBUG1 = 0xF1U; const uint8_t MMDVM_DEBUG2 = 0xF2U; const uint8_t MMDVM_DEBUG3 = 0xF3U; @@ -923,6 +925,29 @@ void CSerialPort::writeRSSIData(const uint8_t* data, uint8_t length) writeInt(1U, reply, count); } +void CSerialPort::writeSamples(unsigned char mode, const q15_t* samples, unsigned char nSamples) +{ + uint8_t reply[130U]; + + reply[0U] = MMDVM_FRAME_START; + reply[1U] = 0U; + reply[2U] = MMDVM_SAMPLES; + + reply[3U] = mode; + + uint8_t count = 4U; + for (uint8_t i = 0U; i < nSamples; i++) { + uint16_t val = uint16_t(samples[i] + 2048); + + reply[count++] = (val >> 8) & 0xFF; + reply[count++] = (val >> 0) & 0xFF; + } + + reply[1U] = count; + + writeInt(1U, reply, count, true); +} + void CSerialPort::writeDebug(const char* text) { uint8_t reply[130U]; diff --git a/SerialPort.h b/SerialPort.h index 630f2e6..b870ac8 100644 --- a/SerialPort.h +++ b/SerialPort.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015,2016 by Jonathan Naylor G4KLX + * Copyright (C) 2015,2016,2017 by Jonathan Naylor G4KLX * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -49,6 +49,8 @@ public: void writeCalData(const uint8_t* data, uint8_t length); void writeRSSIData(const uint8_t* data, uint8_t length); + void writeSamples(unsigned char mode, const q15_t* samples, unsigned char nSamples); + void writeDebug(const char* text); void writeDebug(const char* text, int16_t n1); void writeDebug(const char* text, int16_t n1, int16_t n2); diff --git a/YSFRX.cpp b/YSFRX.cpp index 0857fb6..c91331e 100644 --- a/YSFRX.cpp +++ b/YSFRX.cpp @@ -18,6 +18,8 @@ #define WANT_DEBUG +#define DUMP_SAMPLES + #include "Config.h" #include "Globals.h" #include "YSFRX.h" @@ -48,6 +50,7 @@ m_minSyncPtr(NOENDPTR), m_maxSyncPtr(NOENDPTR), m_maxCorr(0), m_lostCount(0U), +m_countdown(0U), m_centre(), m_centreVal(0), m_centreBest(0), @@ -74,6 +77,7 @@ void CYSFRX::reset() m_centreVal = 0; m_thresholdVal = 0; m_lostCount = 0U; + m_countdown = 0U; m_rssiAccum = 0U; m_rssiCount = 0U; } @@ -92,10 +96,14 @@ void CYSFRX::samples(const q15_t* samples, uint16_t* rssi, uint8_t length) m_buffer[m_dataPtr] = sample; - if (m_state == YSFRXS_NONE) - processNone(sample); - else + switch (m_state) { + case YSFRXS_DATA: processData(sample); + break; + default: + processNone(sample); + break; + } m_dataPtr++; if (m_dataPtr >= YSF_FRAME_LENGTH_SAMPLES) @@ -109,30 +117,33 @@ void CYSFRX::samples(const q15_t* samples, uint16_t* rssi, uint8_t length) void CYSFRX::processNone(q15_t sample) { - bool ret = correlateSync(true); + bool ret = correlateSync(); if (ret) { - m_rssiAccum = 0U; - m_rssiCount = 0U; + // On the first sync, start the countdown to the state change + if (m_countdown == 0U) { + m_rssiAccum = 0U; + m_rssiCount = 0U; - io.setDecode(true); - io.setADCDetection(true); + io.setDecode(true); + io.setADCDetection(true); + + m_countdown = 5U; + } } - if (m_dataPtr == m_endPtr) { + if (m_countdown > 0U) + m_countdown--; + + if (m_countdown == 1U) { for (uint8_t i = 0U; i < 16U; i++) { m_centre[i] = m_centreBest; m_threshold[i] = m_thresholdBest; } + m_centreVal = m_centreBest; m_thresholdVal = m_thresholdBest; m_averagePtr = 0U; - DEBUG4("YSFRX: sync found in None pos/centre/threshold", m_syncPtr, m_centreVal, m_thresholdVal); - - uint16_t ptr = m_endPtr + YSF_RADIO_SYMBOL_LENGTH + 1U; - if (ptr >= YSF_FRAME_LENGTH_SAMPLES) - ptr -= YSF_FRAME_LENGTH_SAMPLES; - m_minSyncPtr = m_syncPtr + YSF_FRAME_LENGTH_SAMPLES - 1U; if (m_minSyncPtr >= YSF_FRAME_LENGTH_SAMPLES) m_minSyncPtr -= YSF_FRAME_LENGTH_SAMPLES; @@ -141,17 +152,8 @@ void CYSFRX::processNone(q15_t sample) if (m_maxSyncPtr >= YSF_FRAME_LENGTH_SAMPLES) m_maxSyncPtr -= YSF_FRAME_LENGTH_SAMPLES; - uint8_t frame[YSF_FRAME_LENGTH_BYTES + 3U]; - samplesToBits(ptr, YSF_FRAME_LENGTH_SYMBOLS, frame, 8U, m_centreVal, m_thresholdVal); - - frame[0U] = 0x01U; - writeRSSIData(frame); - - // Start the next frame - ::memset(frame, 0x00U, YSF_FRAME_LENGTH_BYTES + 3U); - - m_state = YSFRXS_DATA; - m_maxCorr = 0; + m_state = YSFRXS_DATA; + m_countdown = 0U; } } @@ -159,10 +161,10 @@ void CYSFRX::processData(q15_t sample) { if (m_minSyncPtr < m_maxSyncPtr) { if (m_dataPtr >= m_minSyncPtr && m_dataPtr <= m_maxSyncPtr) - correlateSync(false); + correlateSync(); } else { if (m_dataPtr >= m_minSyncPtr || m_dataPtr <= m_maxSyncPtr) - correlateSync(false); + correlateSync(); } if (m_dataPtr == m_endPtr) { @@ -189,8 +191,8 @@ void CYSFRX::processData(q15_t sample) m_centreVal >>= 4; m_thresholdVal >>= 4; - DEBUG4("YSFRX: sync found in Data (best) pos/centre/threshold", m_syncPtr, m_centreBest, m_thresholdBest); - DEBUG4("YSFRX: sync found in Data (val) pos/centre/threshold", m_syncPtr, m_centreVal, m_thresholdVal); + DEBUG4("YSFRX: sync found (best) pos/centre/threshold", m_syncPtr, m_centreBest, m_thresholdBest); + DEBUG4("YSFRX: sync found pos/centre/threshold", m_syncPtr, m_centreVal, m_thresholdVal); m_minSyncPtr = m_syncPtr + YSF_FRAME_LENGTH_SAMPLES - 1U; if (m_minSyncPtr >= YSF_FRAME_LENGTH_SAMPLES) @@ -203,6 +205,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); +#if defined(DUMP_SAMPLES) + writeSync(ptr); +#endif // We've not seen a data sync for too long, signal RXLOST and change to RX_NONE m_lostCount--; @@ -214,22 +219,21 @@ void CYSFRX::processData(q15_t sample) serial.writeYSFLost(); - m_state = YSFRXS_NONE; - m_endPtr = NOENDPTR; - } else { + m_state = YSFRXS_NONE; + m_endPtr = NOENDPTR; + m_countdown = 0U; + m_maxCorr = 0; + } else { frame[0U] = m_lostCount == (MAX_SYNC_FRAMES - 1U) ? 0x01U : 0x00U; writeRSSIData(frame); - // Start the next frame - ::memset(frame, 0x00U, YSF_FRAME_LENGTH_BYTES + 3U); + m_maxCorr = 0; } - - m_maxCorr = 0; } } -bool CYSFRX::correlateSync(bool none) +bool CYSFRX::correlateSync() { if (countBits32((m_bitBuffer[m_bitPtr] & YSF_SYNC_SYMBOLS_MASK) ^ YSF_SYNC_SYMBOLS) <= MAX_SYNC_SYMBOLS_ERRS) { uint16_t ptr = m_dataPtr + YSF_FRAME_LENGTH_SAMPLES - YSF_SYNC_LENGTH_SAMPLES + YSF_RADIO_SYMBOL_LENGTH; @@ -268,7 +272,7 @@ bool CYSFRX::correlateSync(bool none) if (ptr >= YSF_FRAME_LENGTH_SAMPLES) ptr -= YSF_FRAME_LENGTH_SAMPLES; - if (none) + 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); @@ -350,3 +354,17 @@ void CYSFRX::writeRSSIData(uint8_t* data) m_rssiCount = 0U; } +void CYSFRX::writeSync(uint16_t start) +{ + q15_t sync[YSF_SYNC_LENGTH_SYMBOLS]; + + for (uint16_t i = 0U; i < YSF_SYNC_LENGTH_SYMBOLS; i++) { + sync[i] = m_buffer[start]; + + start += YSF_RADIO_SYMBOL_LENGTH; + if (start >= YSF_FRAME_LENGTH_SAMPLES) + start -= YSF_FRAME_LENGTH_SAMPLES; + } + + serial.writeSamples(STATE_YSF, sync, YSF_SYNC_LENGTH_SYMBOLS); +} diff --git a/YSFRX.h b/YSFRX.h index ed7619d..2087e76 100644 --- a/YSFRX.h +++ b/YSFRX.h @@ -47,6 +47,7 @@ private: uint16_t m_syncPtr; q31_t m_maxCorr; uint16_t m_lostCount; + uint8_t m_countdown; q15_t m_centre[16U]; q15_t m_centreVal; q15_t m_centreBest; @@ -59,9 +60,10 @@ private: void processNone(q15_t sample); void processData(q15_t sample); - bool correlateSync(bool none); + bool correlateSync(); void samplesToBits(uint16_t start, uint16_t count, uint8_t* buffer, uint16_t offset, q15_t centre, q15_t threshold); void writeRSSIData(uint8_t* data); + void writeSync(uint16_t start); }; #endif